Diff: main.cpp

Revision:

5:c24490c61022

Parent:

3:82e223a4a4e4

Child:

6:05a5c22cdfc2

--- a/main.cpp	Mon Sep 19 13:00:16 2016 +0000
+++ b/main.cpp	Thu Sep 12 13:40:43 2019 +0000
@@ -1,55 +1,153 @@
 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
 
-#define LOG_RATE 50.0
-#define LOOP_RATE 200.0
+#define HEARTBEAT_RATE 1.0
+#define LOG_RATE 20.0
+#define CTRL_RATE 100.0
+#define LOOP_RATE 500.0
 #define CMD_TIMEOUT 1.0
 #define GEAR_RATIO (1/2.75)
 #define PI 3.14159
 #include "mbed.h"
 
 #include "BNO055.h"
-
-
-BNO055 imu(p9, p10);
+#include "ServoIn.h"
+#include "ServoOut.h"
+//#include "RC_Channel.h"
+#include  <ros.h>
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Twist.h>
 
-
-int left;
-float saturateCmd(float cmd);
-void menuFunction(Serial *port);
 DigitalOut status_LED(LED1);
 DigitalOut armed_LED(LED2);
 DigitalOut auto_LED(LED3);
 DigitalOut imu_LED(LED4);
 
+BNO055 imu(p9, p10);
+
+ServoOut Steer(p26);
+ServoOut Throttle(p22);
+
+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);
+
+
 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
-Serial xbee(p28, p27); // tx, rx for Xbee
+
+class XbeeMbedHardware : public MbedHardware
+{
+public:
+  XbeeMbedHardware(): MbedHardware(p13, p14, 57600) {}; 
+};
+
 
+void cmdCallback(const geometry_msgs::Twist& cmd_msg);
+
+ros::NodeHandle_<XbeeMbedHardware> nh;
+
+sensor_msgs::Imu imu_msg;
+ros::Publisher imu_pub("imu",&imu_msg);
+ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
 
 
 
 Timer t; // create timer instance
+Ticker crtlTick;
+Ticker logTick;
 
-float t_imu,t_cmd,str_cmd,thr_cmd,str,thr;
-float t_hall, dt_hall,t_run,t_stop,t_log;
-bool armed, auto_ctrl;
+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;
+bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
 float wheel_spd;
-float arm_clock;
+float arm_clock,auto_clock;
 bool str_cond,thr_cond,run_ctrl,log_data;
-bool log_imu,log_bno,log_odo,log_mag = false;
+int cmd_mode;
+
+
+void wheel_tick_callback()
+{
+    armed_LED = !armed_LED;
+
+    dt_hall = t.read()-t_hall;
+    t_hall = t.read();
+}
+
+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));
+
+//    pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
+    t_cmd = t.read();
+
+}
+
+void controlLoop()
+{
+    imu.get_angles();
+    imu.get_accel();
+    imu.get_gyro();
+    //imu.get_mag();
+    imu.get_quat();
+
+    if(!auto_ctrl){
+        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));
+    imu_LED = !imu_LED;
+}
+
+void logLoop(){
+    
+    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;
+    
+    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 \r\n",str_cmd,thr_cmd);
+    
+    imu_pub.publish(&imu_msg);
+
+}
+
+float wrapToPi(float ang);
 
 int main()
 {
+    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);
+  //  xbee.baud(115200);
+   // logTick.attach(&logLoop,0.1);
+   // crtlTick.attach(&controlLoop,0.02);
 
-//imu.init_MPU_i2c();
-    left = 0;
-    str_cmd = 0;
-    str=0;
-    thr=0;
-    thr_cmd = 0;
-    arm_clock =0;
+    wheel_sensor.rise(&wheel_tick_callback);
+
     str_cond = false;
     thr_cond = false;
     armed = false;
@@ -59,24 +157,33 @@
 
     t.start();
     t_imu = t.read();
+    t_ctrl = t.read();
+    dt = 0;
     t_cmd = 0;
 
     status_LED = 1;
 
     if(imu.check()) {
-        
+
         pc.printf("BNO055 connected\r\n");
         imu.setmode(OPERATION_MODE_CONFIG);
         imu.SetExternalCrystal(1);
         imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
-        imu.set_angle_units(DEGREES);
+        imu.set_angle_units(RADIANS);
         imu.set_accel_units(MPERSPERS);
-        imu.set_anglerate_units(DEG_PER_SEC);        
+        imu.set_anglerate_units(RAD_PER_SEC);
         imu.setoutputformat(WINDOWS);
         imu.set_mapping(2);
 
-        
-            
+     /*   while(int(imu.calib) < 0xCF) {
+            pc.printf("Calibration = %x.\n\r",imu.calib);
+            imu.get_calib();
+            wait(0.5);
+            imu_LED = !imu_LED;
+        }*/
+        imu_LED = 1;
+
+
     } else {
         pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
         status_LED = 1;
@@ -91,33 +198,54 @@
             wait(0.5);
         }
     }
+    pc.printf("ES456 Vehicle Control\r\n");
+    
+    nh.initNode();
+    nh.advertise(imu_pub);
+    nh.subscribe(cmd_sub);
 
-    pc.printf("ES456 Vehicle Sensor Logger\r\n");
-    while(1){
-                         
-        imu.get_angles();
-        imu.get_accel();
-        imu.get_gyro();
-        imu.get_mag();
-         if(t.read()-t_imu > (1/LOG_RATE)) {
+    
+    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;    
+            t_imu = t.read();
+        } // if t.read
+        
+        if(t.read()-t_ctrl > (1/CTRL_RATE)){
+            controlLoop();
+        }
+        
+        if(t.read()-t_log > (1/LOG_RATE)){
+            logLoop();
+        }
 
 
-             //pc.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);             
-             //xbee.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z);
 
-             pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
-             pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
-             
-           //  xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);             
-           //  xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
-             
-             t_imu = t.read();
-         } // if t.read
-        wait(1/LOOP_RATE);
-        status_LED=!status_LED;
+   // 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)
 
 } // main
 
 
+float wrapToPi(float ang)
+{
 
+    if(ang > PI) {
+
+        ang = ang - 2*PI;
+    }
+    if (ang < -PI) {
+        ang = ang + 2*PI;
+    }
+
+    return ang;
+}
+
+