James Bartholomew / Mbed 2 deprecated mbed_IMU_Servo_matlab

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo LSM9DS1 mbed-rtos Clock3 PidCombine

main.cpp@1:57378b3160b9, 2019-04-15 (annotated)

Committer:
JamB
Date:
Mon Apr 15 14:50:49 2019 +0000
Revision:
1:57378b3160b9
Parent:
0:4cf11edf30cf 
hello

Who changed what in which revision?

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