Diff: main.cpp

Revision:

11:c47a34f047d5

Parent:

6:1b4a677c468c

Child:

12:3b83eb9a98bc

--- a/main.cpp	Sat Apr 21 22:43:32 2018 +0000
+++ b/main.cpp	Sun Apr 22 18:05:58 2018 +0000
@@ -1,148 +1,84 @@
-//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 "globals.h"
 #include "lsc.h"
-
-#define TI 0.001f
-
-#define SCALAR 0.53f
-#define MINM 0.0f
-#define MAXM 0.53f
-#define KPM 0.15f //0.1414f
-#define KI 19.7408f
-
-#define KPS 2.0E-2f //Original 2.0e-2
-#define KD 1.0e-4f
-#define SET 0.0f
-#define MINS 0.05f
-#define MAXS 0.1f
-#define BIAS 0.075f
-#define TOL 0.02f
-#define STEER_FREQ 0.02f //50 Hz
-#define STEERUPDATEPERIOD 0.05
+#include "stateMachine.h"
+#include "control.h"
 
 /***********************************|Pin Declarations|*************************************************************/
-
-//Feedback
-AnalogIn speed(PTB0);   //tachometer
-AnalogIn _right(PTB1);  //Right sensor
-AnalogIn _left(PTB2);   //Left sensor
-//AnalogIn camIn(PTB3);   //Camera Input
-//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);
+//DigitalOut red(LED_RED);
+//DigitalOut green(LED_GREEN);
 //Checkpoint Interrupts
 InterruptIn navRt(PTD2);
 InterruptIn navLft(PTD3);
-//Button Interrupts
-InterruptIn stopButton(PTD4);
-InterruptIn goButton(PTA12);
-//Camera timers
-//DigitalOut si(PTC8);
-//PwmOut clk(PTC9);
-//InterruptIn edgeDetector(PTD5);
+
 
 /***********************************|Variable Declarations|*************************************************************/
 
-Ticker control;
-Timer ctrlTimer;
+
 
 bool lTrig = false;
 bool rTrig = false;
 
 volatile int rightCount;
 volatile int leftCount;
-
-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 waitState()
+//{
+//    Setpoint = 0; //Makes sure the car isn't moving
+//    state = 'w';
+//    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';
+//    spHolder = Setpoint;
+//    Setpoint = 0;
+//    brake.write(1);
+//    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()
-{
-    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
-}
-
+//void applyBrake()
+//{  
+//    spHolder = Setpoint;
+//    brake.write(1);
+//    Setpoint = 0.0;
+//}
+//
+//void releaseBrake()
+//{
+//    brake.write(0);
+//    Setpoint = spHolder;
+//}
+bool paramChanged = false;
 
 void serCb()
 {
@@ -150,69 +86,12 @@
     if (x == 'u')
     {
         Setpoint += 0.025;
-        display();
+        paramChanged = true;
     }
     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();   
+        paramChanged = true;
     }    
     else
     {
@@ -222,7 +101,6 @@
     if(Setpoint < MINM) Setpoint = MINM;    
 }
 
-
 void incL()
 {      
     leftCount++;
@@ -235,94 +113,20 @@
     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()
 {   
-    //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);
+    cameraInit();       
+    controlInit();
     
     rightCount = 0;
     leftCount = 0; 
     
-  
     navRt.fall(&incR);
     navLft.fall(&incL);
-    
-    goButton.fall(&letsGo);
-    stopButton.fall(&stopState);
-    waitState();
+    _state = WAIT;   
     while(1) {
-        
-        if(lTrig){
-            bt.putc('l');
-            lTrig = false;
-        }
-        if(rTrig){
-            bt.putc('r');
-            rTrig = false;
-        }
-        //bt.printf("Cammax %i\r\n", camMax);
-
+        bt.printf("%f %f\r\n", data[0], data[1]);
     }
 }
\ No newline at end of file