File content as of revision 34:1629bdc97a0e:

#include "robot.h"
#include "turn.h"
#include "nudge.h"
#include "my_functions.h"

/*
 * This function will be called at approximately 20 hz when the control mode is LINE_FOLLOW_MODE
 */
void line_follow_loop(){
    led4 = 1;
    
    // Zach's control code
    //////////////////////
    static double previous_position = 0.0;
    static double integral = 0.0;
       
    double derivative = (line_position - previous_position);
    integral += line_position;
    previous_position = line_position;

    double delta = k_p*line_position + k_i*integral + k_d*derivative;

    leftspeed = speed+delta;
    rightspeed = speed-delta;
    
    if(leftspeed > MAX){
        leftspeed = MAX;
    }
    if(rightspeed > MAX){
        rightspeed = MAX;
    }
    if(leftspeed < MIN){
        leftspeed = MIN;
    }
    if(rightspeed < MIN){
        rightspeed = MIN;
    }
    /////////////////////
    
    
    // Do your control calculations here
    ////////////////////////////////////
    

    // set motor speeds here
    // leftspeed = ???;
    // rightspeed = ???;
    ////////////////////////////////////
    
    
    // Code to check if we need to stop
    if(sensors[0] < 1000 && sensors[1] < 1000 && sensors[2] < 1000 && sensors[3] < 1000 && sensors[4] < 1000){
        //dead end
        leftspeed = 0.0;
        rightspeed = 0.0;
        pi.stop();
        mode = MANUAL_MODE;
    }else if(sensors[0] > 1800 && sensors[4] > 1800){
        //intersection
        leftspeed = 0.0;
        rightspeed = 0.0;
        pi.stop();
        mode = MANUAL_MODE;
    }
    
    led4 = 0;
}


// INPUT COMMANDS
// l:<float>    set left wheel speed (only effective in MANUAL_MODE)
// r:<float>    set right wheel speed (only effective in MANUAL_MODE)
// c:<int>      change mode
// g:<p|i|d|s>:<float>
//              change gains/speed
// b:           check battery
// t:<l|r>      turn
// n:<float>    nudge

// OUTPUT MESSAGES
// p:<float>    line position
// s:<int>,<int>,<int>,<int>,<int>
//              light sensor values
// m:<int>      mode
// a:<message>  acknowledge
void communicate()
{
    led1 = 1;
    int* s = sensors; // just to make the next line more compact
    xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
    xbee.printf("p:%f\n", line_position);
    xbee.printf("m:%d\n", mode);
    // send any other variables here
    ////////////////////////////////
    
    ////////////////////////////////
    led1 = 0;
}

void signal_comm(){
    comm_time = true;
}

int parse_command(const char* cmd)
{
    if(cmd[1]==':'){
        // left motor
        if(cmd[0]=='l'){
            if(mode==MANUAL_MODE){
                leftspeed = atof(&cmd[2]);
            }
        // right motor
        }else if(cmd[0]=='r'){
            if(mode==MANUAL_MODE){
                rightspeed = atof(&cmd[2]);
            }
        // mode
        }else if(cmd[0]=='c'){
            mode = atoi(&cmd[2]);
            xbee.printf("a:c:%d\n", mode);// acknowledge the mode change
            // xbee.printf("mode set to %d\n", mode);
        // gains
        }else if(cmd[0]=='g'){
            if(cmd[2]=='p'){
                k_p = atof(&cmd[4]);
                xbee.printf("k_p: %f\n", k_p);
            }else if(cmd[2]=='i'){
                k_i = atof(&cmd[4]);
                xbee.printf("k_i: %f\n", k_i);
            }else if(cmd[2]=='d'){
                k_d = atof(&cmd[4]);
                xbee.printf("k_d: %f\n", k_d);
            }else if(cmd[2]=='s'){
                speed = atof(&cmd[4]);
                xbee.printf("speed: %f\n", speed);
            }
            // xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
        // battery
        }else if(cmd[0]=='b'){
            xbee.printf("battery voltage: %f\n", pi.battery());
        // turn
        }else if(cmd[0]=='t'){
            float dir = 0.0;
            if(cmd[2] == 'l'){
                dir = 1.0;
            }else{
                dir = -1.0;
            }
            start_turn(dir);
            xbee.printf("a:c:%d\n", TURN_MODE);
        }else if(cmd[0]=='n'){
            start_nudge(atof(&cmd[2]));
            xbee.printf("a:c:%d\n", NUDGE_MODE);   
        
        // parse your own commands here
        ///////////////////////////////

        ///////////////////////////////
        }else{
            //XXX disabling this is disabling error checking
            xbee.printf("%s\n",cmd);
        }
    }else{
        //XXX disabling this is disabling error checking
        xbee.printf("%s\n",cmd);
    }
    return 0;
}

void check_incoming(){
    char read;

    while(xbee.readable()){
        led2 = 1;
        read = xbee.getc();
        if(read=='\n'){
            received[r_index]='\0'; // put a null character at the end
            parse_command(received);
            r_index=0;
        } else {
            if(r_index >= 1024){
                r_index=0;
            }
            received[r_index]=read;
            r_index++;
        }
    }
    led2=0;
}

void control()
{
    pi.sensor_reading(sensors);
    line_position = pi.line_position();
    if(mode==LINE_FOLLOW_MODE){
        line_follow_loop();
    }
    pi.left_motor(rightspeed);
    pi.right_motor(leftspeed);
}

int main() {
    
    // xbee.attach(&Rx_interrupt, Serial::RxIrq);
    xbeeReset = 0;
    wait(2);
    xbeeReset = 1;
    pi.sensor_auto_calibrate();
    leftspeed = 0.0;
    rightspeed = 0.0;
   
    r_index = 0;
    received[0] = '\0';
    mode = MANUAL_MODE;
    
    communication.attach(&signal_comm, 0.5); // lowered to 2 Hz because of communications problems
    controls.attach(&control, 0.02);

    // main loop
    while(1){
        led3 = mode;

        check_incoming();

        if(comm_time){
            communicate();
            comm_time = false;
        }

    }

    return 0;
}