Madpulse ROS Code
Dependencies: mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn
Diff: main.cpp
- Revision:
- 5:c24490c61022
- Parent:
- 3:82e223a4a4e4
- Child:
- 6:05a5c22cdfc2
diff -r 0d0d62b0a6bd -r c24490c61022 main.cpp --- 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; +} + +