Diff: main.cpp

Revision:

10:d24a94a677ea

Parent:

9:1ba8ee3f4ee3

--- a/main.cpp	Sun Apr 22 13:45:48 2018 +0000
+++ b/main.cpp	Sun Apr 22 13:52:41 2018 +0000
@@ -9,31 +9,23 @@
 #include "mbed.h"
 #include <iostream>
 #include "lsc.h"
+#include "states.h"
+#include "control.h"
 
 
 
 /***********************************|Pin Declarations|*************************************************************/
-//Output
-PwmOut servoSig(PTA13); //PWM output to control servo position
-PwmOut gateDrive(PTA4); //PWM output to control motor speed
-DigitalOut brake(PTD0);
 //Communication
 Serial bt(PTE22, PTE23); //Serial Pins (Tx, Rx)
-//LEDs
-DigitalOut blue(LED_BLUE);
-DigitalOut red(LED_RED);
-DigitalOut green(LED_GREEN);
+
 //Checkpoint Interrupts
 InterruptIn navRt(PTD2);
 InterruptIn navLft(PTD3);
-//Button Interrupts
-InterruptIn stopButton(PTD4);
-InterruptIn goButton(PTA12);
+
 
 /***********************************|Variable Declarations|*************************************************************/
 
-Ticker control;
-Timer ctrlTimer;
+
 
 bool lTrig = false;
 bool rTrig = false;
@@ -43,74 +35,11 @@
 
 float data[6];
 
-float Setpoint = 0.0;
-float spHolder;
-float errSum = 0.0;
 
-float fbPrev = 0.0f;
-float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
-float Kd = 1.0e-4;
-
-float Kpm = 0.141;
-
-char state;
-bool go = 0;
-
-void waitState()
-{
-    Setpoint = 0; //Makes sure the car isn't moving
-    state = 'w';
-    //bt.putc(state);
-    green = red = 1; 
-    blue = 0;        //blue LED indicating car is in wait state 
-} 
-void offTrack()
-{
-    Setpoint = 0;
-    brake.write(1);
-    state = 'o';
-    //bt.putc(state);
-    green = blue = 1;
-    red = 0;
-    waitState();
-}
-
-void letsGo()
-{
-        state = 'g';
-        //bt.putc(state);
-        red = blue = 1;
-        green = 0;
-        brake.write(0);
-        go = 1;
-        Setpoint = spHolder;
-        
-        
-}
 
 
-void stopState()
-{
-    state = 's';
-    //bt.putc('s');
-    spHolder = Setpoint;
-    Setpoint = 0;
-    brake.write(1);
-    waitState();
-}
 
-inline void applyBrake()
-{  
-    spHolder = Setpoint;
-    brake.write(1);
-    Setpoint = 0.0;
-}
 
-inline void releaseBrake()
-{
-    brake.write(0);
-    Setpoint = spHolder;
-}
 
 void display()
 {
@@ -209,57 +138,7 @@
     rTrig = true;
 }
 
-void steer()
-{
-    
-    float L = _left.read();
-    float R = _right.read();
-    if (L == 0.0 && R == 0.0)
-    {
-        //off track
-        //offTrack();
-    }           
-//    float fb = L - R; 
-    float fb = ((float)camMax)/64.0;;
-    float e = SET - fb;
-    float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
-    if (Controlleroutput > MAXS) Controlleroutput = MAXS;
-    else if (Controlleroutput < MINS) Controlleroutput = MINS;
-    if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD) 
-    {
-        ctrlTimer.reset();
-        servoSig.write(Controlleroutput);
-    }
-    fbPrev = fb;
-//    data[0] = _right.read();
-//    data[1] = _left.read();
-//    data[2] = Controlleroutput;
-//    data[3] = e;
-    
-}
-void drive()
-{
-    float error = Setpoint-speed.read();
-    errSum +=(error * TI);
-    float iTerm = KI*errSum;
-    if(iTerm > MAXM) iTerm = MAXM;
-    if(iTerm < MINM) iTerm = MINM; 
-    float output = KPM*error + iTerm;
-    if(output > MAXM) output = MAXM;
-    if(output < MINM) output = MINM;        
 
-    gateDrive.write(output);
-//    data[4] = gateDrive.read();
-//    data[5] = speed.read(); 
-}
-
-void cb()
-{
-    steer();
-    if (state == 'g'){
-        drive();
-    }
-}
 
 int main()
 {