File content as of revision 1:f4c9926fb4c9:

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

///////////////////////////////////////////////
// Define Control Gains and Controller Variable
//////////////////////////////////////////////


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;


void controlLoop(){
    dt = t.read()-t_ctrl;

    if(auto_flag){   // If vehicle is in Auto Mode
    
        spd = mp.getFreq()/27.8119; // Convert from counts per second to meters/second

        str = 0.001;
        if(t.read()-t_run < 1){            

            thr = 0.1; // Set throttle, range (-1 to 1)
        }else{
            thr = 0.0;
        }
    }else{
        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();
}

void logData(){
// Accel Data: mp.accel.x,mp.accel.y,mp.accel.z
// Gyro Data: mp.gyro.x,mp.gyro.y,mp.gyro.z
// Mag Data: mp.mag.x,mp.mag.y,mp.mag.z
// Euler Data: mp.euler.roll,mp.euler.pitch, mp.euler.yaw
// Encoder Data: mp.getCounts(), mp.getFreq()


    xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
}


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

void stopControl(){
    printf("Stop Control\r\n");    
}

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("h - Display this prompt and return\r\n");
                xbee.printf("l - Toggle Logging\r\n");
                xbee.printf("r - Run, switch vehicle to auto mode\r\n");
                xbee.printf("s - Stop, stop vehicle autor control\r\n");
                xbee.printf("Enter Command\r\n");
            }

            if(c == 'L' || c == 'l') {                
                log_flag = !log_flag; //toggle logging flag
              
            }
            if(c == 'R' || c == 'r') {                
                auto_flag = true;
                log_flag = true;
                initializeControl();                
            }
            if(c == 'S' || c == 's') {
                auto_flag = false;
                log_flag = false;
                stopControl();
            }                                    

    }
}



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

            t_5hz = t.read();           
        }
        
        if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
            // 100 Hz loop for calling control loop
            mp.getAccel();
            mp.getGyro();
            mp.getMag();  
            mp.getEuler(); 
            mp.getSpeed();                                 
            controlLoop();
            t_ctrl = t.read();
        }        


        
        if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
                // Faster loop for logging data for plotting and analysis
            if(log_flag){
                logData();
            }
            t_log = t.read();
        }     
        
        if(rc_conn) {
          //  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
            mp.setSteer(str);
            mp.setThrottle(thr);
    
        } else {
        Steer.write(1500);  // Write Steering Pulse
        Throttle.write(1500);        
          //  mp.setSteer(str);
          //  mp.setThrottle(thr);
        }           

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