File content as of revision 8:9817993e5df7:

#include "mbed.h"
#include "utilityFunctions.h"
#include "BNO055.h"
#include "ServoIn.h"
#include "ServoOut.h"
#include "MODSERIAL.h"
#include "vehicleModel.h"
#include "motor_driver.h"

#define MAX_MESSAGE_SIZE 64
#define IMU_RATE 100.0
#define LOG_RATE 10.0
#define LOOP_RATE 300.0
#define CMD_TIMEOUT 1.0
#define GEAR_RATIO (1/2.75) // gear ratio
#define WHL_RADIUS  0.036    // Wheel radius (m)
#define PI 3.14159

// Reference origin is at entrance to hospital point monument
#define REF_LAT 38.986534
#define REF_LON -76.489914
#define REF_ALT 1.8
#define NUM_LED 28
#define LED_CLUSTERS 4
#define LED_PER_CLUSTER 12

#define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
#define COURSE_MODE 1 //Commands map to heading and speed

#define HARDWARE_MODE 0
#define HIL_MODE 1 // commands map to hardware active simulation
#define SIL_MODE 2 // commands map to software only simulation

//I2C    i2c(p9, p10); // SDA, SCL
BNO055 imu(p9, p10);

int left;

// Function Prototypes
void he_callback();
int parseMessage(char * msg);

// LED Definitions
DigitalOut rc_LED(LED1);
DigitalOut armed_LED(LED2);
DigitalOut auto_LED(LED3);
// DigitalOut imu_LED(LED4);
DigitalOut hall_LED(LED4);


// Comms and control object definitions
//MODSERIAL pc(p28, p27); // tx, rx for serial USB interface to pc
//MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee

MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
MODSERIAL xbee(p28, p27); // tx, rx for Xbee
ServoIn CH1(p15);
ServoIn CH2(p16);

InterruptIn he_sensor(p11);
ServoOut Steer(p22);
ServoOut Throttle(p21);





Timer t; // create timer instance
Ticker log_tick;
Ticker heartbeat;
float t_imu,t_gps,t_cmd,t_sim,t_ctrl;
float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
float psi_err,spd_err, psi_err_i,spd_err_i,speed; // control variables
float t_hall, dt_hall,t_run,t_stop,t_log;
bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
float wheel_spd; // wheel speed variable
float arm_clock,auto_clock; // timing for arming procedures
bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
int cmd_mode; // integer to set command mode of controller
float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
int led1,led2,led3,led4; // neo-strip variables?
volatile bool newpacket = false; // boolean identifier of new odroid packet
float x0; // initial x-position when in software or hardware in the loop simulation
float y0; // initial y-position when in software or hardware in the loop simulation
int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
float x = 0.0; // simulation variables
float y = 0.0; // simulation variables
float psi = 0.0; // simulation variables
float spd = 0.0; // simulation speed
float tau = 0.25; // throttle time constant
float Kdc = 1; // throttle dc gain
float thr_out,thr_out_old;
float KpWhl = 0.6;
float dz;
int reverse;

vehicleModel vm; //create vehicle model object


void rxCallback(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == '\n') {
        newpacket = true;
    }
}


int main()
{


    pc.baud(115200); // set baud rate of serial comm to pc
    xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
    Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
    Throttle.write(1500);   //Set Throttle to Low

    xbee.attach(&rxCallback, MODSERIAL::RxIrq);
   

    he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt

    // initialize necessary float and boolean variables
    left = 0;
    str_cmd = 0;
    str=0;
    thr=0;
    thr_cmd = 0;
    des_psi = 0;
    des_spd = 0;
    psi_err = 0;
    spd_err = 0;
    spd_err_i = 0;
    arm_clock = 0;
    auto_clock = 0;
    Kp_psi = 1/1.57;
    Kp_spd = 0.4;
    Ki_spd = 0.2;
    dz = 0.2;
    str_cond = false;
    thr_cond = false;
    armed = false;
    rc_conn = false;
    auto_ctrl = false;
    auto_ctrl_old = false;
    run_ctrl = false;
    log_data = false;
    cmd_mode = 1;
    reverse = 0;
    

    // timer and timing initializations
    t.start();
    t_imu = t.read();
    t_sim = t.read();
    t_ctrl = t.read();
    t_log = t.read();
    t_cmd = 0;


    rc_LED = 0; // turn off LED 1 to indicate no RC connection
//    imu_LED = 0; // turn off IMU indicator (LED 4)


    // procedure to ensure IMU is operating correctly
    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.setoutputformat(WINDOWS);
        imu.set_mapping(2);

        // Blinks light if IMU is not calibrated, stops when calibration is complete
        /*while(int(imu.calib) < 0xCF) {
            pc.printf("Calibration = %x.\n\r",imu.calib);
            imu.get_calib();
            wait(0.5);
            imu_LED = !imu_LED;
        } // end while(imu.calib)
        */
        //imu_LED = 1; // turns on IMU light when calibration is successful

    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly

        //sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)


        /* // turn on all lights is IMU is not connected correctly
        rc_LED = 1;
        armed_LED = 1;
        imu_LED = 1;
        auto_LED = 1;
        while(1) {
            // blink all lights if IMU is not connected correctly
            rc_LED = !rc_LED;
            armed_LED = !armed_LED;
            imu_LED = !imu_LED;
            auto_LED = !auto_LED;

            wait(0.5);
        } // end while(1) {blink if IMU is not connected}
        */

    } // end if(imu.check)


    //  int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};

    pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running


    while(1) {

        // check for servo pulse from either channel of receiver module
        if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
            rc_conn = false;
        } else {
            rc_conn = true;
        } // end if(Channels connected)

        // turn on RC led if transmitter is connected
        rc_LED = rc_conn;
        auto_LED = auto_ctrl;
        armed_LED = armed;

        str_cond = (CH1.servoPulse > 1800); // If steering is full right
        thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero

        if(t.read()-auto_clock > 1) { //Auto control timeout if no commands recevied after 1 seconds
            auto_ctrl = false;
        } // end if(t.read()-autoclock>3) timeout procedure




        if(newpacket) { // if xbee port receives a complete message, parse it
            char buf[MAX_MESSAGE_SIZE]; // create buffer for message

            // reads from modserial port buffer, stores characters into string "buf"
            int i = 0;
            if(xbee.rxBufferGetCount()>0) {
                char c = xbee.getc();
                //pc.printf("%s",c);
                if(c=='$') {
                    buf[i] = c;
                    i++;
                    while(1) {
                        buf[i] = xbee.getc();
                        if(buf[i]=='\n') {
                            break;
                        }
                        i++;
                    }
                }
            } // end if xbee.rxBufferGetCount
            xbee.rxBufferFlush();// empty receive buffer
            //pc.printf("%s",buf); // print message to PC
            parseMessage(buf);
            newpacket = false; // reset packet flag
        } // end if(newpacket)
        float cdt = t.read()-t_ctrl;
        t_ctrl = t.read();


        if(!rc_conn) { // Is System Armed, system armed if RC not connected
            // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
            if(auto_ctrl) {


                switch (cmd_mode) {
                    case DIRECT_MODE: {

                        str = -str_cmd;


                        des_spd = thr_cmd;
                        spd_err = des_spd - speed;
                        spd_err_i += spd_err*cdt;

                        // saturate integral term
                        if(spd_err_i> 1) {
                            spd_err_i = 1;
                        }
                        if(spd_err<-1) {
                            spd_err_i = -1;
                        }
                        
                        thr = Kp_spd*(des_spd-speed) + Ki_spd*spd_err_i;
                        
                        if(des_spd > 0.02){
                        thr = thr + dz;    
                        }   
                        else if(des_spd < -0.02){
                         // speed has no sign figure out how to make it track direction using accel perhaps.
                         thr = -(dz+0.2); 
                         //thr = 0.3*des_spd;   
                        }else {
                         thr = 0; 
                         spd_err_i = 0;   
                        }
                        //thr = thr_cmd;
                        //  pc.printf("des_spd %.3f, Spd %.3f Thr_cmd %.3f rev %d\r\n",des_spd,reverse*speed,thr,reverse);
                        //  xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
                        break;
                    } // end direct mode case
                    case COURSE_MODE: {

                        if(sim_mode==0) { // if hardware is enabled use gyro and ekf
                            //  psi_err = wrapToPi(des_psi-imu.euler.yaw);

                            //  spd_err = des_spd - ekf.getSpd();
                            //  spd_err_i += spd_err;
                            //  str = -Kp_psi*psi_err/ekf.getSpd();
                        } else { // otherwise design control using simulated variables, bypass ekf states

                            //  psi_err = wrapToPi(des_psi-vm.getYaw());
                            // str = Kp_psi*psi_err;

                            // spd_err = des_spd - vm.getSpd();
                            // spd_err_i += spd_err;


                            /* if(spd>0.05) {
                                 str = Kp_psi*psi_err/vm.getSpd();
                             } else {
                                 str = Kp_psi*psi_err/0.05;
                             }*/
                        } // end if sim_mode



                        thr =  Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;

                        if (thr >= 1.0) {
                            thr = 1.0;
                            spd_err_i = 0.0; // Reset Integral If Saturated
                        } // end if thr>=1.0
                        if (thr < 0.0) {
                            thr = 0.0;
                            spd_err_i = 0.0;  // Reset Integral If Saturated
                        } // end iff thr<0


                       // pc.printf("Psi Err: %.3f,  Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f rev %d\r\n",psi_err,str,spd_err,Kp_spd,thr,reverse);

                        break;

                    } // end COURSE_MODE case
                    default: {
                        break;
                    } // end default status in switch

                } // end switch(cmd_mode)
                str = saturateCmd(str);
                //xbee.printf("Psi: %.3f Psi Err: %.3f Str Cmd %.3f\r\n",vm.getYaw(),psi_err,str);
                if(sim_mode<2) { // only actuates if in experiment or HIL modes
                    //    Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                    //   Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
                } else {
                    // won't send command to motor and servo if in SIL mode
                }


            } else { // goes with if auto_ctrl, manual control mode

                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                Throttle.write(1500); //Write Throttle Pulse

            } // end else

        } else { // goes with if !rc_conn

            // for manual driving
            auto_ctrl = false;
            armed_LED = 0;          //Turn off armed LED indicator
            str = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
            thr = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1

            if(sim_mode<2) { // if hardware is active send command to servo and throttle
                //Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
                // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
            }

        }/// end  else armed

        float thr_dot = -(1/tau)*thr_out + (Kdc/tau)*thr; // first order filter for throttle
        thr_out = thr_out_old + thr_dot*cdt;
        thr_out_old = thr_out;
        if(sim_mode<2) { // if hardware is active send command to servo and throttle
            Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
            //Throttle.write((int)((thr_out*500.0)+1500.0)); //Write Throttle Pulse
            mot_control(thr);
          //  pc.printf("Motcontrol: %f %f\r\n",thr,thr_out);
        }

        if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
            float dt = t.read()-t_imu;
            if(dt > (1/IMU_RATE)) {
                imu.get_angles();
                imu.get_accel();
                imu.get_gyro();
                imu.get_lia();

                float tic = t.read();
                t_imu = t.read();

            // *******************************************
            // Get Sensor Data
            // *******************************************
            // Compute speed from hall effect sensor
            if(t.read()-t_hall < 0.2) {
                speed = 0.5*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall);   // inches/sec
            } else {
                speed = 0;
            }

            if(abs(speed) <0.01){
                 if(imu.accel.x < -0.5 && thr_out < -0.05){
                 reverse = -1;    
                }else{
                 reverse = 1;    
                }
            }


            } // end if simmode ==0





        } else { // Else we are running a simulation
            float T = t.read()-t_sim;
            vm.stepModel(str,thr,T);
            speed = vm.getSpd();
            t_sim = t.read();
        }

        if(t.read()-t_log > (1/LOG_RATE)) {

            if(sim_mode > 0) {
                xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
                xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
                printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
                printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
                //  pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
                //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
            } else {
                //xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
                pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
                xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
                // xbee.printf("Thr in %.3f Thr_out %.3f\r\n",thr,thr_out);
                xbee.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
                //pc.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);

                //  pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
                //pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
            }
            t_log = t.read();
        }

        wait(1/LOOP_RATE);
        // status_LED=!status_LED;
        auto_ctrl_old = auto_ctrl;

        /* } else { // else condition implies a simulation mode is enabled

             float dt = t.read()-t_imu;
             if(dt > (1/IMU_RATE)) {

                 float tic = t.read();


                 x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
                 y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
                 psi = psi + str*dt; // turn rate kinematics
                 float drag = 0.0;
                 if(spd>1) {
                     drag = 0.0059*spd*spd; // based on drag on a cat sized object
                 } else {
                     drag = 0.0059*spd;
                 }
                 spd = spd + (thr-drag)*dt; // give throttle offset

                 xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
                 pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
                 t_imu = t.read();
             } // end if dt


         } // end if sim_mode */


    } // end while(1)

} // end main



void he_callback()
{
    // Hall effect speed sensor callback
    hall_LED = !hall_LED;

    dt_hall = t.read()-t_hall;
    t_hall = t.read();
} // end void he_callback



int parseMessage(char * msg)
{

    if(!strncmp(msg, "$CMD", 4)) {
        int arg1;
        float arg2,arg3;

        sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
        cmd_mode = arg1;
        auto_clock = t.read();
        switch (cmd_mode) {
            case DIRECT_MODE: {
                auto_ctrl = true;
                str_cmd = arg2;
                thr_cmd = arg3;
            }
            case COURSE_MODE: {
                auto_ctrl = true;
                des_psi = arg2;
                des_spd = arg3;
            }
            default: {
            }
        }
        return 1;
    }   //emd of $CMD

    if(!strncmp(msg, "$PRM", 4)) {
        float arg1,arg2,arg3;
        int type;
        // __disable_irq();        // disable interrupts
        sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
        // __enable_irq();         // enable interrupts

        switch(type) {
            case 1: { // sets PID gains on heading and speed controller
                //pc.printf("%s\n",msg);
                Kp_psi = arg1;
                Kp_spd = arg2;
                Ki_spd = arg3;

                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
                break;
            }
            case 2: { // sets origin of local reference frame

                //pc.printf("%s\n",msg);

                float ref_lat = arg1;
                float ref_lon = arg2;
                float ref_alt = arg3;
                // gps.setRefPoint(ref_lat,ref_lon,ref_alt);

                pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);

                break;
            }
            default: {
            }

        }
        return 1;
    } // end of $PRM

    if(!strncmp(msg, "$SIM", 4)) {
        int arg1;
        float arg2,arg3,arg4;
        sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
        sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
        switch (sim_mode) {
            case HIL_MODE: {
                auto_ctrl = true;
                x = arg2;
                y = arg3;
                psi = arg4;
                vm.initialize(x,y,psi);
            }
            case SIL_MODE: {
                auto_ctrl = true;
                x = arg2;
                y = arg3;
                psi = arg4;
                vm.initialize(x,y,psi);

            }
            default: {
            }
        }
        return 1;
    }   //emd of $SIM
    return 0;

}