From 3b651b31483311950c7fa791808cf3fc726baa96 Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Fri, 4 May 2018 16:00:55 -0400 Subject: [PATCH 1/6] Should have a working finAngle() --- control/rocketClass.hpp | 17 ++--------------- control/rocketClassDef.cpp | 39 +++++++++++++++++--------------------- 2 files changed, 19 insertions(+), 37 deletions(-) diff --git a/control/rocketClass.hpp b/control/rocketClass.hpp index 0225009..092fe8d 100644 --- a/control/rocketClass.hpp +++ b/control/rocketClass.hpp @@ -38,16 +38,9 @@ class rocket { float getRollRate(); float getPitch(); float getA_pointing(); - float getDynamicPressure(); - float getDampingConstant() { return dampingConst; } - float getSpringConstant() { return springConst; } - float getRollResistance() { return rollResist; } - float getSystemStrength() { return systemStrength; } - - float goalTorque(); - float inherientTorque(); - float deltaTorque(){return goalTorque()-inherientTorque();}; + float getDampingConstant(); + float getSpringConstant(); int finAngle(); @@ -87,12 +80,6 @@ class rocket { bool rollMatrixUp2Date; bool speedUp2Date; - float rollResist; - float systemStrength; - - float springConst; - float dampingConst; - float omega; float calibrationPressure; float moi; diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index a441048..b96d62a 100644 --- a/control/rocketClassDef.cpp +++ b/control/rocketClassDef.cpp @@ -4,9 +4,13 @@ #define maxQ ((293.15*101300.0*mOverR)*122500.0/2) #define omega_0 4.0 +#define maxSpeed 350.0 +#define maxPress 101300.0 +#define minTemp 273.15 +#define minFullDeflect 5.0 -using imu::Vector; +using imu::Vector; rocket::rocket(){ // Orientation Data @@ -43,9 +47,9 @@ int rocket::updateSensorData(Adafruit_BNO055 &bno, Adafruit_BMP280 &baro){ lastUpdate=current; Q = bno.getQuat(); //Takes a vector and rotates it by the same amount the BNO has since startup - a =bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); // convert a into the orignal frame + a = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); // convert a into the orignal frame - T=baro.readTemperature(); + T=baro.readTemperature()+minTemp; P=baro.readPressure(); up=bno.getVector(Adafruit_BNO055::VECTOR_GRAVITY)*(-1); @@ -103,9 +107,6 @@ float rocket::getA_pointing(){ return a[0]; } -float rocket::getDynamicPressure(){ - return ((P/(T+273.15))*mOverR)*getSpeedSq()/2; -} int rocket::fillModel(int fpsize, int devName){/* int property = 0; @@ -189,26 +190,13 @@ int rocket::sendDataComms(int device){ msg = nullptr; } -float rocket::goalTorque(){ - //return -getSpringConstant()*(plan.getTargetAngle(lastUpdate/1000)-getRoll())-getDampingConstant()*getRollRate(); - float result=-getSpringConstant()*(0-getRoll())-getDampingConstant()*getRollRate(); - //Serial.println(F("hi")); - //Serial.println(result); - return result; -} - -float rocket::inherientTorque(){ - return -getRollRate()*getRollResistance()*getDynamicPressure()/getSpeed(); -} float deltaTheta(float,float); int rocket::finAngle(){ - //Serial.println(getDynamicPressure()); - //Serial.println(goalTorque()); - float k=(5/45)*maxQ/getDynamicPressure(); - float c=4.0*k/(omega_0*omega_0); - int raw = (180.0/PI)*(k*deltaTheta(getRoll(),plan.getTargetAngle())*(180.0/PI)+c*getRollRate()); + float k = getSpringConstant() + float c = getDampingConstant() + int raw = (180.0/PI)*(k*deltaTheta(getRoll(),plan.getTargetAngle()*(PI/180))+c*getRollRate())*(minFullDeflect/180.0); return constrain(-20,raw,20); } @@ -217,4 +205,11 @@ float deltaTheta(float a, float b){ if(res>PI) return res-2*PI; else if(res<-PI) return 2*PI+res; else return res; +} + +float rocket::getDampingConstant(){ + return 2*getDampingConstant()/omega_0; +} +float rocket::getSpringConstant(){ + return getSpeedSq()/(maxSpeed*maxSpeed)*(P/maxPress)*(minTemp/T); } \ No newline at end of file From 7dfc93dc438e14fdac8f4a5b1dbee25db0ccec68 Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Fri, 4 May 2018 17:24:35 -0400 Subject: [PATCH 2/6] Fixed the merge errors --- control/control.ino | 14 +++++--------- control/rocketClassDef.cpp | 19 ++++++++++--------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/control/control.ino b/control/control.ino index 2143cf9..0cdfff5 100644 --- a/control/control.ino +++ b/control/control.ino @@ -22,7 +22,6 @@ float goalTorque(rocket &); float deltaTorque(rocket&,float); void setup() { - delay(1000); // Necessary for syncronization of boards serialDump(); Wire.onRequest(requestHandler); Wire.onReceive(receiveHandler); @@ -77,11 +76,10 @@ void loop() { Serial.println(hprcRock.getPitch()*180.0/PI); Serial.print(F("Roll: ")); Serial.println(hprcRock.getRoll()*180.0/PI); - Serial.print(F("Fin Angle: ")); + Serial.print(F("Fin A: ")); Serial.println(hprcRock.finAngle()); - - hprcRock.sendDataComms(commsDevice); + //hprcRock.sendDataComms(commsDevice); //Serial.println(hprcRock.getA_pointing()); //Send Sensor Data for logging switch (flightMode){ @@ -108,11 +106,9 @@ void loop() { break; case 3: //Coast phase, where we control roll - //ailerons.write(servoZero+5); - ailerons.write(hprcRock.finAngle()); - //ailerons.write(servoZero+5); - //Serial.print(F("Fin a")); - //Serial.println(hprcRock.finAngle()); + ailerons.write(servoZero+5); + Serial.print(F("Fin a")); + Serial.println(hprcRock.finAngle()); //ailerons.write(servoZero+hprcRock.finAngle()); if(millis()-lastEventTime>=3000){ flightMode++; diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index f99f845..94508fe 100644 --- a/control/rocketClassDef.cpp +++ b/control/rocketClassDef.cpp @@ -9,9 +9,9 @@ #define minTemp 273.15 #define minFullDeflect 5.0 - using imu::Vector; + rocket::rocket(){ // Orientation Data pitch = 0; @@ -47,9 +47,9 @@ int rocket::updateSensorData(Adafruit_BNO055 &bno, Adafruit_BMP280 &baro){ lastUpdate=current; Q = bno.getQuat(); //Takes a vector and rotates it by the same amount the BNO has since startup - a = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); // convert a into the orignal frame + a =bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); // convert a into the orignal frame - T=baro.readTemperature()+minTemp; + T=baro.readTemperature(); P=baro.readPressure(); up=bno.getVector(Adafruit_BNO055::VECTOR_GRAVITY)*(-1); @@ -138,9 +138,9 @@ int rocket::sendDataComms(int device){ unsigned char i = 0; toCharViaInt(up,msg); i+=6; - toCharViaInt(north,msg+i) + toCharViaInt(north,msg+i); i+=6; - toChar(a,msg+i) + toChar(a,msg+i); i+=12; toChar(lastUpdate, msg+i); i+=4; @@ -171,10 +171,11 @@ int rocket::sendDataComms(int device){ float deltaTheta(float,float); int rocket::finAngle(){ - float k = getSpringConstant() - float c = getDampingConstant() - int raw = (180.0/PI)*(k*deltaTheta(getRoll(),plan.getTargetAngle()*(PI/180))+c*getRollRate())*(minFullDeflect/180.0); - return constrain(-20,raw,20); + Serial.println(F("Test")); + float k = getSpringConstant(); + float c = getDampingConstant(); + int raw = (180.0/PI)*(k*deltaTheta(getRoll(),plan.getTargetAngle(millis())*(PI/180))+c*getRollRate())*(minFullDeflect/180.0); + return constrainFins(-20,raw,20); } float deltaTheta(float a, float b){ From b40f93f428bde427add2ea1b01b25cc0c1b871bf Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Fri, 4 May 2018 18:11:24 -0400 Subject: [PATCH 3/6] Minor tweaks --- control/control.ino | 2 ++ control/rocketClassDef.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/control/control.ino b/control/control.ino index 0cdfff5..b41ee20 100644 --- a/control/control.ino +++ b/control/control.ino @@ -75,6 +75,7 @@ void loop() { Serial.print(F("Pitch: ")); Serial.println(hprcRock.getPitch()*180.0/PI); Serial.print(F("Roll: ")); + delay(100); Serial.println(hprcRock.getRoll()*180.0/PI); Serial.print(F("Fin A: ")); Serial.println(hprcRock.finAngle()); @@ -93,6 +94,7 @@ void loop() { break; case 1: //boost phase + hprcRock.getSpeed();//To keep the integration working if(hprcRock.getA_pointing()<10){ flightMode++; lastEventTime=millis(); diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index 94508fe..3f973a9 100644 --- a/control/rocketClassDef.cpp +++ b/control/rocketClassDef.cpp @@ -189,5 +189,5 @@ float rocket::getDampingConstant(){ return 2*getDampingConstant()/omega_0; } float rocket::getSpringConstant(){ - return getSpeedSq()/(maxSpeed*maxSpeed)*(P/maxPress)*(minTemp/T); + return (getSpeedSq()/(maxSpeed*maxSpeed))*(P/maxPress)*(minTemp/T); } From 610f152491c9b28a43aee29d52d4f63e4609db14 Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Fri, 4 May 2018 18:39:36 -0400 Subject: [PATCH 4/6] Bug fixes --- control/control.ino | 13 ++++++------- control/rocketClassDef.cpp | 6 +++--- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/control/control.ino b/control/control.ino index b41ee20..b8e4628 100644 --- a/control/control.ino +++ b/control/control.ino @@ -3,7 +3,7 @@ #define commsRst 6 #define controlRst 7 #define servoPin 8 -#define servoZero 10 +#define servoZero 20 //Global variables; @@ -72,13 +72,14 @@ void loop() { } - Serial.print(F("Pitch: ")); + /*Serial.print(F("Pitch: ")); Serial.println(hprcRock.getPitch()*180.0/PI); Serial.print(F("Roll: ")); - delay(100); Serial.println(hprcRock.getRoll()*180.0/PI); - Serial.print(F("Fin A: ")); + Serial.print(F("Fin A: "));*/ + Serial.println(hprcRock.finAngle()); + ailerons.write(hprcRock.finAngle()); //hprcRock.sendDataComms(commsDevice); //Serial.println(hprcRock.getA_pointing()); @@ -109,8 +110,6 @@ void loop() { case 3: //Coast phase, where we control roll ailerons.write(servoZero+5); - Serial.print(F("Fin a")); - Serial.println(hprcRock.finAngle()); //ailerons.write(servoZero+hprcRock.finAngle()); if(millis()-lastEventTime>=3000){ flightMode++; @@ -129,7 +128,7 @@ void loop() { break; } //For debug - delay(1000); + //delay(1000); } void receiveHandler(int bytesReceived){ diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index 3f973a9..681e7d3 100644 --- a/control/rocketClassDef.cpp +++ b/control/rocketClassDef.cpp @@ -171,9 +171,9 @@ int rocket::sendDataComms(int device){ float deltaTheta(float,float); int rocket::finAngle(){ - Serial.println(F("Test")); float k = getSpringConstant(); float c = getDampingConstant(); + int raw = (180.0/PI)*(k*deltaTheta(getRoll(),plan.getTargetAngle(millis())*(PI/180))+c*getRollRate())*(minFullDeflect/180.0); return constrainFins(-20,raw,20); } @@ -186,8 +186,8 @@ float deltaTheta(float a, float b){ } float rocket::getDampingConstant(){ - return 2*getDampingConstant()/omega_0; + return 2*getSpringConstant()/omega_0; } float rocket::getSpringConstant(){ - return (getSpeedSq()/(maxSpeed*maxSpeed))*(P/maxPress)*(minTemp/T); + return getSpeedSq()/(maxSpeed*maxSpeed)*(P/maxPress)*(minTemp/T); } From 151cf76095f9a93fac73bd37e37aefcc089842d9 Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Sat, 5 May 2018 16:50:55 -0400 Subject: [PATCH 5/6] Fixed get spring constant --- control/rocketClassDef.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index 681e7d3..dd72637 100644 --- a/control/rocketClassDef.cpp +++ b/control/rocketClassDef.cpp @@ -189,5 +189,5 @@ float rocket::getDampingConstant(){ return 2*getSpringConstant()/omega_0; } float rocket::getSpringConstant(){ - return getSpeedSq()/(maxSpeed*maxSpeed)*(P/maxPress)*(minTemp/T); + return ((maxSpeed*maxSpeed)/getSpeedSq())*(maxPress/P)*(T/minTemp); } From 6969cd9ac2d67f6204f63d14e426212d6b1f271c Mon Sep 17 00:00:00 2001 From: lunar-mycroft Date: Sat, 5 May 2018 20:08:22 -0400 Subject: [PATCH 6/6] Flight code for 05-06 --- control/control.ino | 59 ++++++++++++++++++++++++++++------------- control/rocketClass.hpp | 1 - 2 files changed, 40 insertions(+), 20 deletions(-) diff --git a/control/control.ino b/control/control.ino index b8e4628..cef4cc6 100644 --- a/control/control.ino +++ b/control/control.ino @@ -4,9 +4,14 @@ #define controlRst 7 #define servoPin 8 #define servoZero 20 +#define launchPin 14 +#define systemPin 15 +#define targetAnglePin 16 //Global variables; +int launchConnection = HIGH; + int flightMode; rocket hprcRock; Adafruit_BMP280 bmp; @@ -15,6 +20,7 @@ Servo ailerons; bool nfpValid; bool wireFlag = false; unsigned long lastEventTime=0; +int finA; //Control algorithm functions @@ -22,6 +28,13 @@ float goalTorque(rocket &); float deltaTorque(rocket&,float); void setup() { + pinMode(launchPin, OUTPUT); + digitalWrite(launchPin, LOW); + pinMode(launchPin, INPUT); + + pinMode(systemPin, OUTPUT); + pinMode(targetAnglePin, OUTPUT); + serialDump(); Wire.onRequest(requestHandler); Wire.onReceive(receiveHandler); @@ -62,6 +75,7 @@ void setup() { flightMode=0; delay(3000); + digitalWrite(systemPin, HIGH); //hprcRock.createRefrence(orient, bmp,commsDevice); } @@ -72,48 +86,55 @@ void loop() { } - /*Serial.print(F("Pitch: ")); - Serial.println(hprcRock.getPitch()*180.0/PI); - Serial.print(F("Roll: ")); - Serial.println(hprcRock.getRoll()*180.0/PI); - Serial.print(F("Fin A: "));*/ - - Serial.println(hprcRock.finAngle()); - ailerons.write(hprcRock.finAngle()); - - //hprcRock.sendDataComms(commsDevice); - //Serial.println(hprcRock.getA_pointing()); //Send Sensor Data for logging switch (flightMode){ case 0 : //prelaunch - if(hprcRock.getA_pointing()>20) { - flightMode++; - lastEventTime=millis(); + ailerons.write(servoZero); + launchConnection = digitalRead(launchPin); + if(/*hprcRock.getA_pointing()>20 ||*/ LOW == launchConnection) { + digitalWrite(systemPin, LOW); + flightMode++; + lastEventTime=millis(); } break; case 1: //boost phase + ailerons.write(servoZero); hprcRock.getSpeed();//To keep the integration working - if(hprcRock.getA_pointing()<10){ + if(/*hprcRock.getA_pointing()<10 ||*/ millis()-lastEventTime>=3000){ flightMode++; lastEventTime=millis(); + hprcRock.beginRotation(); } break; case 2: - if(millis()-lastEventTime>=250){ + ailerons.write(servoZero); + hprcRock.getSpeed(); + if(millis()-lastEventTime>=0){ flightMode++; lastEventTime=millis(); + digitalWrite(systemPin, HIGH); } break; case 3: //Coast phase, where we control roll - ailerons.write(servoZero+5); - //ailerons.write(servoZero+hprcRock.finAngle()); - if(millis()-lastEventTime>=3000){ + //ailerons.write(servoZero+5); + finA=hprcRock.finAngle(); + ailerons.write(servoZero+finA); + if(hprcRock.getPitch() up; imu::Vector<3> north; - imu::Vector<3> east; //Rocked basis vectors imu::Vector<3> pointing; //Something like (0,0,1)