Minor fixes

control/control.ino

@@ -3,10 +3,15 @@
 #define commsRst 6
 #define controlRst 7
 #define servoPin 8
-#define servoZero 10
+#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,14 +20,21 @@ Servo ailerons;
 bool nfpValid;
 bool wireFlag = false;
 unsigned long lastEventTime=0;
+int finA;
 
 //Control algorithm functions
 
 float goalTorque(rocket &);
 float deltaTorque(rocket&,float);
 
 void setup() {
-    delay(1000); // Necessary for syncronization of boards
+     pinMode(launchPin, OUTPUT);
+     digitalWrite(launchPin, LOW);
+     pinMode(launchPin, INPUT);
+
+    pinMode(systemPin, OUTPUT);
+    pinMode(targetAnglePin, OUTPUT);
+
     serialDump();
     Wire.onRequest(requestHandler);
     Wire.onReceive(receiveHandler);
@@ -63,6 +75,7 @@ void setup() {
     flightMode=0;
 
     delay(3000);
+    digitalWrite(systemPin, HIGH);
     //hprcRock.createRefrence(orient, bmp,commsDevice);
 }
 
@@ -73,50 +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 Angle: "));
-    Serial.println(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
-            if(hprcRock.getA_pointing()<10){
+            ailerons.write(servoZero);
+            hprcRock.getSpeed();//To keep the integration working
+            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(hprcRock.finAngle());
-            //ailerons.write(servoZero+5);
-            //Serial.print(F("Fin a"));
-            //Serial.println(hprcRock.finAngle());
-            //ailerons.write(servoZero+hprcRock.finAngle());
-            if(millis()-lastEventTime>=3000){
+            finA=hprcRock.finAngle();
+            ailerons.write(servoZero+finA);
+            if(hprcRock.getPitch()<PI/4){
               flightMode++;
               lastEventTime=millis();
+              digitalWrite(systemPin, LOW);
+            }
+            if (finA == 0)
+            {
+               digitalWrite(targetAnglePin, HIGH);
+            }
+            else
+            {
+                digitalWrite(targetAnglePin, LOW);
             }
             break;
         case 4:
@@ -131,7 +149,7 @@ void loop() {
             break;
     }
     //For debug
-    delay(1000);
+    //delay(1000);
 }
 
 void receiveHandler(int bytesReceived){

control/rocketClass.hpp

@@ -29,7 +29,6 @@ class rocket {
     //Inflight sensor update and logging
     int updateSensorData(Adafruit_BNO055 &, Adafruit_BMP280 &);
     int sendDataComms(int);
-    int sendRefComs(int,const imu::Vector<3> &,imu::Vector<3> &);
 
     //In flight info extraction
     float getSpeed();
@@ -38,16 +37,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();
 
@@ -66,7 +58,6 @@ class rocket {
     //Ground frame basis vectors:
     imu::Vector<3> up;
     imu::Vector<3> north;
-    imu::Vector<3> east;
 
     //Rocked basis vectors
     imu::Vector<3> pointing; //Something like (0,0,1)
@@ -87,12 +78,6 @@ class rocket {
     bool rollMatrixUp2Date;
     bool speedUp2Date;
 
-    float rollResist;
-    float systemStrength;
-
-    float springConst;
-    float dampingConst;
-
     float omega;
     float calibrationPressure;
     float moi;

control/rocketClassDef.cpp

@@ -4,6 +4,10 @@
 #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;
 
@@ -44,7 +48,7 @@ int rocket::updateSensorData(Adafruit_BNO055 &bno, Adafruit_BMP280 &baro){
 
         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
-
+        
         T=baro.readTemperature();
         P=baro.readPressure();
 
@@ -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;
@@ -132,42 +133,19 @@ int rocket::fillModel(int fpsize, int devName){/*
     return 0;
 }
 
-int rocket::sendRefComs(int device,const imu::Vector<3> & g,imu::Vector<3> & m){
-    unsigned char* msg = new unsigned char[packetSize];
-    unsigned char i = 0;
-    toChar(g,msg);
-    i+=3;
-    toChar(m,msg+(i*4));
-    msg[40]=2;
-
-    Wire.beginTransmission(device);
-
-    char j = 0;
-    while (j < packetSize){
-        Wire.write(msg[j]);
-        ++j;
-    }
-
-    Wire.endTransmission();
-    //delete[] out;
-    //out = nullptr;
-    delete[] msg;
-    msg = nullptr;
-    return 0;
-}
-
 int rocket::sendDataComms(int device){
-    unsigned char* msg = new unsigned char[/*packetSize*/32];
+    unsigned char* msg = new unsigned char[32];
     unsigned char i = 0;
-    toChar(Q, msg);
-    i += 4;
-    toChar(a, msg+(i*4));
-    i += 3;
-    /*toChar(P, msg+(i*4));
-    ++i;
-    toChar(T, msg+(i*4));
-    ++i;*/
-    toChar(lastUpdate, msg+(i*4));
+    toCharViaInt(up,msg);
+    i+=6;
+    toCharViaInt(north,msg+i);
+    i+=6;
+    toChar(a,msg+i);
+    i+=12;
+    toChar(lastUpdate, msg+i);
+    i+=4;
+    msg[i]=1;
+
     //msg[4*(++i)] = 1;
 
     //Serial.println("SENDING");
@@ -189,27 +167,15 @@ 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(millis()))*(180.0/PI)+c*getRollRate());
-    return constrainFin(-20,raw,20);
+    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){
@@ -218,3 +184,10 @@ float deltaTheta(float a, float b){
     else if(res<-PI) return 2*PI+res;
     else return res;
 }
+
+float rocket::getDampingConstant(){
+    return 2*getSpringConstant()/omega_0;
+}
+float rocket::getSpringConstant(){
+    return ((maxSpeed*maxSpeed)/getSpeedSq())*(maxPress/P)*(T/minTemp);
+}