Code to run automated boat
Dependencies: BNO055_fusion ServoOut mbed
Revision 0:189488114aaa, committed 2016-12-12
- Comitter:
- m176846
- Date:
- Mon Dec 12 14:49:19 2016 +0000
- Commit message:
- ES413 Boat Control;
Changed in this revision
diff -r 000000000000 -r 189488114aaa BNO055_fusion.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055_fusion.lib Mon Dec 12 14:49:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/kenjiArai/code/BNO055_fusion/#9e6fead1e93e
diff -r 000000000000 -r 189488114aaa ServoOut.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ServoOut.lib Mon Dec 12 14:49:19 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/jebradshaw/code/ServoOut/#6a59017c4f62
diff -r 000000000000 -r 189488114aaa main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 12 14:49:19 2016 +0000 @@ -0,0 +1,105 @@ +#include "mbed.h" +#include "ServoOut.h" +#include "BNO055.h" + +I2C i2c(p28, p27); // SDA, SCL +BNO055 imu(i2c, p29); +ServoOut leftmotor(p20); +ServoOut rightmotor (p19); + + +// RAM ------------------------------------------------------------------------------------------- +BNO055_ID_INF_TypeDef bno055_id_inf; +BNO055_EULER_TypeDef euler_angles; +BNO055_QUATERNION_TypeDef quaternion; +BNO055_LIN_ACC_TypeDef linear_acc; +BNO055_GRAVITY_TypeDef gravity; +BNO055_TEMPERATURE_TypeDef chip_temp; + +//---------- Gyroscope Caliblation ------------------------------------------------------------ +// (a) Place the device in a single stable position for a period of few seconds to allow the + +DigitalOut led(LED1); +Timer tim; //Outputs time during each trial +float fl; //Force of left motor +float fr; //Force of right motor +float old_Mz=0; //Initializes previous torque value +float old_e=0; //Initializes previous error value +float e=0; //Initializes error value +float Mz=0; //Initializes torque value +float psi; //Computes actual heading +float psi_d; //Compues desired heading +float L; //Gain value +float angle_des; //change this based on what desired angle is +float t_s; //Time +float pi=3.14159; +float vL; //Left voltage +float vR; //Right voltage +float bias; //Addes extra force to move boat forward + + +void direction_ctrl(); +void motor_ctrl(); + +int main() +{ + tim.start(); + while(tim.read()<45.0) { + direction_ctrl(); //read direction of boath through imu + motor_ctrl(); //implement direction at with direction desired to calculate motor power values and power motors with lbf values + wait(0.01); + } + leftmotor = 0; + rightmotor = 0; + + // Trap + while(1); +} + +//end of main +//start of void +void direction_ctrl() +{ + // end of calibration, start of reading the yaw, pitch and roll + imu.get_Euler_Angles(&euler_angles); + psi=euler_angles.h*(pi/180.0); //convert degrees to radians for actual position + // eular_angles.h is yaw which is psi +} + + +void motor_ctrl() +{ + // running controller with variable aging + if (tim.read()<=5 ) { //segment turns boat to desired heading, goes forward, turns around, and comes back all in five second incriments + angle_des = 80; + bias = 0; + } else if(tim.read()>5 && tim.read()<=10) { + bias = 5; //Increase voltage to move forward + } else if(tim.read()>10 && tim.read()<=15+10) { + angle_des = 260; + bias = 0; + } else if (tim.read()>15+10 && tim.read()<= 20+10) { + bias = 5; + } +//Sample time (NOT SETTLE TIME) + t_s=0.01; + +//calculate error + psi_d=angle_des*pi/180.0; //find desired angle in radians + e=psi_d-psi; //find difference between desired and actual angle + Mz= 6*(0.9608*old_Mz + 32.56*e - 32.53*old_e); //Torque Control + L=(24.0/12.0)*0.3048; //in to ft to m + fl=Mz/(2.0*L)+bias; //convert torque to force + fr=-Mz/(2.0*L)+bias; //convert torque to force + + 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) + 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 + + leftmotor=1500.0+((500.0/12.0)*vL) ; // convert from voltage to proper pulsewidth + rightmotor= 1500.0 + ((500.0/12.0)*vR); //convert from voltage to proper pulsewidth, 2000 pulsewidth gotten at 12 V + + // printf("angle=%+6.1f vL=%f vR=%f\r\n", euler_angles.h, vL, vR); +//Age variables + old_e=e; + old_Mz=Mz; +} \ No newline at end of file
diff -r 000000000000 -r 189488114aaa mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 12 14:49:19 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9bcdf88f62b0 \ No newline at end of file