USNA ES456 Autonomous Vehicles / Mbed 2 deprecated MadPulse_Controller_ros

Madpulse ROS Code

Dependencies:   mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn

main.cpp@8:c07db2a00c8e, 2019-10-10 (annotated)

Committer:
jdawkins
Date:
Thu Oct 10 13:51:05 2019 +0000
Revision:
8:c07db2a00c8e
Parent:
7:945b05cb8683 
Changed the Messages Sent.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:42d1dda7d9c0 1 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
jdawkins 0:42d1dda7d9c0 2
jdawkins 5:c24490c61022 3 #define HEARTBEAT_RATE 1.0
jdawkins 5:c24490c61022 4 #define LOG_RATE 20.0
jdawkins 5:c24490c61022 5 #define CTRL_RATE 100.0
jdawkins 5:c24490c61022 6 #define LOOP_RATE 500.0
jdawkins 0:42d1dda7d9c0 7 #define CMD_TIMEOUT 1.0
jdawkins 0:42d1dda7d9c0 8 #define GEAR_RATIO (1/2.75)
jdawkins 6:05a5c22cdfc2 9 #define CTS_REV 11.0
jdawkins 0:42d1dda7d9c0 10 #define PI 3.14159
jdawkins 0:42d1dda7d9c0 11 #include "mbed.h"
jdawkins 0:42d1dda7d9c0 12
jdawkins 0:42d1dda7d9c0 13 #include "BNO055.h"
jdawkins 5:c24490c61022 14 #include "ServoIn.h"
jdawkins 5:c24490c61022 15 #include "ServoOut.h"
jdawkins 5:c24490c61022 16 #include <ros.h>
jdawkins 5:c24490c61022 17 #include <sensor_msgs/Imu.h>
jdawkins 5:c24490c61022 18 #include <geometry_msgs/Twist.h>
jdawkins 7:945b05cb8683 19 #include <geometry_msgs/TwistStamped.h>
jdawkins 7:945b05cb8683 20
jdawkins 0:42d1dda7d9c0 21
jdawkins 0:42d1dda7d9c0 22 DigitalOut status_LED(LED1);
jdawkins 6:05a5c22cdfc2 23 DigitalOut wheel_LED(LED2);
jdawkins 0:42d1dda7d9c0 24 DigitalOut auto_LED(LED3);
jdawkins 2:899128d20215 25 DigitalOut imu_LED(LED4);
jdawkins 0:42d1dda7d9c0 26
jdawkins 5:c24490c61022 27 BNO055 imu(p9, p10);
jdawkins 5:c24490c61022 28
jdawkins 5:c24490c61022 29 ServoOut Steer(p26);
jdawkins 5:c24490c61022 30 ServoOut Throttle(p22);
jdawkins 5:c24490c61022 31
jdawkins 5:c24490c61022 32 InterruptIn wheel_sensor(p17);
jdawkins 5:c24490c61022 33
jdawkins 5:c24490c61022 34 ServoIn CH1(p15);
jdawkins 5:c24490c61022 35 ServoIn CH2(p16);
jdawkins 5:c24490c61022 36
jdawkins 5:c24490c61022 37
jdawkins 0:42d1dda7d9c0 38 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 5:c24490c61022 39
jdawkins 5:c24490c61022 40 class XbeeMbedHardware : public MbedHardware
jdawkins 5:c24490c61022 41 {
jdawkins 5:c24490c61022 42 public:
jdawkins 5:c24490c61022 43 XbeeMbedHardware(): MbedHardware(p13, p14, 57600) {};
jdawkins 5:c24490c61022 44 };
jdawkins 5:c24490c61022 45
jdawkins 0:42d1dda7d9c0 46
jdawkins 5:c24490c61022 47 void cmdCallback(const geometry_msgs::Twist& cmd_msg);
jdawkins 5:c24490c61022 48
jdawkins 5:c24490c61022 49 ros::NodeHandle_<XbeeMbedHardware> nh;
jdawkins 5:c24490c61022 50
jdawkins 5:c24490c61022 51 sensor_msgs::Imu imu_msg;
jdawkins 7:945b05cb8683 52 geometry_msgs::TwistStamped vin_msg;
jdawkins 7:945b05cb8683 53
jdawkins 8:c07db2a00c8e 54 ros::Publisher vin_pub("state",&vin_msg);
jdawkins 7:945b05cb8683 55
jdawkins 7:945b05cb8683 56
jdawkins 5:c24490c61022 57 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
jdawkins 0:42d1dda7d9c0 58
jdawkins 0:42d1dda7d9c0 59
jdawkins 0:42d1dda7d9c0 60 Timer t; // create timer instance
jdawkins 5:c24490c61022 61 Ticker crtlTick;
jdawkins 5:c24490c61022 62 Ticker logTick;
jdawkins 0:42d1dda7d9c0 63
jdawkins 5:c24490c61022 64 float t_imu,t_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
jdawkins 5:c24490c61022 65
jdawkins 6:05a5c22cdfc2 66 float t_ws, dt_ws,t_run,t_stop,t_log,dt,t_ctrl;
jdawkins 5:c24490c61022 67 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
jdawkins 8:c07db2a00c8e 68 float spd,spd_filt;
jdawkins 5:c24490c61022 69 float arm_clock,auto_clock;
jdawkins 0:42d1dda7d9c0 70 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 5:c24490c61022 71 int cmd_mode;
jdawkins 6:05a5c22cdfc2 72 int spd_dir;
jdawkins 5:c24490c61022 73
jdawkins 8:c07db2a00c8e 74 float tau = 0.04;
jdawkins 8:c07db2a00c8e 75 float a = 1/tau;
jdawkins 8:c07db2a00c8e 76
jdawkins 5:c24490c61022 77
jdawkins 5:c24490c61022 78 void wheel_tick_callback()
jdawkins 5:c24490c61022 79 {
jdawkins 6:05a5c22cdfc2 80 wheel_LED = !wheel_LED;
jdawkins 5:c24490c61022 81
jdawkins 6:05a5c22cdfc2 82 dt_ws = t.read()-t_ws;
jdawkins 6:05a5c22cdfc2 83 t_ws = t.read();
jdawkins 5:c24490c61022 84 }
jdawkins 5:c24490c61022 85
jdawkins 5:c24490c61022 86 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
jdawkins 5:c24490c61022 87 {
jdawkins 8:c07db2a00c8e 88
jdawkins 5:c24490c61022 89 str_cmd = cmd_msg.angular.z;
jdawkins 5:c24490c61022 90 thr_cmd = cmd_msg.linear.x;
jdawkins 8:c07db2a00c8e 91
jdawkins 8:c07db2a00c8e 92 t_cmd = t.read();
jdawkins 5:c24490c61022 93 // pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
jdawkins 7:945b05cb8683 94
jdawkins 5:c24490c61022 95 }
jdawkins 5:c24490c61022 96
jdawkins 5:c24490c61022 97 void controlLoop()
jdawkins 5:c24490c61022 98 {
jdawkins 5:c24490c61022 99 imu.get_angles();
jdawkins 5:c24490c61022 100 imu.get_accel();
jdawkins 5:c24490c61022 101 imu.get_gyro();
jdawkins 5:c24490c61022 102 //imu.get_mag();
jdawkins 5:c24490c61022 103 imu.get_quat();
jdawkins 6:05a5c22cdfc2 104
jdawkins 7:945b05cb8683 105
jdawkins 8:c07db2a00c8e 106
jdawkins 8:c07db2a00c8e 107
jdawkins 6:05a5c22cdfc2 108 if(t.read()-t_ws < 0.2) {
jdawkins 8:c07db2a00c8e 109 //This would be the notional forward velocity under zero slip condition (not true for hard accel/deceel)
jdawkins 8:c07db2a00c8e 110 spd = 0.0436/(dt_ws); // 0.045 converts ticks/s to m/s
jdawkins 6:05a5c22cdfc2 111 } else {
jdawkins 8:c07db2a00c8e 112 spd = 0;
jdawkins 8:c07db2a00c8e 113 }
jdawkins 8:c07db2a00c8e 114
jdawkins 8:c07db2a00c8e 115 spd_filt = (1-a*dt)*spd_filt + a*spd;
jdawkins 8:c07db2a00c8e 116
jdawkins 8:c07db2a00c8e 117 if(t.read()-t_cmd > 0.2){
jdawkins 8:c07db2a00c8e 118 auto_ctrl = false;
jdawkins 8:c07db2a00c8e 119 }
jdawkins 8:c07db2a00c8e 120 else {
jdawkins 8:c07db2a00c8e 121 auto_ctrl = true;
jdawkins 6:05a5c22cdfc2 122 }
jdawkins 5:c24490c61022 123
jdawkins 5:c24490c61022 124 if(!auto_ctrl){
jdawkins 8:c07db2a00c8e 125 str_cmd = -((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 126 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 127 }
jdawkins 5:c24490c61022 128
jdawkins 8:c07db2a00c8e 129 //saturate throttle
jdawkins 8:c07db2a00c8e 130 if(thr_cmd > 0.3)
jdawkins 8:c07db2a00c8e 131 thr_cmd = 0.3;
jdawkins 8:c07db2a00c8e 132
jdawkins 8:c07db2a00c8e 133 if(thr_cmd < -0.3)
jdawkins 8:c07db2a00c8e 134 thr_cmd = -0.3;
jdawkins 8:c07db2a00c8e 135
jdawkins 8:c07db2a00c8e 136 //saturate steering
jdawkins 8:c07db2a00c8e 137 if(str_cmd > 1.0)
jdawkins 8:c07db2a00c8e 138 str_cmd = 1.0;
jdawkins 8:c07db2a00c8e 139
jdawkins 8:c07db2a00c8e 140 if(str_cmd < -1.0)
jdawkins 8:c07db2a00c8e 141 str_cmd = -1.0;
jdawkins 8:c07db2a00c8e 142
jdawkins 8:c07db2a00c8e 143 if(rc_conn){
jdawkins 8:c07db2a00c8e 144 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 8:c07db2a00c8e 145 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 8:c07db2a00c8e 146 }
jdawkins 8:c07db2a00c8e 147 else{
jdawkins 8:c07db2a00c8e 148 Steer.write(1500);
jdawkins 8:c07db2a00c8e 149 Throttle.write(1500);
jdawkins 8:c07db2a00c8e 150 }
jdawkins 8:c07db2a00c8e 151
jdawkins 5:c24490c61022 152 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 153 }
jdawkins 5:c24490c61022 154
jdawkins 5:c24490c61022 155 void logLoop(){
jdawkins 5:c24490c61022 156
jdawkins 8:c07db2a00c8e 157 pc.printf("str %d, thr %d",CH1.servoPulse,CH2.servoPulse);
jdawkins 8:c07db2a00c8e 158
jdawkins 8:c07db2a00c8e 159 vin_msg.header.stamp = nh.now();
jdawkins 8:c07db2a00c8e 160 vin_msg.header.frame_id = "body";
jdawkins 8:c07db2a00c8e 161 vin_msg.twist.linear.x = spd;
jdawkins 8:c07db2a00c8e 162 vin_msg.twist.linear.y = thr_cmd;
jdawkins 8:c07db2a00c8e 163 vin_msg.twist.linear.z = str_cmd;
jdawkins 5:c24490c61022 164
jdawkins 8:c07db2a00c8e 165 vin_msg.twist.angular.x = imu.accel.x;
jdawkins 8:c07db2a00c8e 166 vin_msg.twist.angular.y = imu.accel.y;
jdawkins 8:c07db2a00c8e 167 vin_msg.twist.angular.z = imu.gyro.z;
jdawkins 8:c07db2a00c8e 168
jdawkins 7:945b05cb8683 169
jdawkins 7:945b05cb8683 170 vin_pub.publish(&vin_msg);
jdawkins 7:945b05cb8683 171
jdawkins 5:c24490c61022 172 }
jdawkins 5:c24490c61022 173
jdawkins 5:c24490c61022 174 float wrapToPi(float ang);
jdawkins 0:42d1dda7d9c0 175
jdawkins 0:42d1dda7d9c0 176 int main()
jdawkins 0:42d1dda7d9c0 177 {
jdawkins 5:c24490c61022 178 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)
jdawkins 0:42d1dda7d9c0 179
jdawkins 0:42d1dda7d9c0 180 pc.baud(115200);
jdawkins 0:42d1dda7d9c0 181
jdawkins 5:c24490c61022 182 wheel_sensor.rise(&wheel_tick_callback);
jdawkins 5:c24490c61022 183
jdawkins 0:42d1dda7d9c0 184 str_cond = false;
jdawkins 0:42d1dda7d9c0 185 thr_cond = false;
jdawkins 0:42d1dda7d9c0 186 armed = false;
jdawkins 0:42d1dda7d9c0 187 auto_ctrl = false;
jdawkins 0:42d1dda7d9c0 188 run_ctrl = false;
jdawkins 0:42d1dda7d9c0 189 log_data = false;
jdawkins 6:05a5c22cdfc2 190 spd_dir = 1;
jdawkins 8:c07db2a00c8e 191 spd_filt = 0;
jdawkins 0:42d1dda7d9c0 192 t.start();
jdawkins 0:42d1dda7d9c0 193 t_imu = t.read();
jdawkins 5:c24490c61022 194 t_ctrl = t.read();
jdawkins 7:945b05cb8683 195 t_hb = t.read();
jdawkins 8:c07db2a00c8e 196 dt = 1/CTRL_RATE;
jdawkins 0:42d1dda7d9c0 197 t_cmd = 0;
jdawkins 7:945b05cb8683 198
jdawkins 7:945b05cb8683 199 thr_cmd = 0;
jdawkins 7:945b05cb8683 200 str_cmd = 0;
jdawkins 7:945b05cb8683 201
jdawkins 0:42d1dda7d9c0 202
jdawkins 0:42d1dda7d9c0 203 status_LED = 1;
jdawkins 0:42d1dda7d9c0 204
jdawkins 0:42d1dda7d9c0 205 if(imu.check()) {
jdawkins 5:c24490c61022 206
jdawkins 0:42d1dda7d9c0 207 pc.printf("BNO055 connected\r\n");
jdawkins 0:42d1dda7d9c0 208 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:42d1dda7d9c0 209 imu.SetExternalCrystal(1);
jdawkins 0:42d1dda7d9c0 210 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 5:c24490c61022 211 imu.set_angle_units(RADIANS);
jdawkins 3:82e223a4a4e4 212 imu.set_accel_units(MPERSPERS);
jdawkins 5:c24490c61022 213 imu.set_anglerate_units(RAD_PER_SEC);
jdawkins 3:82e223a4a4e4 214 imu.setoutputformat(WINDOWS);
jdawkins 2:899128d20215 215 imu.set_mapping(2);
jdawkins 3:82e223a4a4e4 216
jdawkins 5:c24490c61022 217 /* while(int(imu.calib) < 0xCF) {
jdawkins 5:c24490c61022 218 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 5:c24490c61022 219 imu.get_calib();
jdawkins 5:c24490c61022 220 wait(0.5);
jdawkins 5:c24490c61022 221 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 222 }*/
jdawkins 5:c24490c61022 223 imu_LED = 1;
jdawkins 5:c24490c61022 224
jdawkins 5:c24490c61022 225
jdawkins 0:42d1dda7d9c0 226 } else {
jdawkins 0:42d1dda7d9c0 227 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:42d1dda7d9c0 228 status_LED = 1;
jdawkins 6:05a5c22cdfc2 229 wheel_LED = 1;
jdawkins 2:899128d20215 230 imu_LED = 1;
jdawkins 0:42d1dda7d9c0 231 auto_LED = 1;
jdawkins 0:42d1dda7d9c0 232 while(1) {
jdawkins 0:42d1dda7d9c0 233 status_LED = !status_LED;
jdawkins 6:05a5c22cdfc2 234 wheel_LED = !wheel_LED;
jdawkins 2:899128d20215 235 imu_LED = !imu_LED;
jdawkins 0:42d1dda7d9c0 236 auto_LED = !auto_LED;
jdawkins 0:42d1dda7d9c0 237 wait(0.5);
jdawkins 0:42d1dda7d9c0 238 }
jdawkins 0:42d1dda7d9c0 239 }
jdawkins 5:c24490c61022 240 pc.printf("ES456 Vehicle Control\r\n");
jdawkins 5:c24490c61022 241
jdawkins 7:945b05cb8683 242 //Initialize ROS Node and Advertise Publishers
jdawkins 5:c24490c61022 243 nh.initNode();
jdawkins 7:945b05cb8683 244 nh.advertise(vin_pub);
jdawkins 5:c24490c61022 245 nh.subscribe(cmd_sub);
jdawkins 0:42d1dda7d9c0 246
jdawkins 5:c24490c61022 247
jdawkins 5:c24490c61022 248 while(1) {
jdawkins 8:c07db2a00c8e 249
jdawkins 8:c07db2a00c8e 250 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 8:c07db2a00c8e 251 rc_conn = false;
jdawkins 8:c07db2a00c8e 252 } else {
jdawkins 8:c07db2a00c8e 253 rc_conn = true;
jdawkins 8:c07db2a00c8e 254 }
jdawkins 6:05a5c22cdfc2 255
jdawkins 7:945b05cb8683 256 if(t.read()-t_hb > (1/HEARTBEAT_RATE)) {
jdawkins 7:945b05cb8683 257
jdawkins 5:c24490c61022 258 status_LED=!status_LED;
jdawkins 7:945b05cb8683 259 t_hb = t.read();
jdawkins 5:c24490c61022 260 } // if t.read
jdawkins 5:c24490c61022 261
jdawkins 5:c24490c61022 262 if(t.read()-t_ctrl > (1/CTRL_RATE)){
jdawkins 5:c24490c61022 263 controlLoop();
jdawkins 7:945b05cb8683 264 t_ctrl = t.read();
jdawkins 5:c24490c61022 265 }
jdawkins 5:c24490c61022 266
jdawkins 5:c24490c61022 267 if(t.read()-t_log > (1/LOG_RATE)){
jdawkins 5:c24490c61022 268 logLoop();
jdawkins 7:945b05cb8683 269 t_log = t.read();
jdawkins 5:c24490c61022 270 }
jdawkins 0:42d1dda7d9c0 271
jdawkins 3:82e223a4a4e4 272
jdawkins 5:c24490c61022 273 nh.spinOnce();
jdawkins 5:c24490c61022 274 wait_us(10);
jdawkins 0:42d1dda7d9c0 275 } // while (1)
jdawkins 0:42d1dda7d9c0 276
jdawkins 0:42d1dda7d9c0 277 } // main
jdawkins 0:42d1dda7d9c0 278
jdawkins 0:42d1dda7d9c0 279
jdawkins 5:c24490c61022 280 float wrapToPi(float ang)
jdawkins 5:c24490c61022 281 {
jdawkins 7:945b05cb8683 282 while(ang > PI) {
jdawkins 5:c24490c61022 283 ang = ang - 2*PI;
jdawkins 5:c24490c61022 284 }
jdawkins 7:945b05cb8683 285 while (ang < -PI) {
jdawkins 5:c24490c61022 286 ang = ang + 2*PI;
jdawkins 5:c24490c61022 287 }
jdawkins 5:c24490c61022 288 return ang;
jdawkins 5:c24490c61022 289 }
jdawkins 5:c24490c61022 290
jdawkins 5:c24490c61022 291