File content as of revision 5:c24490c61022:

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

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_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,auto_clock;
bool str_cond,thr_cond,run_ctrl,log_data;
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);
   // 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;

    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;
        armed_LED = 1;
        imu_LED = 1;
        auto_LED = 1;
        while(1) {
            status_LED = !status_LED;
            armed_LED = !armed_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;
}