Diff: main.cpp

Revision:

6:05a5c22cdfc2

Parent:

5:c24490c61022

Child:

7:945b05cb8683

--- a/main.cpp	Thu Sep 12 13:40:43 2019 +0000
+++ b/main.cpp	Thu Sep 19 13:14:39 2019 +0000
@@ -6,6 +6,7 @@
 #define LOOP_RATE 500.0
 #define CMD_TIMEOUT 1.0
 #define GEAR_RATIO (1/2.75)
+#define CTS_REV 11.0
 #define PI 3.14159
 #include "mbed.h"
 
@@ -18,7 +19,7 @@
 #include <geometry_msgs/Twist.h>
 
 DigitalOut status_LED(LED1);
-DigitalOut armed_LED(LED2);
+DigitalOut wheel_LED(LED2);
 DigitalOut auto_LED(LED3);
 DigitalOut imu_LED(LED4);
 
@@ -61,20 +62,21 @@
 
 float t_imu,t_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
 
-float t_hall, dt_hall,t_run,t_stop,t_log,dt,t_ctrl;
+float t_ws, dt_ws,t_run,t_stop,t_log,dt,t_ctrl;
 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
 float wheel_spd;
 float arm_clock,auto_clock;
 bool str_cond,thr_cond,run_ctrl,log_data;
 int cmd_mode;
+int spd_dir;
 
 
 void wheel_tick_callback()
 {
-    armed_LED = !armed_LED;
+    wheel_LED = !wheel_LED;
 
-    dt_hall = t.read()-t_hall;
-    t_hall = t.read();
+    dt_ws = t.read()-t_ws;
+    t_ws = t.read();
 }
 
 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
@@ -102,6 +104,19 @@
     imu.get_gyro();
     //imu.get_mag();
     imu.get_quat();
+    
+ 
+    
+    if(t.read()-t_ws < 0.2) {
+            wheel_spd = spd_dir*(1/CTS_REV)/(dt_ws); // 0.036 is the wheel radius  v = omega*r
+    } else {
+            wheel_spd = 0;
+            /*if((imu.accel.x < -0.5) && (thr_cmd < 0)){
+                spd_dir = -1;
+            }else{
+                spd_dir = 1;
+            }*/
+    }
 
     if(!auto_ctrl){
         str_cmd = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
@@ -129,7 +144,7 @@
     imu_msg.linear_acceleration.y = imu.accel.y;
     imu_msg.linear_acceleration.z = imu.accel.z;
     
-    pc.printf("st %.2f thr %.2f \r\n",str_cmd,thr_cmd);
+    pc.printf("st %.2f thr %.2f %.2f\r\n",str_cmd,thr_cmd,wheel_spd);
     
     imu_pub.publish(&imu_msg);
 
@@ -154,6 +169,7 @@
     auto_ctrl = false;
     run_ctrl = false;
     log_data = false;
+    spd_dir = 1;
 
     t.start();
     t_imu = t.read();
@@ -187,12 +203,12 @@
     } else {
         pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
         status_LED = 1;
-        armed_LED = 1;
+        wheel_LED = 1;
         imu_LED = 1;
         auto_LED = 1;
         while(1) {
             status_LED = !status_LED;
-            armed_LED = !armed_LED;
+            wheel_LED = !wheel_LED;
             imu_LED = !imu_LED;
             auto_LED = !auto_LED;
             wait(0.5);
@@ -206,8 +222,7 @@
 
     
     while(1) {
-        
-        
+                
         if(t.read()-t_imu > (1/HEARTBEAT_RATE)) {
          //   pc.printf("RC0: %4d   RC1: %4d  ", RC[0].read(), RC[1].read());
             status_LED=!status_LED;