Madpulse ROS Code
Dependencies: mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn
main.cpp
- Committer:
- jdawkins
- Date:
- 2019-09-19
- Revision:
- 6:05a5c22cdfc2
- Parent:
- 5:c24490c61022
- Child:
- 7:945b05cb8683
File content as of revision 6:05a5c22cdfc2:
//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed #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 CTS_REV 11.0 #define PI 3.14159 #include "mbed.h" #include "BNO055.h" #include "ServoIn.h" #include "ServoOut.h" //#include "RC_Channel.h" #include <ros.h> #include <sensor_msgs/Imu.h> #include <geometry_msgs/Twist.h> DigitalOut status_LED(LED1); DigitalOut wheel_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 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_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd; float t_ws, dt_ws,t_run,t_stop,t_log,dt,t_ctrl; bool armed, auto_ctrl,auto_ctrl_old,rc_conn; float wheel_spd; float arm_clock,auto_clock; bool str_cond,thr_cond,run_ctrl,log_data; int cmd_mode; int spd_dir; void wheel_tick_callback() { wheel_LED = !wheel_LED; dt_ws = t.read()-t_ws; t_ws = 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(t.read()-t_ws < 0.2) { wheel_spd = spd_dir*(1/CTS_REV)/(dt_ws); // 0.036 is the wheel radius v = omega*r } else { wheel_spd = 0; /*if((imu.accel.x < -0.5) && (thr_cmd < 0)){ spd_dir = -1; }else{ spd_dir = 1; }*/ } 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 %.2f\r\n",str_cmd,thr_cmd,wheel_spd); 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); // logTick.attach(&logLoop,0.1); // crtlTick.attach(&controlLoop,0.02); wheel_sensor.rise(&wheel_tick_callback); str_cond = false; thr_cond = false; armed = false; auto_ctrl = false; run_ctrl = false; log_data = false; spd_dir = 1; 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(RADIANS); imu.set_accel_units(MPERSPERS); 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; wheel_LED = 1; imu_LED = 1; auto_LED = 1; while(1) { status_LED = !status_LED; wheel_LED = !wheel_LED; imu_LED = !imu_LED; auto_LED = !auto_LED; wait(0.5); } } pc.printf("ES456 Vehicle Control\r\n"); nh.initNode(); nh.advertise(imu_pub); nh.subscribe(cmd_sub); 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(); } // 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; }