James Bartholomew / Mbed 2 deprecated mbed_IMU_Servo

No description

Dependencies:   mbed Servo LSM9DS1 Clock PidCombine

main.cpp@0:4cf11edf30cf, 2019-04-11 (annotated)

Committer:
JamB
Date:
Thu Apr 11 22:24:36 2019 +0000
Revision:
0:4cf11edf30cf
Here it is

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamB 0:4cf11edf30cf 1 #include "mbed.h"
JamB 0:4cf11edf30cf 2 #include "LSM9DS1.h"
JamB 0:4cf11edf30cf 3 #include "Servo.h"
JamB 0:4cf11edf30cf 4 #include "clock.h"
JamB 0:4cf11edf30cf 5
JamB 0:4cf11edf30cf 6 #define PI 3.14159
JamB 0:4cf11edf30cf 7 #define DECLINATION -2.44
JamB 0:4cf11edf30cf 8 #define deltat_ 0.004f //250Hz
JamB 0:4cf11edf30cf 9
JamB 0:4cf11edf30cf 10 //Setup Patameters
JamB 0:4cf11edf30cf 11 Servo servo_1(p24); //left servo
JamB 0:4cf11edf30cf 12 Servo servo_2(p25); //right servo
JamB 0:4cf11edf30cf 13 DigitalOut myled(LED1);
JamB 0:4cf11edf30cf 14 Serial pc(USBTX, USBRX);
JamB 0:4cf11edf30cf 15
JamB 0:4cf11edf30cf 16 AnalogIn ain(A0);
JamB 0:4cf11edf30cf 17 AnalogIn ain1(A1);
JamB 0:4cf11edf30cf 18
JamB 0:4cf11edf30cf 19 void Setup()
JamB 0:4cf11edf30cf 20 {
JamB 0:4cf11edf30cf 21 set_tick_(deltat_);
JamB 0:4cf11edf30cf 22 }
JamB 0:4cf11edf30cf 23
JamB 0:4cf11edf30cf 24 int main()
JamB 0:4cf11edf30cf 25 {
JamB 0:4cf11edf30cf 26 Setup();
JamB 0:4cf11edf30cf 27 //Display Setup
JamB 0:4cf11edf30cf 28 Timer timer;
JamB 0:4cf11edf30cf 29 Timer timer2;
JamB 0:4cf11edf30cf 30 // int timee =0;
JamB 0:4cf11edf30cf 31 int i=50;
JamB 0:4cf11edf30cf 32 int j =0;
JamB 0:4cf11edf30cf 33 // int begin, end;
JamB 0:4cf11edf30cf 34 // PID config Roll
JamB 0:4cf11edf30cf 35 float error=0.0;
JamB 0:4cf11edf30cf 36 float P =0.0; //Proportional term
JamB 0:4cf11edf30cf 37 float I =0.0; //Intergral term
JamB 0:4cf11edf30cf 38 float prevD =0.0;
JamB 0:4cf11edf30cf 39 //float prevDy=0.0l;
JamB 0:4cf11edf30cf 40 float D =0.0; //Derivative term
JamB 0:4cf11edf30cf 41 float k1 = -0.9776; //Proportaional PID weight
JamB 0:4cf11edf30cf 42 float k2 = -0.5656; //Integral PID
JamB 0:4cf11edf30cf 43 float k3 = -4.2247; //Derivative PID
JamB 0:4cf11edf30cf 44 float PV=0.0; //Process variable
JamB 0:4cf11edf30cf 45 float SP=0; //Set Point (Desired Roll in degerees)
JamB 0:4cf11edf30cf 46 // PID config Pitch
JamB 0:4cf11edf30cf 47 float errorp =0.0; //Proportional term
JamB 0:4cf11edf30cf 48 float Pp =0.0; //Proportional term
JamB 0:4cf11edf30cf 49 float Ip =0.0; //Intergral term
JamB 0:4cf11edf30cf 50 float prevDp =0.0;
JamB 0:4cf11edf30cf 51 float Dp =0.0; //Derivative term
JamB 0:4cf11edf30cf 52 float k1p = -1; //PID weights
JamB 0:4cf11edf30cf 53 float k2p = 0.5;
JamB 0:4cf11edf30cf 54 float k3p = 10;
JamB 0:4cf11edf30cf 55 float PVp=0.0; //Process variable
JamB 0:4cf11edf30cf 56 float SPp=0; //Set Point
JamB 0:4cf11edf30cf 57 //PID combine setup
JamB 0:4cf11edf30cf 58 float rollc;
JamB 0:4cf11edf30cf 59 float pitchc;
JamB 0:4cf11edf30cf 60 float top;
JamB 0:4cf11edf30cf 61 float bot;
JamB 0:4cf11edf30cf 62 float centre;
JamB 0:4cf11edf30cf 63 float diff;
JamB 0:4cf11edf30cf 64 //Servo initial conditions
JamB 0:4cf11edf30cf 65 float range = 0.0005;
JamB 0:4cf11edf30cf 66 float roll_correct = 0;
JamB 0:4cf11edf30cf 67 float pitch_correct= 0; //Range 0-1
JamB 0:4cf11edf30cf 68 float left_servo=0;
JamB 0:4cf11edf30cf 69 float right_servo=0;
JamB 0:4cf11edf30cf 70 servo_1.calibrate(range, 45.0);
JamB 0:4cf11edf30cf 71 servo_2.calibrate(range, 45.0);
JamB 0:4cf11edf30cf 72 //PWM conversion
JamB 0:4cf11edf30cf 73 float left_scale = 0.01778; //convert degrees to PWM. Assumes linear realtionship (is not). optimised for linearity around neutral.
JamB 0:4cf11edf30cf 74 float right_scale = -0.01778;
JamB 0:4cf11edf30cf 75 float left_neutral = 0.39; //neutral position PWM
JamB 0:4cf11edf30cf 76 float right_neutral = 0.61;
JamB 0:4cf11edf30cf 77 //IMU
JamB 0:4cf11edf30cf 78 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//SDA, SDL
JamB 0:4cf11edf30cf 79 IMU.begin();
JamB 0:4cf11edf30cf 80 if (!IMU.begin()) {
JamB 0:4cf11edf30cf 81 pc.printf("Failed to communicate with LSM9DS1.\n");
JamB 0:4cf11edf30cf 82 }
JamB 0:4cf11edf30cf 83 IMU.calibrate(0); //If this is set to 1 the Glider will assume it starts level
JamB 0:4cf11edf30cf 84
JamB 0:4cf11edf30cf 85 printf("Start");
JamB 0:4cf11edf30cf 86 while(1)
JamB 0:4cf11edf30cf 87 {
JamB 0:4cf11edf30cf 88 if(tock())
JamB 0:4cf11edf30cf 89 {
JamB 0:4cf11edf30cf 90
JamB 0:4cf11edf30cf 91 //Attitude
JamB 0:4cf11edf30cf 92 while(!IMU.accelAvailable());
JamB 0:4cf11edf30cf 93 IMU.readAccel();
JamB 0:4cf11edf30cf 94 float roll = atan2((double)IMU.ax, (double)IMU.az); //swtiched the roll and pitch eqautions round
JamB 0:4cf11edf30cf 95 float pitch = atan2(-(double)IMU.ay, sqrt((double)IMU.ax * (double)IMU.ax + (double)IMU.az * (double)IMU.az));
JamB 0:4cf11edf30cf 96 pitch *= 180.0 / PI;
JamB 0:4cf11edf30cf 97 roll *= 180.0 / PI;
JamB 0:4cf11edf30cf 98 // printf("%f %f\n", roll, pitch);
JamB 0:4cf11edf30cf 99
JamB 0:4cf11edf30cf 100 //If moving up or down adjust the SPp for pitch. Tether should deal with this
JamB 0:4cf11edf30cf 101 //shift_SPp=IMU.az;//scale with time and shift by 9.81.
JamB 0:4cf11edf30cf 102 //SPp+=shift_SPp ; Adjusting the setpoint would require new PID values.
JamB 0:4cf11edf30cf 103
JamB 0:4cf11edf30cf 104 //PID for Roll
JamB 0:4cf11edf30cf 105 PV =roll;
JamB 0:4cf11edf30cf 106 error= PV-SP; //in degrees
JamB 0:4cf11edf30cf 107 P = k1*error;
JamB 0:4cf11edf30cf 108 I += k2*error;
JamB 0:4cf11edf30cf 109 D = k3*error - prevD; //change in glider's roll in degrees since last sample
JamB 0:4cf11edf30cf 110 //float N=5.3;
JamB 0:4cf11edf30cf 111 //D = (k3*error -prevD)*N -(N-1)*prevDy;
JamB 0:4cf11edf30cf 112 prevD =k3*error;
JamB 0:4cf11edf30cf 113 //prevDy = D;
JamB 0:4cf11edf30cf 114 roll_correct = P + I + D ; //Try PID
JamB 0:4cf11edf30cf 115 //Clamp if PID has saturated integral.
JamB 0:4cf11edf30cf 116 if(roll_correct<-18 || roll_correct>48) { //Is the PID saturated?
JamB 0:4cf11edf30cf 117 if(!((k2*error>0)^(roll_correct>0))) //Is the new intergral contributing to saturation?
JamB 0:4cf11edf30cf 118 {
JamB 0:4cf11edf30cf 119 I -=k2*error; //Don let accError increase, its saturated
JamB 0:4cf11edf30cf 120 roll_correct = P + I + D ; //Update PID
JamB 0:4cf11edf30cf 121 }
JamB 0:4cf11edf30cf 122 }
JamB 0:4cf11edf30cf 123
JamB 0:4cf11edf30cf 124 //PID for pitch
JamB 0:4cf11edf30cf 125 PVp =pitch;
JamB 0:4cf11edf30cf 126 errorp= PVp-SPp; //in degrees
JamB 0:4cf11edf30cf 127 Pp = k1p*errorp;
JamB 0:4cf11edf30cf 128 Ip += k2p*errorp;
JamB 0:4cf11edf30cf 129 Dp = k3p*errorp - prevDp; //change in glider's roll in degrees since last sample
JamB 0:4cf11edf30cf 130 prevDp =k3p*errorp;
JamB 0:4cf11edf30cf 131 pitch_correct = Pp + Ip + Dp ; //Try PID
JamB 0:4cf11edf30cf 132 //Clamp if PID has saturated integral.
JamB 0:4cf11edf30cf 133 if(pitch_correct<-18 || pitch_correct>48) { //Is the PID saturated?
JamB 0:4cf11edf30cf 134 if(!((k2p*errorp>0)^(pitch_correct>0))) //Is the new intergral contributing to saturation?
JamB 0:4cf11edf30cf 135 {
JamB 0:4cf11edf30cf 136 Ip -=k2p*errorp; //Don let accError increase, its saturated
JamB 0:4cf11edf30cf 137 pitch_correct = Pp + Ip + Dp ; //Update PID
JamB 0:4cf11edf30cf 138 }
JamB 0:4cf11edf30cf 139 }
JamB 0:4cf11edf30cf 140
JamB 0:4cf11edf30cf 141 //Update Servos, left and right
JamB 0:4cf11edf30cf 142 //left_servo = pitch_correct + roll_correct; //Angle deg from -18 to (18)
JamB 0:4cf11edf30cf 143 //right_servo = pitch_correct +(1 - roll_correct); //Angle deg from -18 to (18)
JamB 0:4cf11edf30cf 144
JamB 0:4cf11edf30cf 145 //Take in the corrects and combine them for Left and Right elevator
JamB 0:4cf11edf30cf 146 rollc = (roll_correct/2);
JamB 0:4cf11edf30cf 147 pitchc = (-pitch_correct-15);
JamB 0:4cf11edf30cf 148 top = rollc+15;
JamB 0:4cf11edf30cf 149 bot = 15-rollc;
JamB 0:4cf11edf30cf 150 centre = 15;
JamB 0:4cf11edf30cf 151
JamB 0:4cf11edf30cf 152 while (pitchc != 0)
JamB 0:4cf11edf30cf 153 {
JamB 0:4cf11edf30cf 154 if (pitchc > 0)
JamB 0:4cf11edf30cf 155 {
JamB 0:4cf11edf30cf 156 if (top<48)
JamB 0:4cf11edf30cf 157 {
JamB 0:4cf11edf30cf 158 centre = centre+1;
JamB 0:4cf11edf30cf 159 pitchc = pitchc-1;
JamB 0:4cf11edf30cf 160 top=top+1;
JamB 0:4cf11edf30cf 161 }else{
JamB 0:4cf11edf30cf 162 pitchc=0;
JamB 0:4cf11edf30cf 163 }
JamB 0:4cf11edf30cf 164 }else{
JamB 0:4cf11edf30cf 165 if (bot>-18)
JamB 0:4cf11edf30cf 166 {
JamB 0:4cf11edf30cf 167 centre=centre-1;
JamB 0:4cf11edf30cf 168 pitchc=pitchc+1;
JamB 0:4cf11edf30cf 169 bot=bot-1;
JamB 0:4cf11edf30cf 170 }else{
JamB 0:4cf11edf30cf 171 pitchc=0;
JamB 0:4cf11edf30cf 172 }
JamB 0:4cf11edf30cf 173 }
JamB 0:4cf11edf30cf 174 }
JamB 0:4cf11edf30cf 175
JamB 0:4cf11edf30cf 176 diff=rollc*2;
JamB 0:4cf11edf30cf 177 centre=centre;
JamB 0:4cf11edf30cf 178
JamB 0:4cf11edf30cf 179 //Convert to PWM
JamB 0:4cf11edf30cf 180 left_servo = left_neutral + (centre + diff) * left_scale;
JamB 0:4cf11edf30cf 181 right_servo = right_neutral + (centre - diff) * right_scale;
JamB 0:4cf11edf30cf 182
JamB 0:4cf11edf30cf 183 //Saturate outputs(For Safety) -18 to 48deg
JamB 0:4cf11edf30cf 184 if(left_servo>0.94){left_servo=0.94;}//0.54
JamB 0:4cf11edf30cf 185 if(left_servo<0.07){left_servo=0.07;}
JamB 0:4cf11edf30cf 186 if(right_servo>0.93){right_servo=0.93;}
JamB 0:4cf11edf30cf 187 if(right_servo<0.06){right_servo=0.06;}//0.41
JamB 0:4cf11edf30cf 188
JamB 0:4cf11edf30cf 189 //Output to Servos
JamB 0:4cf11edf30cf 190 servo_1 = left_servo; //0.07 - 0.59
JamB 0:4cf11edf30cf 191 servo_2 = right_servo;//0.93 - 0.41
JamB 0:4cf11edf30cf 192
JamB 0:4cf11edf30cf 193 //timer.reset();
JamB 0:4cf11edf30cf 194 //timer.start();
JamB 0:4cf11edf30cf 195 // begin = timer.read_us();
JamB 0:4cf11edf30cf 196 //Display
JamB 0:4cf11edf30cf 197 if (i>0) //5 every second at 250Hz =5Hz (50)
JamB 0:4cf11edf30cf 198 {
JamB 0:4cf11edf30cf 199 //pc.printf("(Roll %f) (Pitch %f) \r",roll,pitch);
JamB 0:4cf11edf30cf 200 //pc.printf("%f %f \r",roll,pitch);
JamB 0:4cf11edf30cf 201 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());
JamB 0:4cf11edf30cf 202 //pc.printf("%f %f %f\r",P,I,D);
JamB 0:4cf11edf30cf 203 //pc.printf("%f %f %f %f %f %f\r",P,I,D,Pp,Ip,Dp);
JamB 0:4cf11edf30cf 204 i=0;
JamB 0:4cf11edf30cf 205 }
JamB 0:4cf11edf30cf 206 i++;
JamB 0:4cf11edf30cf 207 j++;
JamB 0:4cf11edf30cf 208 //end = timer.read_us();
JamB 0:4cf11edf30cf 209 //timee=end-begin;
JamB 0:4cf11edf30cf 210
JamB 0:4cf11edf30cf 211 }
JamB 0:4cf11edf30cf 212 }
JamB 0:4cf11edf30cf 213 }