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?
User | Revision | Line number | New 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 |