diff --git a/control/control.ino b/control/control.ino index 2143cf9..cef4cc6 100644 --- a/control/control.ino +++ b/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,6 +20,7 @@ Servo ailerons; bool nfpValid; bool wireFlag = false; unsigned long lastEventTime=0; +int finA; //Control algorithm functions @@ -22,7 +28,13 @@ 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() &,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; diff --git a/control/rocketClassDef.cpp b/control/rocketClassDef.cpp index 57da49c..dd72637 100644 --- a/control/rocketClassDef.cpp +++ b/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); +}