Madpulse ROS Code
Dependencies: mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn
Diff: main.cpp
- Revision:
- 7:945b05cb8683
- Parent:
- 6:05a5c22cdfc2
- Child:
- 8:c07db2a00c8e
diff -r 05a5c22cdfc2 -r 945b05cb8683 main.cpp --- a/main.cpp Thu Sep 19 13:14:39 2019 +0000 +++ b/main.cpp Thu Oct 03 14:28:03 2019 +0000 @@ -13,10 +13,11 @@ #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> +#include <geometry_msgs/TwistStamped.h> + DigitalOut status_LED(LED1); DigitalOut wheel_LED(LED2); @@ -30,9 +31,6 @@ 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); @@ -51,11 +49,15 @@ ros::NodeHandle_<XbeeMbedHardware> nh; sensor_msgs::Imu imu_msg; +geometry_msgs::TwistStamped vin_msg; + ros::Publisher imu_pub("imu",&imu_msg); +ros::Publisher vin_pub("vin",&vin_msg); + + ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback); - Timer t; // create timer instance Ticker crtlTick; Ticker logTick; @@ -93,8 +95,7 @@ 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() @@ -105,17 +106,11 @@ //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 + wheel_spd = (2*PI)/(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){ @@ -130,6 +125,7 @@ void logLoop(){ + imu_msg.header.stamp = nh.now(); imu_msg.header.frame_id = "body"; imu_msg.orientation.x = imu.quat.x; imu_msg.orientation.y = imu.quat.y; @@ -147,6 +143,17 @@ pc.printf("st %.2f thr %.2f %.2f\r\n",str_cmd,thr_cmd,wheel_spd); imu_pub.publish(&imu_msg); + + + vin_msg.header.frame_id = "body"; + vin_msg.twist.linear.x = thr_cmd; + vin_msg.twist.angular.z = str_cmd; + + vin_msg.twist.linear.z = wheel_spd; + + vin_pub.publish(&vin_msg); + + } @@ -157,9 +164,6 @@ 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); @@ -174,8 +178,13 @@ t.start(); t_imu = t.read(); t_ctrl = t.read(); + t_hb = t.read(); dt = 0; t_cmd = 0; + + thr_cmd = 0; + str_cmd = 0; + status_LED = 1; @@ -216,25 +225,29 @@ } pc.printf("ES456 Vehicle Control\r\n"); + //Initialize ROS Node and Advertise Publishers nh.initNode(); nh.advertise(imu_pub); + nh.advertise(vin_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()); + if(t.read()-t_hb > (1/HEARTBEAT_RATE)) { + status_LED=!status_LED; - t_imu = t.read(); + t_hb = t.read(); } // if t.read if(t.read()-t_ctrl > (1/CTRL_RATE)){ controlLoop(); + t_ctrl = t.read(); } if(t.read()-t_log > (1/LOG_RATE)){ logLoop(); + t_log = t.read(); } @@ -251,15 +264,12 @@ float wrapToPi(float ang) { - - if(ang > PI) { - + while(ang > PI) { ang = ang - 2*PI; } - if (ang < -PI) { + while (ang < -PI) { ang = ang + 2*PI; } - return ang; }