Diff: main.cpp

Revision:

7:945b05cb8683

Parent:

6:05a5c22cdfc2

Child:

8:c07db2a00c8e

--- a/main.cpp	Thu Sep 19 13:14:39 2019 +0000
+++ b/main.cpp	Thu Oct 03 14:28:03 2019 +0000
@@ -13,10 +13,11 @@
 #include "BNO055.h"
 #include "ServoIn.h"
 #include "ServoOut.h"
-//#include "RC_Channel.h"
 #include  <ros.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistStamped.h>
+
 
 DigitalOut status_LED(LED1);
 DigitalOut wheel_LED(LED2);
@@ -30,9 +31,6 @@
 
 InterruptIn wheel_sensor(p17);
 
-//RC_Channel  RC[] = {RC_Channel(p15,1), RC_Channel(p16,2)};
-//RC_Channel  RC(p15,1); // instanciate the class
-//RC_Channel  RC(p16,2);
 ServoIn CH1(p15);
 ServoIn CH2(p16);
 
@@ -51,11 +49,15 @@
 ros::NodeHandle_<XbeeMbedHardware> nh;
 
 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::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
 
 
-
 Timer t; // create timer instance
 Ticker crtlTick;
 Ticker logTick;
@@ -93,8 +95,7 @@
     Throttle.write((int)((thr_cmd*500.0)+1500.0));
 
 //    pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
-    t_cmd = t.read();
-
+ 
 }
 
 void controlLoop()
@@ -105,17 +106,11 @@
     //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
+            wheel_spd = (2*PI)/(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){
@@ -130,6 +125,7 @@
 
 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;
@@ -147,6 +143,17 @@
     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_pub.publish(&vin_msg);
+    
+ 
 
 }
 
@@ -157,9 +164,6 @@
     NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
 
     pc.baud(115200);
-  //  xbee.baud(115200);
-   // logTick.attach(&logLoop,0.1);
-   // crtlTick.attach(&controlLoop,0.02);
 
     wheel_sensor.rise(&wheel_tick_callback);
 
@@ -174,8 +178,13 @@
     t.start();
     t_imu = t.read();
     t_ctrl = t.read();
+    t_hb = t.read();
     dt = 0;
     t_cmd = 0;
+    
+    thr_cmd = 0;
+    str_cmd = 0;
+    
 
     status_LED = 1;
 
@@ -216,25 +225,29 @@
     }
     pc.printf("ES456 Vehicle Control\r\n");
     
+    //Initialize ROS Node and Advertise Publishers
     nh.initNode();
     nh.advertise(imu_pub);
+    nh.advertise(vin_pub);
     nh.subscribe(cmd_sub);
 
     
     while(1) {
                 
-        if(t.read()-t_imu > (1/HEARTBEAT_RATE)) {
-         //   pc.printf("RC0: %4d   RC1: %4d  ", RC[0].read(), RC[1].read());
+        if(t.read()-t_hb > (1/HEARTBEAT_RATE)) {
+
             status_LED=!status_LED;    
-            t_imu = t.read();
+            t_hb = t.read();
         } // if t.read
         
         if(t.read()-t_ctrl > (1/CTRL_RATE)){
             controlLoop();
+            t_ctrl = t.read();
         }
         
         if(t.read()-t_log > (1/LOG_RATE)){
             logLoop();
+            t_log = t.read();
         }
 
 
@@ -251,15 +264,12 @@
 
 float wrapToPi(float ang)
 {
-
-    if(ang > PI) {
-
+    while(ang > PI) {
         ang = ang - 2*PI;
     }
-    if (ang < -PI) {
+    while (ang < -PI) {
         ang = ang + 2*PI;
     }
-
     return ang;
 }