Diff: main.cpp

Revision:

8:c07db2a00c8e

Parent:

7:945b05cb8683

--- a/main.cpp	Thu Oct 03 14:28:03 2019 +0000
+++ b/main.cpp	Thu Oct 10 13:51:05 2019 +0000
@@ -51,8 +51,7 @@
 sensor_msgs::Imu imu_msg;
 geometry_msgs::TwistStamped vin_msg;
 
-ros::Publisher imu_pub("imu",&imu_msg);
-ros::Publisher vin_pub("vin",&vin_msg);
+ros::Publisher vin_pub("state",&vin_msg);
 
 
 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
@@ -66,12 +65,15 @@
 
 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 spd,spd_filt;
 float arm_clock,auto_clock;
 bool str_cond,thr_cond,run_ctrl,log_data;
 int cmd_mode;
 int spd_dir;
 
+float tau = 0.04;
+float a = 1/tau;
+
 
 void wheel_tick_callback()
 {
@@ -83,17 +85,11 @@
 
 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
 {
-    if(t.read()-t_cmd > 0.2){
-      auto_ctrl = false;
-    }
-    else {
-      auto_ctrl = true;
-    }
+
     str_cmd = cmd_msg.angular.z;
     thr_cmd = cmd_msg.linear.x;
-    Steer.write((int)((str_cmd*500.0)+1500.0));
-    Throttle.write((int)((thr_cmd*500.0)+1500.0));
-
+       
+    t_cmd = t.read();
 //    pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
  
 }
@@ -107,54 +103,72 @@
     imu.get_quat();
     
      
+     
+     
     if(t.read()-t_ws < 0.2) {
-            wheel_spd = (2*PI)/(dt_ws); // 0.036 is the wheel radius  v = omega*r
+            //This would be the notional forward velocity under zero slip condition (not true for hard accel/deceel)
+            spd = 0.0436/(dt_ws); // 0.045 converts ticks/s to m/s 
     } else {
-            wheel_spd = 0;
+            spd = 0;
+    }
+    
+    spd_filt = (1-a*dt)*spd_filt + a*spd;
+    
+    if(t.read()-t_cmd > 0.2){
+      auto_ctrl = false;
+    }
+    else {
+      auto_ctrl = true;
     }
 
     if(!auto_ctrl){
-        str_cmd = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
+        str_cmd = -((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
         thr_cmd = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
     }
     
-    Steer.write((int)((str_cmd*500.0)+1500.0));
-    Throttle.write((int)((thr_cmd*500.0)+1500.0));
+        //saturate throttle
+    if(thr_cmd > 0.3)
+        thr_cmd = 0.3;
+    
+    if(thr_cmd < -0.3)
+        thr_cmd = -0.3;
+        
+    //saturate steering
+    if(str_cmd > 1.0)
+        str_cmd = 1.0;
+        
+    if(str_cmd < -1.0)
+        str_cmd = -1.0;  
+    
+    if(rc_conn){
+        Steer.write((int)((str_cmd*500.0)+1500.0));
+        Throttle.write((int)((thr_cmd*500.0)+1500.0));
+    }
+    else{
+        Steer.write(1500);
+        Throttle.write(1500);    
+    }
+    
     imu_LED = !imu_LED;
 }
 
 void logLoop(){
     
-    imu_msg.header.stamp = nh.now();
-    imu_msg.header.frame_id = "body";
-    imu_msg.orientation.x = imu.quat.x;
-    imu_msg.orientation.y = imu.quat.y;
-    imu_msg.orientation.z = imu.quat.z;
-    imu_msg.orientation.w = imu.quat.w;    
-    
-    imu_msg.angular_velocity.x = imu.gyro.x;
-    imu_msg.angular_velocity.y = imu.gyro.y;
-    imu_msg.angular_velocity.z = imu.gyro.z;
+    pc.printf("str %d, thr %d",CH1.servoPulse,CH2.servoPulse);
+
+    vin_msg.header.stamp = nh.now();
+    vin_msg.header.frame_id = "body";
+    vin_msg.twist.linear.x = spd;
+    vin_msg.twist.linear.y = thr_cmd;
+    vin_msg.twist.linear.z = str_cmd;
     
-    imu_msg.linear_acceleration.x = imu.accel.x;
-    imu_msg.linear_acceleration.y = imu.accel.y;
-    imu_msg.linear_acceleration.z = imu.accel.z;
-    
-    pc.printf("st %.2f thr %.2f %.2f\r\n",str_cmd,thr_cmd,wheel_spd);
-    
-    imu_pub.publish(&imu_msg);
-    
-    
-    vin_msg.header.frame_id = "body";
-    vin_msg.twist.linear.x = thr_cmd;
-    vin_msg.twist.angular.z = str_cmd;
-    
-    vin_msg.twist.linear.z = wheel_spd;
+    vin_msg.twist.angular.x = imu.accel.x;
+    vin_msg.twist.angular.y = imu.accel.y;
+    vin_msg.twist.angular.z = imu.gyro.z;
+
     
     vin_pub.publish(&vin_msg);
     
- 
-
 }
 
 float wrapToPi(float ang);
@@ -174,12 +188,12 @@
     run_ctrl = false;
     log_data = false;
     spd_dir = 1;
-
+    spd_filt = 0;
     t.start();
     t_imu = t.read();
     t_ctrl = t.read();
     t_hb = t.read();
-    dt = 0;
+    dt = 1/CTRL_RATE;
     t_cmd = 0;
     
     thr_cmd = 0;
@@ -227,12 +241,17 @@
     
     //Initialize ROS Node and Advertise Publishers
     nh.initNode();
-    nh.advertise(imu_pub);
     nh.advertise(vin_pub);
     nh.subscribe(cmd_sub);
 
     
     while(1) {
+        
+        if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
+            rc_conn = false;
+        } else {
+            rc_conn = true;
+        }
                 
         if(t.read()-t_hb > (1/HEARTBEAT_RATE)) {
 
@@ -251,10 +270,6 @@
         }
 
 
-
-   // Steer.write((int)((str_cmd*500.0)+1500.0));
-    //Throttle.write((int)((thr_cmd*500.0)+1500.0));
-
         nh.spinOnce();
         wait_us(10);
     } // while (1)