Nicholas Weinhardt / Mbed 2 deprecated ES413Integration

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?

UserRevisionLine numberNew 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 }