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@7:945b05cb8683, 2019-10-03 (annotated)

Committer:
jdawkins
Date:
Thu Oct 03 14:28:03 2019 +0000
Revision:
7:945b05cb8683
Parent:
6:05a5c22cdfc2
Child:
8:c07db2a00c8e
Added Wheel speed to Publisher

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 5:c24490c61022 54 ros::Publisher imu_pub("imu",&imu_msg);
jdawkins 7:945b05cb8683 55 ros::Publisher vin_pub("vin",&vin_msg);
jdawkins 7:945b05cb8683 56
jdawkins 7:945b05cb8683 57
jdawkins 5:c24490c61022 58 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
jdawkins 0:42d1dda7d9c0 59
jdawkins 0:42d1dda7d9c0 60
jdawkins 0:42d1dda7d9c0 61 Timer t; // create timer instance
jdawkins 5:c24490c61022 62 Ticker crtlTick;
jdawkins 5:c24490c61022 63 Ticker logTick;
jdawkins 0:42d1dda7d9c0 64
jdawkins 5:c24490c61022 65 float t_imu,t_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
jdawkins 5:c24490c61022 66
jdawkins 6:05a5c22cdfc2 67 float t_ws, dt_ws,t_run,t_stop,t_log,dt,t_ctrl;
jdawkins 5:c24490c61022 68 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
jdawkins 0:42d1dda7d9c0 69 float wheel_spd;
jdawkins 5:c24490c61022 70 float arm_clock,auto_clock;
jdawkins 0:42d1dda7d9c0 71 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 5:c24490c61022 72 int cmd_mode;
jdawkins 6:05a5c22cdfc2 73 int spd_dir;
jdawkins 5:c24490c61022 74
jdawkins 5:c24490c61022 75
jdawkins 5:c24490c61022 76 void wheel_tick_callback()
jdawkins 5:c24490c61022 77 {
jdawkins 6:05a5c22cdfc2 78 wheel_LED = !wheel_LED;
jdawkins 5:c24490c61022 79
jdawkins 6:05a5c22cdfc2 80 dt_ws = t.read()-t_ws;
jdawkins 6:05a5c22cdfc2 81 t_ws = t.read();
jdawkins 5:c24490c61022 82 }
jdawkins 5:c24490c61022 83
jdawkins 5:c24490c61022 84 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
jdawkins 5:c24490c61022 85 {
jdawkins 5:c24490c61022 86 if(t.read()-t_cmd > 0.2){
jdawkins 5:c24490c61022 87 auto_ctrl = false;
jdawkins 5:c24490c61022 88 }
jdawkins 5:c24490c61022 89 else {
jdawkins 5:c24490c61022 90 auto_ctrl = true;
jdawkins 5:c24490c61022 91 }
jdawkins 5:c24490c61022 92 str_cmd = cmd_msg.angular.z;
jdawkins 5:c24490c61022 93 thr_cmd = cmd_msg.linear.x;
jdawkins 5:c24490c61022 94 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 95 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 96
jdawkins 5:c24490c61022 97 // pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
jdawkins 7:945b05cb8683 98
jdawkins 5:c24490c61022 99 }
jdawkins 5:c24490c61022 100
jdawkins 5:c24490c61022 101 void controlLoop()
jdawkins 5:c24490c61022 102 {
jdawkins 5:c24490c61022 103 imu.get_angles();
jdawkins 5:c24490c61022 104 imu.get_accel();
jdawkins 5:c24490c61022 105 imu.get_gyro();
jdawkins 5:c24490c61022 106 //imu.get_mag();
jdawkins 5:c24490c61022 107 imu.get_quat();
jdawkins 6:05a5c22cdfc2 108
jdawkins 7:945b05cb8683 109
jdawkins 6:05a5c22cdfc2 110 if(t.read()-t_ws < 0.2) {
jdawkins 7:945b05cb8683 111 wheel_spd = (2*PI)/(dt_ws); // 0.036 is the wheel radius v = omega*r
jdawkins 6:05a5c22cdfc2 112 } else {
jdawkins 6:05a5c22cdfc2 113 wheel_spd = 0;
jdawkins 6:05a5c22cdfc2 114 }
jdawkins 5:c24490c61022 115
jdawkins 5:c24490c61022 116 if(!auto_ctrl){
jdawkins 5:c24490c61022 117 str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 118 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 119 }
jdawkins 5:c24490c61022 120
jdawkins 5:c24490c61022 121 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 122 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 123 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 124 }
jdawkins 5:c24490c61022 125
jdawkins 5:c24490c61022 126 void logLoop(){
jdawkins 5:c24490c61022 127
jdawkins 7:945b05cb8683 128 imu_msg.header.stamp = nh.now();
jdawkins 5:c24490c61022 129 imu_msg.header.frame_id = "body";
jdawkins 5:c24490c61022 130 imu_msg.orientation.x = imu.quat.x;
jdawkins 5:c24490c61022 131 imu_msg.orientation.y = imu.quat.y;
jdawkins 5:c24490c61022 132 imu_msg.orientation.z = imu.quat.z;
jdawkins 5:c24490c61022 133 imu_msg.orientation.w = imu.quat.w;
jdawkins 5:c24490c61022 134
jdawkins 5:c24490c61022 135 imu_msg.angular_velocity.x = imu.gyro.x;
jdawkins 5:c24490c61022 136 imu_msg.angular_velocity.y = imu.gyro.y;
jdawkins 5:c24490c61022 137 imu_msg.angular_velocity.z = imu.gyro.z;
jdawkins 5:c24490c61022 138
jdawkins 5:c24490c61022 139 imu_msg.linear_acceleration.x = imu.accel.x;
jdawkins 5:c24490c61022 140 imu_msg.linear_acceleration.y = imu.accel.y;
jdawkins 5:c24490c61022 141 imu_msg.linear_acceleration.z = imu.accel.z;
jdawkins 5:c24490c61022 142
jdawkins 6:05a5c22cdfc2 143 pc.printf("st %.2f thr %.2f %.2f\r\n",str_cmd,thr_cmd,wheel_spd);
jdawkins 5:c24490c61022 144
jdawkins 5:c24490c61022 145 imu_pub.publish(&imu_msg);
jdawkins 7:945b05cb8683 146
jdawkins 7:945b05cb8683 147
jdawkins 7:945b05cb8683 148 vin_msg.header.frame_id = "body";
jdawkins 7:945b05cb8683 149 vin_msg.twist.linear.x = thr_cmd;
jdawkins 7:945b05cb8683 150 vin_msg.twist.angular.z = str_cmd;
jdawkins 7:945b05cb8683 151
jdawkins 7:945b05cb8683 152 vin_msg.twist.linear.z = wheel_spd;
jdawkins 7:945b05cb8683 153
jdawkins 7:945b05cb8683 154 vin_pub.publish(&vin_msg);
jdawkins 7:945b05cb8683 155
jdawkins 7:945b05cb8683 156
jdawkins 5:c24490c61022 157
jdawkins 5:c24490c61022 158 }
jdawkins 5:c24490c61022 159
jdawkins 5:c24490c61022 160 float wrapToPi(float ang);
jdawkins 0:42d1dda7d9c0 161
jdawkins 0:42d1dda7d9c0 162 int main()
jdawkins 0:42d1dda7d9c0 163 {
jdawkins 5:c24490c61022 164 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 165
jdawkins 0:42d1dda7d9c0 166 pc.baud(115200);
jdawkins 0:42d1dda7d9c0 167
jdawkins 5:c24490c61022 168 wheel_sensor.rise(&wheel_tick_callback);
jdawkins 5:c24490c61022 169
jdawkins 0:42d1dda7d9c0 170 str_cond = false;
jdawkins 0:42d1dda7d9c0 171 thr_cond = false;
jdawkins 0:42d1dda7d9c0 172 armed = false;
jdawkins 0:42d1dda7d9c0 173 auto_ctrl = false;
jdawkins 0:42d1dda7d9c0 174 run_ctrl = false;
jdawkins 0:42d1dda7d9c0 175 log_data = false;
jdawkins 6:05a5c22cdfc2 176 spd_dir = 1;
jdawkins 0:42d1dda7d9c0 177
jdawkins 0:42d1dda7d9c0 178 t.start();
jdawkins 0:42d1dda7d9c0 179 t_imu = t.read();
jdawkins 5:c24490c61022 180 t_ctrl = t.read();
jdawkins 7:945b05cb8683 181 t_hb = t.read();
jdawkins 5:c24490c61022 182 dt = 0;
jdawkins 0:42d1dda7d9c0 183 t_cmd = 0;
jdawkins 7:945b05cb8683 184
jdawkins 7:945b05cb8683 185 thr_cmd = 0;
jdawkins 7:945b05cb8683 186 str_cmd = 0;
jdawkins 7:945b05cb8683 187
jdawkins 0:42d1dda7d9c0 188
jdawkins 0:42d1dda7d9c0 189 status_LED = 1;
jdawkins 0:42d1dda7d9c0 190
jdawkins 0:42d1dda7d9c0 191 if(imu.check()) {
jdawkins 5:c24490c61022 192
jdawkins 0:42d1dda7d9c0 193 pc.printf("BNO055 connected\r\n");
jdawkins 0:42d1dda7d9c0 194 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:42d1dda7d9c0 195 imu.SetExternalCrystal(1);
jdawkins 0:42d1dda7d9c0 196 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 5:c24490c61022 197 imu.set_angle_units(RADIANS);
jdawkins 3:82e223a4a4e4 198 imu.set_accel_units(MPERSPERS);
jdawkins 5:c24490c61022 199 imu.set_anglerate_units(RAD_PER_SEC);
jdawkins 3:82e223a4a4e4 200 imu.setoutputformat(WINDOWS);
jdawkins 2:899128d20215 201 imu.set_mapping(2);
jdawkins 3:82e223a4a4e4 202
jdawkins 5:c24490c61022 203 /* while(int(imu.calib) < 0xCF) {
jdawkins 5:c24490c61022 204 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 5:c24490c61022 205 imu.get_calib();
jdawkins 5:c24490c61022 206 wait(0.5);
jdawkins 5:c24490c61022 207 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 208 }*/
jdawkins 5:c24490c61022 209 imu_LED = 1;
jdawkins 5:c24490c61022 210
jdawkins 5:c24490c61022 211
jdawkins 0:42d1dda7d9c0 212 } else {
jdawkins 0:42d1dda7d9c0 213 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:42d1dda7d9c0 214 status_LED = 1;
jdawkins 6:05a5c22cdfc2 215 wheel_LED = 1;
jdawkins 2:899128d20215 216 imu_LED = 1;
jdawkins 0:42d1dda7d9c0 217 auto_LED = 1;
jdawkins 0:42d1dda7d9c0 218 while(1) {
jdawkins 0:42d1dda7d9c0 219 status_LED = !status_LED;
jdawkins 6:05a5c22cdfc2 220 wheel_LED = !wheel_LED;
jdawkins 2:899128d20215 221 imu_LED = !imu_LED;
jdawkins 0:42d1dda7d9c0 222 auto_LED = !auto_LED;
jdawkins 0:42d1dda7d9c0 223 wait(0.5);
jdawkins 0:42d1dda7d9c0 224 }
jdawkins 0:42d1dda7d9c0 225 }
jdawkins 5:c24490c61022 226 pc.printf("ES456 Vehicle Control\r\n");
jdawkins 5:c24490c61022 227
jdawkins 7:945b05cb8683 228 //Initialize ROS Node and Advertise Publishers
jdawkins 5:c24490c61022 229 nh.initNode();
jdawkins 5:c24490c61022 230 nh.advertise(imu_pub);
jdawkins 7:945b05cb8683 231 nh.advertise(vin_pub);
jdawkins 5:c24490c61022 232 nh.subscribe(cmd_sub);
jdawkins 0:42d1dda7d9c0 233
jdawkins 5:c24490c61022 234
jdawkins 5:c24490c61022 235 while(1) {
jdawkins 6:05a5c22cdfc2 236
jdawkins 7:945b05cb8683 237 if(t.read()-t_hb > (1/HEARTBEAT_RATE)) {
jdawkins 7:945b05cb8683 238
jdawkins 5:c24490c61022 239 status_LED=!status_LED;
jdawkins 7:945b05cb8683 240 t_hb = t.read();
jdawkins 5:c24490c61022 241 } // if t.read
jdawkins 5:c24490c61022 242
jdawkins 5:c24490c61022 243 if(t.read()-t_ctrl > (1/CTRL_RATE)){
jdawkins 5:c24490c61022 244 controlLoop();
jdawkins 7:945b05cb8683 245 t_ctrl = t.read();
jdawkins 5:c24490c61022 246 }
jdawkins 5:c24490c61022 247
jdawkins 5:c24490c61022 248 if(t.read()-t_log > (1/LOG_RATE)){
jdawkins 5:c24490c61022 249 logLoop();
jdawkins 7:945b05cb8683 250 t_log = t.read();
jdawkins 5:c24490c61022 251 }
jdawkins 0:42d1dda7d9c0 252
jdawkins 3:82e223a4a4e4 253
jdawkins 3:82e223a4a4e4 254
jdawkins 5:c24490c61022 255 // Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 256 //Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 257
jdawkins 5:c24490c61022 258 nh.spinOnce();
jdawkins 5:c24490c61022 259 wait_us(10);
jdawkins 0:42d1dda7d9c0 260 } // while (1)
jdawkins 0:42d1dda7d9c0 261
jdawkins 0:42d1dda7d9c0 262 } // main
jdawkins 0:42d1dda7d9c0 263
jdawkins 0:42d1dda7d9c0 264
jdawkins 5:c24490c61022 265 float wrapToPi(float ang)
jdawkins 5:c24490c61022 266 {
jdawkins 7:945b05cb8683 267 while(ang > PI) {
jdawkins 5:c24490c61022 268 ang = ang - 2*PI;
jdawkins 5:c24490c61022 269 }
jdawkins 7:945b05cb8683 270 while (ang < -PI) {
jdawkins 5:c24490c61022 271 ang = ang + 2*PI;
jdawkins 5:c24490c61022 272 }
jdawkins 5:c24490c61022 273 return ang;
jdawkins 5:c24490c61022 274 }
jdawkins 5:c24490c61022 275
jdawkins 5:c24490c61022 276