File content as of revision 2:2bb375ab3b2b:

#include "madpulse.h"

#define BUFFER_SIZE 1024

#define FIVE_HZ_LOOP 5.0
#define LOG_LOOP_FREQ 25.0 // Four messages are taking turns logging, it should be about 25Hz overall
#define CTRL_LOOP_FREQ 100.0
#define MAIN_LOOP_FREQ 300.0
#define LOG_IMU 1
#define LOG_MAG 2
#define LOG_EUL 3
#define LOG_ODO 4
#define VEH_ID 1

#define PI (3.14159)


// Instantiate Serial Ports
Serial pc(USBTX, USBRX, 115200);           // USB UART Terminal
Serial xbee(p13,p14,115200);

// Instantiate Class Objects
BNO055 imu(p9,p10);             // BNO055 Class Object I2C(SDL,SCL)
ServoIn CH1(p15);               // Read RC Servo Channel 1 (steering) Pin 15
ServoIn CH2(p16);               // Read RC Servo Channel 2 (Throttle) Pin 16
InterruptIn wheel_irq(p17);     // Attach Interrupt for Wheel Encoder Pin 17
ServoOut Steer(p21);            // Steer command object  on Pin 21 goes to steering servo
ServoOut Throttle(p22);         // Throttle command object on Pin 22 goes to esc

Ticker ctrl_ticker;

// Create Instance of Madpulse class which manages low level vehicle functions and exposes a set of methods for
// controlling the vehicle
MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);

// Control Gains

float Kp_spd = 0.0;
float Ki_spd = 0.0;

float Kp_psi = 0.0;
float Kd_psi = 0.0;
float L = 0.01;

float psi_est,psi_est_old;
float spd_err,spd_err_i;
float spd,des_spd;
float dist;

Timer t;    // Create Timer
int log_it; // Iterator for Spacing logging of different messages

uint8_t buf[BUFFER_SIZE];

float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
float dt;
float str,thr,wheel_spd;
float wrapTo2pi(float ang);
float wrapTopi(float ang);
bool rc_conn,auto_flag,log_flag;


// ============================================
// Control Loop
// ============================================
// Heading & Speed Control Function
// Sampled at 100 HZ
void controlLoop(){
    dt = t.read()-t_ctrl;     // Sample time (sec)

    if(auto_flag){
    // Vehicle is in Auto Mode
    
        spd = mp.getFreq()*0.0436;  // Read odmeter  (meters/sec)
        
        // Steering Command (+/- 1)
        str = 0.0;  // Set steering trim
        
        // Throttle Command (+/- 1)
        if(t.read()-t_run < 1){      // Set throttle value for 1 secs      
            thr = 0.05;          
        }else{
            thr = 0.0;
        }
        // Use manual steering for speed control testing
        str = ((CH1.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1        
    }else{
    // Maunal RC Mode
        str = ((CH1.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1
        thr = ((CH2.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1            
    }

    t_ctrl = t.read();
}

// ============================================
// Data Log
// ============================================
// Print data to Xbee serial port (115200 Baud)
void dataLog(){
     xbee.printf("%.3f,%d,%.3f,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),dist,spd,thr,str);   
}


void initializeControl(){
    printf("Initialize Control\r\n");
    t_run = t.read();
    dist = 0.0;
    spd_err_i = 0;
}

void stopControl(){
    printf("Stop Control\r\n"); 
    spd_err_i = 0;   
}
void readRCSignal(){
    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)
    
   /* if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
        auto_flag = 0;        
    }*/
}
void readKeyboard(){
    if(xbee.readable()) {
            char c = xbee.getc();
            if(c == 'H' || c == 'h') {
                xbee.printf("Command List...\r\n");
                xbee.printf("d - set open loop duty cycle\r\n");
                xbee.printf("r - run open loop control with current duty cycle\r\n");
                xbee.printf("h - display this prompt and exit\r\n");
                xbee.printf("t - set duration of test\r\n\n\n");
                xbee.printf("Enter Command\r\n");
            }

            if(c == 'R' || c == 'r') {                
                auto_flag = true;
                log_flag = true;
                initializeControl();                
            }
            if(c == 'S' || c == 's') {
                auto_flag = false;
                log_flag = false;
            }            

            if(c == 'L' || c == 'l') {
                log_flag = !log_flag;
            }
    }
}



int main()
{

    log_it = 0;
    auto_mode = 0;
// Default Steering and Throttle commands to neutral
    Steer.write(1500);  // Write Steering Pulse
    Throttle.write(1500);


    t.start();
    t_start = t.read();
    t_log = t.read();
    t_ctrl = t.read();
    t_5hz = t.read();

    while(1) {
        auto_LED = auto_flag;
        dwm_LED = rc_conn;
        readKeyboard(); // Check for Input from Keyboard over xbee

        // check for servo pulse from either channel of receiver module
         readRCSignal();       





        if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
            // 5 Hz loop intended for debugging print statements

           // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
            t_5hz = t.read();
            
        }
        
        if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
            // 100 Hz loop for calling control loop
            controlLoop();
            t_ctrl = t.read();
        }        


        
        if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
                // Faster loop for logging data for plotting and analysis

                    mp.getAccel();
                    mp.getGyro();
                    mp.getMag();
                    mp.getEuler();
                    mp.getSpeed();
                    //xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);

                    if(log_flag){
                    dataLog();
                    }
            t_log = t.read();
        }     
        
        if(rc_conn) {
            mp.setSteer(str);
            mp.setThrottle(thr);
    
        } else {
        Steer.write(1500);  // Write Steering Pulse
        Throttle.write(1500);        
        }           

        wait(1.0/MAIN_LOOP_FREQ);
    } // End While 1 Loop
} // End Main Function

float wrapTo2pi(float ang)
{
    if(ang > 2*PI) {
        ang = ang - 2*PI;
    }

    if(ang < 0) {
        ang = ang + 2*PI;
    }
    return ang;
}

float wrapTopi(float ang)
{
    if(ang > PI) {
        ang = ang - 2*PI;
    }

    if(ang < -PI) {
        ang = ang + 2*PI;
    }
    return ang;
}