No description
Dependencies: mbed Servo LSM9DS1 Clock PidCombine
Revision 0:4cf11edf30cf, committed 2019-04-11
- Comitter:
- JamB
- Date:
- Thu Apr 11 22:24:36 2019 +0000
- Commit message:
- Here it is
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Clock.lib Thu Apr 11 22:24:36 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/JamB/code/Clock/#293ea2789df6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS1.lib Thu Apr 11 22:24:36 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/JamB/code/LSM9DS1/#801ebe391b00
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PidCombine.lib Thu Apr 11 22:24:36 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/JamB/code/PidCombine/#458d6472bd2d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Thu Apr 11 22:24:36 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 11 22:24:36 2019 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "Servo.h"
+#include "clock.h"
+
+#define PI 3.14159
+#define DECLINATION -2.44
+#define deltat_ 0.004f //250Hz
+
+//Setup Patameters
+Servo servo_1(p24); //left servo
+Servo servo_2(p25); //right servo
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+
+AnalogIn ain(A0);
+AnalogIn ain1(A1);
+
+void Setup()
+{
+ set_tick_(deltat_);
+}
+
+int main()
+{
+ Setup();
+ //Display Setup
+ Timer timer;
+ Timer timer2;
+ // int timee =0;
+ int i=50;
+ int j =0;
+ // int begin, end;
+ // PID config Roll
+ float error=0.0;
+ float P =0.0; //Proportional term
+ float I =0.0; //Intergral term
+ float prevD =0.0;
+ //float prevDy=0.0l;
+ float D =0.0; //Derivative term
+ float k1 = -0.9776; //Proportaional PID weight
+ float k2 = -0.5656; //Integral PID
+ float k3 = -4.2247; //Derivative PID
+ float PV=0.0; //Process variable
+ float SP=0; //Set Point (Desired Roll in degerees)
+ // PID config Pitch
+ float errorp =0.0; //Proportional term
+ float Pp =0.0; //Proportional term
+ float Ip =0.0; //Intergral term
+ float prevDp =0.0;
+ float Dp =0.0; //Derivative term
+ float k1p = -1; //PID weights
+ float k2p = 0.5;
+ float k3p = 10;
+ float PVp=0.0; //Process variable
+ float SPp=0; //Set Point
+ //PID combine setup
+ float rollc;
+ float pitchc;
+ float top;
+ float bot;
+ float centre;
+ float diff;
+ //Servo initial conditions
+ float range = 0.0005;
+ float roll_correct = 0;
+ float pitch_correct= 0; //Range 0-1
+ float left_servo=0;
+ float right_servo=0;
+ servo_1.calibrate(range, 45.0);
+ servo_2.calibrate(range, 45.0);
+ //PWM conversion
+ float left_scale = 0.01778; //convert degrees to PWM. Assumes linear realtionship (is not). optimised for linearity around neutral.
+ float right_scale = -0.01778;
+ float left_neutral = 0.39; //neutral position PWM
+ float right_neutral = 0.61;
+ //IMU
+ LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//SDA, SDL
+ IMU.begin();
+ if (!IMU.begin()) {
+ pc.printf("Failed to communicate with LSM9DS1.\n");
+ }
+ IMU.calibrate(0); //If this is set to 1 the Glider will assume it starts level
+
+ printf("Start");
+ while(1)
+ {
+ if(tock())
+ {
+
+ //Attitude
+ while(!IMU.accelAvailable());
+ IMU.readAccel();
+ float roll = atan2((double)IMU.ax, (double)IMU.az); //swtiched the roll and pitch eqautions round
+ float pitch = atan2(-(double)IMU.ay, sqrt((double)IMU.ax * (double)IMU.ax + (double)IMU.az * (double)IMU.az));
+ pitch *= 180.0 / PI;
+ roll *= 180.0 / PI;
+ // printf("%f %f\n", roll, pitch);
+
+ //If moving up or down adjust the SPp for pitch. Tether should deal with this
+ //shift_SPp=IMU.az;//scale with time and shift by 9.81.
+ //SPp+=shift_SPp ; Adjusting the setpoint would require new PID values.
+
+ //PID for Roll
+ PV =roll;
+ error= PV-SP; //in degrees
+ P = k1*error;
+ I += k2*error;
+ D = k3*error - prevD; //change in glider's roll in degrees since last sample
+ //float N=5.3;
+ //D = (k3*error -prevD)*N -(N-1)*prevDy;
+ prevD =k3*error;
+ //prevDy = D;
+ roll_correct = P + I + D ; //Try PID
+ //Clamp if PID has saturated integral.
+ if(roll_correct<-18 || roll_correct>48) { //Is the PID saturated?
+ if(!((k2*error>0)^(roll_correct>0))) //Is the new intergral contributing to saturation?
+ {
+ I -=k2*error; //Don let accError increase, its saturated
+ roll_correct = P + I + D ; //Update PID
+ }
+ }
+
+ //PID for pitch
+ PVp =pitch;
+ errorp= PVp-SPp; //in degrees
+ Pp = k1p*errorp;
+ Ip += k2p*errorp;
+ Dp = k3p*errorp - prevDp; //change in glider's roll in degrees since last sample
+ prevDp =k3p*errorp;
+ pitch_correct = Pp + Ip + Dp ; //Try PID
+ //Clamp if PID has saturated integral.
+ if(pitch_correct<-18 || pitch_correct>48) { //Is the PID saturated?
+ if(!((k2p*errorp>0)^(pitch_correct>0))) //Is the new intergral contributing to saturation?
+ {
+ Ip -=k2p*errorp; //Don let accError increase, its saturated
+ pitch_correct = Pp + Ip + Dp ; //Update PID
+ }
+ }
+
+ //Update Servos, left and right
+ //left_servo = pitch_correct + roll_correct; //Angle deg from -18 to (18)
+ //right_servo = pitch_correct +(1 - roll_correct); //Angle deg from -18 to (18)
+
+ //Take in the corrects and combine them for Left and Right elevator
+ rollc = (roll_correct/2);
+ pitchc = (-pitch_correct-15);
+ top = rollc+15;
+ bot = 15-rollc;
+ centre = 15;
+
+ while (pitchc != 0)
+ {
+ if (pitchc > 0)
+ {
+ if (top<48)
+ {
+ centre = centre+1;
+ pitchc = pitchc-1;
+ top=top+1;
+ }else{
+ pitchc=0;
+ }
+ }else{
+ if (bot>-18)
+ {
+ centre=centre-1;
+ pitchc=pitchc+1;
+ bot=bot-1;
+ }else{
+ pitchc=0;
+ }
+ }
+ }
+
+ diff=rollc*2;
+ centre=centre;
+
+ //Convert to PWM
+ left_servo = left_neutral + (centre + diff) * left_scale;
+ right_servo = right_neutral + (centre - diff) * right_scale;
+
+ //Saturate outputs(For Safety) -18 to 48deg
+ if(left_servo>0.94){left_servo=0.94;}//0.54
+ if(left_servo<0.07){left_servo=0.07;}
+ if(right_servo>0.93){right_servo=0.93;}
+ if(right_servo<0.06){right_servo=0.06;}//0.41
+
+ //Output to Servos
+ servo_1 = left_servo; //0.07 - 0.59
+ servo_2 = right_servo;//0.93 - 0.41
+
+ //timer.reset();
+ //timer.start();
+ // begin = timer.read_us();
+ //Display
+ if (i>0) //5 every second at 250Hz =5Hz (50)
+ {
+ //pc.printf("(Roll %f) (Pitch %f) \r",roll,pitch);
+ //pc.printf("%f %f \r",roll,pitch);
+ pc.printf("%d %d %d %f %f %f %f %f %f %f %f \r",IMU.ax,IMU.ay,IMU.az,roll, pitch,roll_correct,pitch_correct,left_servo, right_servo, (float)ain.read(), (float)ain1.read());
+ //pc.printf("%f %f %f\r",P,I,D);
+ //pc.printf("%f %f %f %f %f %f\r",P,I,D,Pp,Ip,Dp);
+ i=0;
+ }
+ i++;
+ j++;
+ //end = timer.read_us();
+ //timee=end-begin;
+
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 11 22:24:36 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file