File content as of revision 7:945b05cb8683:

//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  <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);
DigitalOut auto_LED(LED3);
DigitalOut imu_LED(LED4);

BNO055 imu(p9, p10);

ServoOut Steer(p26);
ServoOut Throttle(p22);

InterruptIn wheel_sensor(p17);

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;
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;

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);
 
}

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 = (2*PI)/(dt_ws); // 0.036 is the wheel radius  v = omega*r
    } else {
            wheel_spd = 0;
    }

    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.stamp = nh.now();
    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);
    
    
    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);
    
 

}

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);

    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();
    t_hb = t.read();
    dt = 0;
    t_cmd = 0;
    
    thr_cmd = 0;
    str_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");
    
    //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_hb > (1/HEARTBEAT_RATE)) {

            status_LED=!status_LED;    
            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();
        }



   // 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)
{
    while(ang > PI) {
        ang = ang - 2*PI;
    }
    while (ang < -PI) {
        ang = ang + 2*PI;
    }
    return ang;
}