File content as of revision 10:d24a94a677ea:

//Works at speed 2.0, KPS 2.0e-2, KD 1.0e-4

//Has tolerance check, Steering doesn't
//Values from Simulation
//#define KPS 0.0896852742245504f
//#define KD 0.072870569295611f
//#define KPS 0.03f

#include "mbed.h"
#include <iostream>
#include "lsc.h"
#include "states.h"
#include "control.h"



/***********************************|Pin Declarations|*************************************************************/
//Communication
Serial bt(PTE22, PTE23); //Serial Pins (Tx, Rx)

//Checkpoint Interrupts
InterruptIn navRt(PTD2);
InterruptIn navLft(PTD3);


/***********************************|Variable Declarations|*************************************************************/



bool lTrig = false;
bool rTrig = false;

volatile int rightCount;
volatile int leftCount;

float data[6];







void display()
{
    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
}


void serCb()
{
    char x = bt.getc();
    if (x == 'u')
    {
        Setpoint += 0.025;
        display();
    }
    else if(x == 'h')
    {
        Setpoint -= 0.025;
        display();
    }
    else if (x == 'i')
    {
        Kps += 1.0e-3;
        display();
    }
    else if (x == 'j')
    {
        Kps -= 1.0e-3;
        display();
    }
    else if (x == 'o')
    {
        Kd += 1.0e-5;
        display();
    }
    else if (x == 'k')
    {
        Kd -= 1.0e-5;
        display();
    }
    else if (x == 'b')
    {
        applyBrake();
        display();
       
    }
    else if (x == 'n')
    {
        releaseBrake();
        display();
    }
    else if (x == 'p')
    {
        display();
    }
    else if (x == 'y')
    {
        Kpm += 0.003;
        display();   
    }   
    else if(x == 'g')
    {
        Kpm -= 0.003; 
        display();   
    } 
    else if (x == 'z')
    {
        bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
    }
    else if(x== 's')
    {
        stopState();
    }
    else if(x == 'a')
    {
        letsGo();   
    }    
    else
    {
        bt.printf("Invalid input");
    }
    if(Setpoint > MAXM) Setpoint = MAXM;
    if(Setpoint < MINM) Setpoint = MINM;    
}


void incL()
{      
    leftCount++;
    lTrig = true;
}

void incR()
{
    rightCount++;
    rTrig = true;
}



int main()
{   
    //startup checks
    bt.baud(115200);
    bt.attach(&serCb);
    cameraInit();
    cameraDaq.attach(&acquire, 0.01f);    
    servoSig.period(STEER_FREQ);
    gateDrive.period(.00005f);
    gateDrive.write(Setpoint);
   

    ctrlTimer.start();
    control.attach(&cb, TI);
    
    rightCount = 0;
    leftCount = 0; 
    
  
    navRt.fall(&incR);
    navLft.fall(&incL);
    
    goButton.fall(&letsGo);
    stopButton.fall(&stopState);
    waitState();
    while(1) {
        
        if(lTrig){
            bt.putc('l');
            lTrig = false;
        }
        if(rTrig){
            bt.putc('r');
            rTrig = false;
        }
        //bt.printf("Cammax %i\r\n", camMax);

    }
}