Revision 3:eb933ff5ccaf, committed 2018-04-24

Comitter:

KDrainEE

Date:

Tue Apr 24 17:55:58 2018 +0000

Parent:

0:8bf709d3440b

Commit message:

tuning

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lsc.h	Tue Apr 24 17:55:58 2018 +0000
@@ -0,0 +1,65 @@
+#ifndef LSC_H
+#define LSC_H
+
+#include <cmath>
+//*****Line-Scan Camera******//
+AnalogIn cameraIn(PTB3);
+DigitalOut si(PTC8);
+PwmOut clk(PTC9);
+InterruptIn edgeDetector(PTA5);
+Ticker cameraDaq;
+DigitalOut test(PTE2);
+
+int sensorVal;
+int dataCount;
+float pixArray[128];
+float pixelToInductor[128];
+int camMax;
+bool rejectReading;
+
+void getValue(){
+    test = !test;
+    if(dataCount == 0){
+        si.write(0);
+    }
+    if(dataCount < 128){
+        pixArray[dataCount] = cameraIn.read();
+        dataCount++;
+    }
+    else{
+        //end aquisition cycle
+        edgeDetector.rise(NULL);
+    }
+}    
+
+void beginAcq(){
+    si.write(1);
+    edgeDetector.fall(NULL);
+    edgeDetector.rise(&getValue);  
+}
+
+void acquire()
+{
+    dataCount = 0;
+    int tempMax = 8;
+    edgeDetector.fall(&beginAcq);  
+    for(int i = 9; i < 118;i++){
+        if(pixArray[i] > pixArray[tempMax]){
+            tempMax = i;
+        }       
+    }
+    if(pixArray[tempMax] < 0.2424){   
+        camMax = 64 - tempMax;
+    }
+}
+
+void cameraInit()
+{
+    for(int i = 0; i<128;i++){
+        pixelToInductor[i] = 2E-06*i*i*i - 0.0004*i*i + 0.0047*i + 0.569;
+    }
+    clk.period(5e-5);
+    clk.write(0.5f);
+    cameraDaq.attach(&acquire, 0.01f);
+}
+#endif
\ No newline at end of file

--- a/main.cpp	Wed Apr 18 14:58:02 2018 +0000
+++ b/main.cpp	Tue Apr 24 17:55:58 2018 +0000
@@ -6,11 +6,10 @@
 //#define KD 0.072870569295611f
 //#define KPS 0.03f
 
+#include <iostream>
 #include "mbed.h"
-#include <iostream>
-#include <cmath>
+#include "lsc.h"
 
-using std::pow;
 
 #define TI 0.001f
 
@@ -18,10 +17,10 @@
 #define MINM 0.0f
 #define MAXM 0.53f
 #define KPM 0.15f //0.1414f
-//#define KI 19.7408f
+#define KI 19.7408f
 
-//#define KPS 2.0E-2f //Original 2.0e-2
-//#define KD 1.0e-4f
+#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
@@ -30,22 +29,42 @@
 #define STEER_FREQ 0.02f //50 Hz
 #define STEERUPDATEPERIOD 0.05
 
-AnalogIn _right(PTB1); //Right sensor
-AnalogIn _left(PTB3); //Left sensor
-AnalogIn speed(PTB2);   //tachometer
+/***********************************|Pin Declarations|*************************************************************/
+
+//Feedback
+AnalogIn speed(PTB0);   //tachometer
+AnalogIn _right(PTB1);  //Right sensor
+AnalogIn _left(PTB2);   //Left sensor
 
-PwmOut servoSig(PTA13); //PWM output to control servo
-PwmOut gateDrive(PTA4);
+//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);
+//Checkpoint LEDS
+BusOut checkLED(PTB8,PTB9,PTB10,PTB11);
 
-Serial bt(PTE0, PTE1); //COM12
-DigitalOut myled(LED_BLUE);
-DigitalOut brake(PTD0);
+
+
+/***********************************|Variable Declarations|*************************************************************/
 
 Ticker control;
 Timer ctrlTimer;
+Timer checkpointTimer;
 
-InterruptIn navRt(PTD2);
-InterruptIn navLft(PTD3);
+bool lTrig = false;
+bool rTrig = false;
 
 volatile int rightCount;
 volatile int leftCount;
@@ -55,15 +74,72 @@
 float Setpoint = 0.0;
 float spHolder;
 float errSum = 0.0;
+
+float fbPrev = 0.0f;
+float Kps = 2.0E-3; //0.013 for setpoint = 0.0/0.05
+float Kd = 1.0e-5;
+
 float Kpm = 0.141;
+int checkpoint = 0;
+
+char state;
 
 
-float fbPrev = 0.0f;
-float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
-float Kd = 1.0e-4;
+void waitState()
+{
+    Setpoint = 0; //Makes sure the car isn't moving
+    state = 'w';
+    //bt.putc(state);
+    if (_left.read() == 0.0 &&  _right.read() == 0.0)
+    {
+        green = blue = 1;
+        red = 0;   
+    } else {
+    brake.write(0);    
+    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 stopState()
+{
+    state = 's';
+    //bt.putc('s');
+    spHolder = Setpoint;
+    Setpoint = 0;
+    brake.write(1);
+    waitState();
+}
+void letsGo()
+{
+        checkpoint = 0;
+        if (_left.read() == 0.0 &&  _right.read() == 0.0)
+        {
+            stopState();
+                
+        }
+        state = 'g';
+        //bt.putc(state);
+        red = blue = 1;
+        green = 0;
+        brake.write(0);
+        Setpoint = spHolder;
+        
+        
+}
 inline void applyBrake()
 {  
     spHolder = Setpoint;
@@ -82,6 +158,12 @@
     bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
 }
 
+void displayCheckpoint()
+{
+    bt.printf("Checkpoint reached: %i\r\n", checkpoint);   
+} 
+
+
 void serCb()
 {
     char x = bt.getc();
@@ -144,6 +226,14 @@
     {
         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");
@@ -152,32 +242,54 @@
     if(Setpoint < MINM) Setpoint = MINM;    
 }
 
+void incrimentCheckpoint()
+{
+    if (checkpointTimer > 0.1)
+    {
+        checkpoint++;  
+        checkpointTimer.reset();      
+    }   
+    displayCheckpoint();  
+}    
 
 void incL()
 {      
     leftCount++;
+    lTrig = true;
+    incrimentCheckpoint();
+    
 }
 
 void incR()
 {
     rightCount++;
+    rTrig = true;
+    incrimentCheckpoint();
 }
 
-void controller()
+void steer()
 {
-    //steering
+    
     float L = _left.read();
     float R = _right.read();
-    float velocity = speed.read();
-//    if (L == 0.0 && R == 0.0)
-//    {
-//        //off track
-//        Setpoint = 0.0;
-//    }           
-    float fb = L - R; 
+    if (L == 0.0 && R == 0.0)
+    {
+        stopState();
+    }           
+    float fb = 0.8*(L - R)+0.2*pixelToInductor[camMax]; 
+    //if(checkpoint == 1 || checkpoint == 8){
+//        fb = 0.5*(L - R)+0.5*pixelToInductor[camMax];
+//    }
+//    else{
+//        fb = L - R;
+//    }
     float e = SET - fb;
-    Kps = 0.001/Setpoint;
-    if (Kps > 0.03f) Kps = 0.03f;
+////    Kps = 2.0E-2;
+//    Kps = 0.001/(0.5*speed.read()+0.5*Setpoint);
+//    if(Kps > 0.02)Kps = 0.02;
+////    Kd = 1.0e-4
+//    Kps = 0.00001/(0.5*speed.read()+0.5*Setpoint);
+    //if(Kd > 1.0e-4)Kd = 1.0e-4;
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
     else if (Controlleroutput < MINS) Controlleroutput = MINS;
@@ -187,58 +299,71 @@
         servoSig.write(Controlleroutput);
     }
     fbPrev = fb;
-    data[0] = L;
-    data[1] = R;
+    data[0] = _right.read();
+    data[1] = _left.read();
     data[2] = Controlleroutput;
     data[3] = e;
     
-    //driving
-    float error = Setpoint-velocity;
+}
+void drive()
+{
+    float error = Setpoint-speed.read();
     errSum +=(error * TI);
-
     float Ki = 19.7408;
     if (Setpoint < 0.15){
         Ki = 4.9352;
     } else if(Setpoint >= 0.15 && Setpoint < 0.3){
         Ki = 9.8704;
-    }     
+    }
     float iTerm = Ki*errSum;
     if(iTerm > MAXM) iTerm = MAXM;
     if(iTerm < MINM) iTerm = MINM; 
-    float output = Kpm*error + iTerm;
+    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();    
+    data[5] = speed.read(); 
+}
+
+void cb()
+{
+    steer();
+    if (state == 'g'){
+        drive();
+    }
 }
 
 int main()
 {   
     //startup checks
     bt.baud(115200);
-    bt.attach(&serCb);    
+    bt.attach(&serCb);
     servoSig.period(STEER_FREQ);
     gateDrive.period(.00005f);
     gateDrive.write(Setpoint);
+    checkpointTimer.start();
+   
 
     ctrlTimer.start();
-    control.attach(&controller, TI);
+    control.attach(&cb, TI);
     
     rightCount = 0;
     leftCount = 0; 
-
+    
+  
     navRt.fall(&incR);
     navLft.fall(&incL);
     
+    goButton.fall(&letsGo);
+    stopButton.fall(&stopState);
+    waitState();
     while(1) {
-        myled = !myled;
-        //bt.printf("%f ",data[0]);
-//        bt.printf("%f ", data[1]);
-//        bt.printf("%f ", data[2]);
-//        bt.printf("%f ", data[3]);
-//        bt.printf("%f ", data[4]);
-//        bt.printf("%f\r\n", data[5]);
+        if (checkpoint > 10) checkpoint = 0;
+        checkLED.write(checkpoint);
+
+    
+
     }
 }
\ No newline at end of file