main.cpp

00001 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
00002 
00003 #define HEARTBEAT_RATE 1.0
00004 #define LOG_RATE 20.0
00005 #define CTRL_RATE 100.0
00006 #define LOOP_RATE 500.0
00007 #define CMD_TIMEOUT 1.0
00008 #define GEAR_RATIO (1/2.75)
00009 #define CTS_REV 11.0
00010 #define PI 3.14159
00011 #include "mbed.h"
00012 
00013 #include "BNO055.h"
00014 #include "ServoIn.h"
00015 #include "ServoOut.h"
00016 #include  <ros.h>
00017 #include <sensor_msgs/Imu.h>
00018 #include <geometry_msgs/Twist.h>
00019 #include <geometry_msgs/TwistStamped.h>
00020 
00021 
00022 DigitalOut status_LED(LED1);
00023 DigitalOut wheel_LED(LED2);
00024 DigitalOut auto_LED(LED3);
00025 DigitalOut imu_LED(LED4);
00026 
00027 BNO055 imu(p9, p10);
00028 
00029 ServoOut Steer(p26);
00030 ServoOut Throttle(p22);
00031 
00032 InterruptIn wheel_sensor(p17);
00033 
00034 ServoIn CH1(p15);
00035 ServoIn CH2(p16);
00036 
00037 
00038 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
00039 
00040 class XbeeMbedHardware : public MbedHardware
00041 {
00042 public:
00043   XbeeMbedHardware(): MbedHardware(p13, p14, 57600) {}; 
00044 };
00045 
00046 
00047 void cmdCallback(const geometry_msgs::Twist& cmd_msg);
00048 
00049 ros::NodeHandle_<XbeeMbedHardware> nh;
00050 
00051 sensor_msgs::Imu imu_msg;
00052 geometry_msgs::TwistStamped vin_msg;
00053 
00054 ros::Publisher vin_pub("state",&vin_msg);
00055 
00056 
00057 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
00058 
00059 
00060 Timer t; // create timer instance
00061 Ticker crtlTick;
00062 Ticker logTick;
00063 
00064 float t_imu,t_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
00065 
00066 float t_ws, dt_ws,t_run,t_stop,t_log,dt,t_ctrl;
00067 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
00068 float spd,spd_filt;
00069 float arm_clock,auto_clock;
00070 bool str_cond,thr_cond,run_ctrl,log_data;
00071 int cmd_mode;
00072 int spd_dir;
00073 
00074 float tau = 0.04;
00075 float a = 1/tau;
00076 
00077 
00078 void wheel_tick_callback()
00079 {
00080     wheel_LED = !wheel_LED;
00081 
00082     dt_ws = t.read()-t_ws;
00083     t_ws = t.read();
00084 }
00085 
00086 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
00087 {
00088 
00089     str_cmd = cmd_msg.angular.z;
00090     thr_cmd = cmd_msg.linear.x;
00091        
00092     t_cmd = t.read();
00093 //    pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
00094  
00095 }
00096 
00097 void controlLoop()
00098 {
00099     imu.get_angles();
00100     imu.get_accel();
00101     imu.get_gyro();
00102     //imu.get_mag();
00103     imu.get_quat();
00104     
00105      
00106      
00107      
00108     if(t.read()-t_ws < 0.2) {
00109             //This would be the notional forward velocity under zero slip condition (not true for hard accel/deceel)
00110             spd = 0.0436/(dt_ws); // 0.045 converts ticks/s to m/s 
00111     } else {
00112             spd = 0;
00113     }
00114     
00115     spd_filt = (1-a*dt)*spd_filt + a*spd;
00116     
00117     if(t.read()-t_cmd > 0.2){
00118       auto_ctrl = false;
00119     }
00120     else {
00121       auto_ctrl = true;
00122     }
00123 
00124     if(!auto_ctrl){
00125         str_cmd = -((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
00126         thr_cmd = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
00127     }
00128     
00129         //saturate throttle
00130     if(thr_cmd > 0.3)
00131         thr_cmd = 0.3;
00132     
00133     if(thr_cmd < -0.3)
00134         thr_cmd = -0.3;
00135         
00136     //saturate steering
00137     if(str_cmd > 1.0)
00138         str_cmd = 1.0;
00139         
00140     if(str_cmd < -1.0)
00141         str_cmd = -1.0;  
00142     
00143     if(rc_conn){
00144         Steer.write((int)((str_cmd*500.0)+1500.0));
00145         Throttle.write((int)((thr_cmd*500.0)+1500.0));
00146     }
00147     else{
00148         Steer.write(1500);
00149         Throttle.write(1500);    
00150     }
00151     
00152     imu_LED = !imu_LED;
00153 }
00154 
00155 void logLoop(){
00156     
00157     pc.printf("str %d, thr %d",CH1.servoPulse,CH2.servoPulse);
00158 
00159     vin_msg.header.stamp = nh.now();
00160     vin_msg.header.frame_id = "body";
00161     vin_msg.twist.linear.x = spd;
00162     vin_msg.twist.linear.y = thr_cmd;
00163     vin_msg.twist.linear.z = str_cmd;
00164     
00165     vin_msg.twist.angular.x = imu.accel.x;
00166     vin_msg.twist.angular.y = imu.accel.y;
00167     vin_msg.twist.angular.z = imu.gyro.z;
00168 
00169     
00170     vin_pub.publish(&vin_msg);
00171     
00172 }
00173 
00174 float wrapToPi(float ang);
00175 
00176 int main()
00177 {
00178     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)
00179 
00180     pc.baud(115200);
00181 
00182     wheel_sensor.rise(&wheel_tick_callback);
00183 
00184     str_cond = false;
00185     thr_cond = false;
00186     armed = false;
00187     auto_ctrl = false;
00188     run_ctrl = false;
00189     log_data = false;
00190     spd_dir = 1;
00191     spd_filt = 0;
00192     t.start();
00193     t_imu = t.read();
00194     t_ctrl = t.read();
00195     t_hb = t.read();
00196     dt = 1/CTRL_RATE;
00197     t_cmd = 0;
00198     
00199     thr_cmd = 0;
00200     str_cmd = 0;
00201     
00202 
00203     status_LED = 1;
00204 
00205     if(imu.check()) {
00206 
00207         pc.printf("BNO055 connected\r\n");
00208         imu.setmode(OPERATION_MODE_CONFIG);
00209         imu.SetExternalCrystal(1);
00210         imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
00211         imu.set_angle_units(RADIANS);
00212         imu.set_accel_units(MPERSPERS);
00213         imu.set_anglerate_units(RAD_PER_SEC);
00214         imu.setoutputformat(WINDOWS);
00215         imu.set_mapping(2);
00216 
00217      /*   while(int(imu.calib) < 0xCF) {
00218             pc.printf("Calibration = %x.\n\r",imu.calib);
00219             imu.get_calib();
00220             wait(0.5);
00221             imu_LED = !imu_LED;
00222         }*/
00223         imu_LED = 1;
00224 
00225 
00226     } else {
00227         pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
00228         status_LED = 1;
00229         wheel_LED = 1;
00230         imu_LED = 1;
00231         auto_LED = 1;
00232         while(1) {
00233             status_LED = !status_LED;
00234             wheel_LED = !wheel_LED;
00235             imu_LED = !imu_LED;
00236             auto_LED = !auto_LED;
00237             wait(0.5);
00238         }
00239     }
00240     pc.printf("ES456 Vehicle Control\r\n");
00241     
00242     //Initialize ROS Node and Advertise Publishers
00243     nh.initNode();
00244     nh.advertise(vin_pub);
00245     nh.subscribe(cmd_sub);
00246 
00247     
00248     while(1) {
00249         
00250         if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
00251             rc_conn = false;
00252         } else {
00253             rc_conn = true;
00254         }
00255                 
00256         if(t.read()-t_hb > (1/HEARTBEAT_RATE)) {
00257 
00258             status_LED=!status_LED;    
00259             t_hb = t.read();
00260         } // if t.read
00261         
00262         if(t.read()-t_ctrl > (1/CTRL_RATE)){
00263             controlLoop();
00264             t_ctrl = t.read();
00265         }
00266         
00267         if(t.read()-t_log > (1/LOG_RATE)){
00268             logLoop();
00269             t_log = t.read();
00270         }
00271 
00272 
00273         nh.spinOnce();
00274         wait_us(10);
00275     } // while (1)
00276 
00277 } // main
00278 
00279 
00280 float wrapToPi(float ang)
00281 {
00282     while(ang > PI) {
00283         ang = ang - 2*PI;
00284     }
00285     while (ang < -PI) {
00286         ang = ang + 2*PI;
00287     }
00288     return ang;
00289 }
00290 
00291