Code to run automated boat
Dependencies: BNO055_fusion ServoOut mbed
main.cpp@0:189488114aaa, 2016-12-12 (annotated)
- Committer:
- m176846
- Date:
- Mon Dec 12 14:49:19 2016 +0000
- Revision:
- 0:189488114aaa
ES413 Boat Control;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m176846 | 0:189488114aaa | 1 | #include "mbed.h" |
m176846 | 0:189488114aaa | 2 | #include "ServoOut.h" |
m176846 | 0:189488114aaa | 3 | #include "BNO055.h" |
m176846 | 0:189488114aaa | 4 | |
m176846 | 0:189488114aaa | 5 | I2C i2c(p28, p27); // SDA, SCL |
m176846 | 0:189488114aaa | 6 | BNO055 imu(i2c, p29); |
m176846 | 0:189488114aaa | 7 | ServoOut leftmotor(p20); |
m176846 | 0:189488114aaa | 8 | ServoOut rightmotor (p19); |
m176846 | 0:189488114aaa | 9 | |
m176846 | 0:189488114aaa | 10 | |
m176846 | 0:189488114aaa | 11 | // RAM ------------------------------------------------------------------------------------------- |
m176846 | 0:189488114aaa | 12 | BNO055_ID_INF_TypeDef bno055_id_inf; |
m176846 | 0:189488114aaa | 13 | BNO055_EULER_TypeDef euler_angles; |
m176846 | 0:189488114aaa | 14 | BNO055_QUATERNION_TypeDef quaternion; |
m176846 | 0:189488114aaa | 15 | BNO055_LIN_ACC_TypeDef linear_acc; |
m176846 | 0:189488114aaa | 16 | BNO055_GRAVITY_TypeDef gravity; |
m176846 | 0:189488114aaa | 17 | BNO055_TEMPERATURE_TypeDef chip_temp; |
m176846 | 0:189488114aaa | 18 | |
m176846 | 0:189488114aaa | 19 | //---------- Gyroscope Caliblation ------------------------------------------------------------ |
m176846 | 0:189488114aaa | 20 | // (a) Place the device in a single stable position for a period of few seconds to allow the |
m176846 | 0:189488114aaa | 21 | |
m176846 | 0:189488114aaa | 22 | DigitalOut led(LED1); |
m176846 | 0:189488114aaa | 23 | Timer tim; //Outputs time during each trial |
m176846 | 0:189488114aaa | 24 | float fl; //Force of left motor |
m176846 | 0:189488114aaa | 25 | float fr; //Force of right motor |
m176846 | 0:189488114aaa | 26 | float old_Mz=0; //Initializes previous torque value |
m176846 | 0:189488114aaa | 27 | float old_e=0; //Initializes previous error value |
m176846 | 0:189488114aaa | 28 | float e=0; //Initializes error value |
m176846 | 0:189488114aaa | 29 | float Mz=0; //Initializes torque value |
m176846 | 0:189488114aaa | 30 | float psi; //Computes actual heading |
m176846 | 0:189488114aaa | 31 | float psi_d; //Compues desired heading |
m176846 | 0:189488114aaa | 32 | float L; //Gain value |
m176846 | 0:189488114aaa | 33 | float angle_des; //change this based on what desired angle is |
m176846 | 0:189488114aaa | 34 | float t_s; //Time |
m176846 | 0:189488114aaa | 35 | float pi=3.14159; |
m176846 | 0:189488114aaa | 36 | float vL; //Left voltage |
m176846 | 0:189488114aaa | 37 | float vR; //Right voltage |
m176846 | 0:189488114aaa | 38 | float bias; //Addes extra force to move boat forward |
m176846 | 0:189488114aaa | 39 | |
m176846 | 0:189488114aaa | 40 | |
m176846 | 0:189488114aaa | 41 | void direction_ctrl(); |
m176846 | 0:189488114aaa | 42 | void motor_ctrl(); |
m176846 | 0:189488114aaa | 43 | |
m176846 | 0:189488114aaa | 44 | int main() |
m176846 | 0:189488114aaa | 45 | { |
m176846 | 0:189488114aaa | 46 | tim.start(); |
m176846 | 0:189488114aaa | 47 | while(tim.read()<45.0) { |
m176846 | 0:189488114aaa | 48 | direction_ctrl(); //read direction of boath through imu |
m176846 | 0:189488114aaa | 49 | motor_ctrl(); //implement direction at with direction desired to calculate motor power values and power motors with lbf values |
m176846 | 0:189488114aaa | 50 | wait(0.01); |
m176846 | 0:189488114aaa | 51 | } |
m176846 | 0:189488114aaa | 52 | leftmotor = 0; |
m176846 | 0:189488114aaa | 53 | rightmotor = 0; |
m176846 | 0:189488114aaa | 54 | |
m176846 | 0:189488114aaa | 55 | // Trap |
m176846 | 0:189488114aaa | 56 | while(1); |
m176846 | 0:189488114aaa | 57 | } |
m176846 | 0:189488114aaa | 58 | |
m176846 | 0:189488114aaa | 59 | //end of main |
m176846 | 0:189488114aaa | 60 | //start of void |
m176846 | 0:189488114aaa | 61 | void direction_ctrl() |
m176846 | 0:189488114aaa | 62 | { |
m176846 | 0:189488114aaa | 63 | // end of calibration, start of reading the yaw, pitch and roll |
m176846 | 0:189488114aaa | 64 | imu.get_Euler_Angles(&euler_angles); |
m176846 | 0:189488114aaa | 65 | psi=euler_angles.h*(pi/180.0); //convert degrees to radians for actual position |
m176846 | 0:189488114aaa | 66 | // eular_angles.h is yaw which is psi |
m176846 | 0:189488114aaa | 67 | } |
m176846 | 0:189488114aaa | 68 | |
m176846 | 0:189488114aaa | 69 | |
m176846 | 0:189488114aaa | 70 | void motor_ctrl() |
m176846 | 0:189488114aaa | 71 | { |
m176846 | 0:189488114aaa | 72 | // running controller with variable aging |
m176846 | 0:189488114aaa | 73 | if (tim.read()<=5 ) { //segment turns boat to desired heading, goes forward, turns around, and comes back all in five second incriments |
m176846 | 0:189488114aaa | 74 | angle_des = 80; |
m176846 | 0:189488114aaa | 75 | bias = 0; |
m176846 | 0:189488114aaa | 76 | } else if(tim.read()>5 && tim.read()<=10) { |
m176846 | 0:189488114aaa | 77 | bias = 5; //Increase voltage to move forward |
m176846 | 0:189488114aaa | 78 | } else if(tim.read()>10 && tim.read()<=15+10) { |
m176846 | 0:189488114aaa | 79 | angle_des = 260; |
m176846 | 0:189488114aaa | 80 | bias = 0; |
m176846 | 0:189488114aaa | 81 | } else if (tim.read()>15+10 && tim.read()<= 20+10) { |
m176846 | 0:189488114aaa | 82 | bias = 5; |
m176846 | 0:189488114aaa | 83 | } |
m176846 | 0:189488114aaa | 84 | //Sample time (NOT SETTLE TIME) |
m176846 | 0:189488114aaa | 85 | t_s=0.01; |
m176846 | 0:189488114aaa | 86 | |
m176846 | 0:189488114aaa | 87 | //calculate error |
m176846 | 0:189488114aaa | 88 | psi_d=angle_des*pi/180.0; //find desired angle in radians |
m176846 | 0:189488114aaa | 89 | e=psi_d-psi; //find difference between desired and actual angle |
m176846 | 0:189488114aaa | 90 | Mz= 6*(0.9608*old_Mz + 32.56*e - 32.53*old_e); //Torque Control |
m176846 | 0:189488114aaa | 91 | L=(24.0/12.0)*0.3048; //in to ft to m |
m176846 | 0:189488114aaa | 92 | fl=Mz/(2.0*L)+bias; //convert torque to force |
m176846 | 0:189488114aaa | 93 | fr=-Mz/(2.0*L)+bias; //convert torque to force |
m176846 | 0:189488114aaa | 94 | |
m176846 | 0:189488114aaa | 95 | vL=.019*(fl*fl*fl)-.4*(fl*fl)+3.3*fl-1.6; //convert force to voltage with the voltage to Newtons conversion for motor (/133 for boat motors) |
m176846 | 0:189488114aaa | 96 | vR=.019*(fr*fr*fr)-.4*(fr*fr)+3.3*fr-1.6; //convert force to voltage with the voltage to Newtons conversion for motor |
m176846 | 0:189488114aaa | 97 | |
m176846 | 0:189488114aaa | 98 | leftmotor=1500.0+((500.0/12.0)*vL) ; // convert from voltage to proper pulsewidth |
m176846 | 0:189488114aaa | 99 | rightmotor= 1500.0 + ((500.0/12.0)*vR); //convert from voltage to proper pulsewidth, 2000 pulsewidth gotten at 12 V |
m176846 | 0:189488114aaa | 100 | |
m176846 | 0:189488114aaa | 101 | // printf("angle=%+6.1f vL=%f vR=%f\r\n", euler_angles.h, vL, vR); |
m176846 | 0:189488114aaa | 102 | //Age variables |
m176846 | 0:189488114aaa | 103 | old_e=e; |
m176846 | 0:189488114aaa | 104 | old_Mz=Mz; |
m176846 | 0:189488114aaa | 105 | } |