File content as of revision 6:d133964667f3:

// =========================================================================
// ES456 Autonomous Vehicles
// Template for MadPulse Vehicle Control
// Dawkins, Piper - Nov 2016
// =========================================================================

#define LOG_RATE 25.0
#define LOOP_RATE 200.0
#define CMD_TIMEOUT 1.0
#define WHL_RADIUS  0.7188    // Wheel radius (inches)
#define GEAR_RATIO (1/2.75)
#define PI 3.14159
#include "mbed.h"

#include "BNO055.h"
#include "ServoIn.h"
#include "ServoOut.h"

BNO055 imu(p9, p10);

void he_callback();
void menuFunction(Serial *port);
DigitalOut status_LED(LED1);
DigitalOut armed_LED(LED2);
DigitalOut auto_LED(LED3);
DigitalOut hall_LED(LED4);

Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
Serial 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_cmd,str_cmd,thr_cmd,dt;
float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;

bool armed, auto_ctrl;
float speed;
float arm_clock;
bool str_cond,thr_cond,run_ctrl,log_data;

// USER DEFINED VARIABLES
float distance;
float steering_offset;
float hdg_comp, hdg_gyro, hdg_est; 


// ============================================
// Main Program
// ============================================
int main()
{

    pc.baud(115200);
    xbee.baud(115200);

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

    str_cmd = 0;
    thr_cmd = 0;
    arm_clock =0; 
    str_cond = false;
    thr_cond = false;
    armed = false;
    auto_ctrl = false;
    run_ctrl = false;
    log_data = false;

    t.start();
    t_log = t.read();
    t_log_start = t.read();
    t_cmd = 0;

    status_LED = 1;

    if(imu.check()) {
        pc.printf("BNO055 connected\r\n");
        xbee.printf("BNO055 connected\r\n");
        imu.setmode(OPERATION_MODE_CONFIG);
        imu.SetExternalCrystal(1);
        imu.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
        imu.set_angle_units(DEGREES);
        imu.set_accel_units(MPERSPERS);
        imu.set_anglerate_units(DEG_PER_SEC);
        imu.set_mapping(2);
       
    } else {
        pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
        status_LED = 1;
        armed_LED = 1;
        hall_LED = 1;
        auto_LED = 1;
        while(1) {
            status_LED = !status_LED;
            armed_LED = !armed_LED;
            hall_LED = !hall_LED;
            auto_LED = !auto_LED;
            wait(0.5);
        }
    } // imu.check

    
    // INITIALIZE USER DEFINED VARIABLES  
    distance = 0;  // Initialize distance
    steering_offset = 0; // Steering command offset
    hdg_gyro = 0; // Initialize integrated gyro heading
    hdg_est = 0; // Initialize heading estimate
   
    
    // ============================================
    // Control Loop
    // ============================================
    while(1) {
        
        // *******************************************
        // Check failsafe conditions
        // *******************************************
        // Arm car when RC Reciever is connected
        if(CH1.servoPulse == 0 || CH2.servoPulse ==0) {
            armed = false;
        } else {
            armed = true;
        }
        // -------------------------------------------
        // Enable auto control when throttle is zero
        str_cond = (CH1.servoPulse > 1800); // If steering is full right
        thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
        if(str_cond&thr_cond) {     // Both of the above conditions must be met
            if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
                Steer.write((int)(1500.0));
                auto_ctrl = true;   //Active auto control loop
            }
        } else {
            arm_clock = t.read();
        }       
        // ------------------------------------------
        // Check PC terminal for run command
        //menuFunction(&pc);
        menuFunction(&xbee);
        
        // *******************************************
        // Get Sensor Data
        // *******************************************
        // Compute speed from hall effect sensor
        if(t.read()-t_hall < 0.2) {
            speed = 2*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall);   // inches/sec
        } else {
            speed = 0;
        }
        // ------------------------------------------
        // IMU data
        imu.get_accel();    // imu.accel.x, imu.accel.y, imu.accel.z
        imu.get_gyro();     // imu.gyro.x, imu.gyro.y, imu.gyro.z
        imu.get_mag();      // imu.mag.x, imu.mag.y, imu.mag.z

 
        // *******************************************
        // Begin Control
        // *******************************************
 
        if(armed) { // Is System Armed
            armed_LED = 1;

            if(auto_ctrl) { // Armed and Auto Control enabled
                auto_LED = 1; // Turn on LED to indicate Auto Control

                if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved

                    float run_time = t.read()-t_run;
 
                    //Integrate speed to determine distance (inches)
                    distance = distance + speed *dt;
                    
                    // ***COMPUTE HEADING HERE***
                    // Compute Heading via magnetometer (deg)
                    //hdg_comp = ... ;
                    
                    // Heading via integrated gyro
                    //hdg_gyro = ... ; 
                     
                   // Heading estimate
                    //hdg_est = ... ;  
                    
                   
                   // ***COMPUTE CONTROL LAW HERE *** 
                   // For now it is just a step input steering angle
                   if(distance <= 72 ) {
                        thr_cmd = 0.2;
                        str_cmd = 0.0;
                    } else if(distance > 72 && distance <= 150) {
                        thr_cmd = 0.2;
                        str_cmd = 0.5;
                    } else {  // Run completed
                        thr_cmd = 0;
                        str_cmd = 0;
                        distance = 0;  // Reset distance for next run
                        run_ctrl = false; // Turn off run control
                        log_data = false;
                    }
                
                    // Compensate for steering offset
                    str_cmd = str_cmd + steering_offset;

                } // End if run_ctrl

                // Write steering and throttle commands to vehicle
                Steer.write((int)((str_cmd*500.0)+1500.0));
                Throttle.write((int)((thr_cmd*500.0)+1500.0));

                if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control
                    auto_ctrl = false;
                    run_ctrl = false;
                }
            } // End if auto_ctrl

            else { // Armed but in Manual Control

                if(xbee.readable()) {
                    t_run = t.read();
                    log_data = !log_data;
                }
                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));  // Write Steering Pulse
                Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse

                auto_LED = 0;
            } // end if autocontrol

        } else {  // System is not aArmed
            armed_LED = 0;          //Turn off armed LED indicator
            str_cmd = 0;
            thr_cmd = 0;
            Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
            Throttle.write(1500);   //Set Throttle to Low
        }/// end  else armed

        
        // ***************************************
        // Transmit data to ground station
        // ***************************************
        if(t.read()-t_log > (1/LOG_RATE)) {
            if(log_data) {
              xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z);
             }
             t_log = t.read();
         } // End transmit data
        
        dt = t.read()-t_loop;  // Sample period (sec)
        t_loop = t.read();
  
        wait(1/LOOP_RATE);
        status_LED=!status_LED;
        
    } // End control loop

} // End main


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

    dt_hall = t.read()-t_hall;
    t_hall = t.read();
}


void menuFunction(Serial *port){
        if(port->readable()) {
            char c = port->getc();
            if(c == 'H' || c == 'h') {
                port->printf("Command List...\r\n");
                port->printf("a - set autonomous mode\r\n");              
                port->printf("l - toggle data logging\r\n");
                port->printf("r - run auto control loop\r\n");
                port->printf("h - display this prompt and exit\r\n");
                port->printf("Enter Command\r\n");
            }
            if(c == 'A' || c == 'a') {
                auto_ctrl = true;
                Steer.write((int)(1500.0));
            }           

            if(c == 'R' || c == 'r') {
                auto_ctrl = true;
                Steer.write((int)(1500.0));
                                
                if(!log_data) {
                    t_log_start = t.read();
                    t_run = t.read();
                    xbee.printf("\r\n\r\n");
                }
                log_data = !log_data;
                run_ctrl = true;
            }

        }

}