Diff: main.cpp

Revision:

0:189488114aaa

--- /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