Diff: TFC_run_track_oneline.cpp

Revision:

0:aebd709fc6bb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TFC_run_track_oneline.cpp	Fri Jan 30 18:16:40 2015 +0000
@@ -0,0 +1,129 @@
+#include "mbed.h"
+#include "TFC.h"
+
+//Serial pc(USBTX, USBRX);
+
+float servoTrim();
+float motorTrim();
+float avgPos();
+int readCamCenter();
+
+int cam[122],
+    peaks[2];
+
+int filter[]={-1,-1,-1,6,-1,-1,-1}; //size must not change
+
+float servBuf[100] = {0},
+      weight[100];
+
+int main(){
+    float pos = 0,
+          speed = 0;
+          
+    int prevState1 = 0,
+        prevState2 = 0;
+        
+        
+    weight[0] = 1;
+    for(int i = 1; i < 100; i++){
+        weight[i] = weight[i-1] * 0.95;
+    }
+
+    //pc.baud(115200);
+    TFC_Init();
+    
+    while(TFC_ReadPushButton(0) == 0){
+    }
+    
+    wait(3);
+    
+    TFC_HBRIDGE_ENABLE;
+    
+    while(1){        
+        if(TFC_ReadPushButton(0) != 0 && prevState1 != TFC_ReadPushButton(0)){
+            speed -= 0.05;
+        }
+        else if(TFC_ReadPushButton(1) != 0 && prevState2 != TFC_ReadPushButton(1)){
+            speed += 0.05;
+        }
+        
+        prevState1 = TFC_ReadPushButton(0);
+        prevState2 = TFC_ReadPushButton(1);
+        
+        TFC_SetMotorPWM(speed, speed);
+            
+        pos = float(readCamCenter() - 61) / 61;
+    
+        pos *= (TFC_ReadPot(1) + 1) / 2;
+        
+        pos += TFC_ReadPot(0);
+        
+        if((TFC_ServoTicker % 5) == 0){
+            for(int i = 99; i > 0; i--){
+                servBuf[i] = servBuf[i-1];
+            }
+            servBuf[0] = pos;
+        }
+    
+        TFC_SetServo(0, avgPos());
+    }
+}
+
+float servoTrim(){
+    return TFC_ReadPot(0);
+}
+
+float motorTrim(){
+    return TFC_ReadPot(1);
+}
+
+int readCamCenter(){
+    int temp;
+    int peaksval[] ={0, 0};
+    
+    if(TFC_LineScanImageReady != 0){
+        
+        TFC_LineScanImageReady = 0;
+        for(int i = 0; i < 122; i++){
+            temp = 0;
+            for(int h = 0; h < 7; h++){
+                temp = temp + (TFC_LineScanImage0[i+h] * filter[h]);
+            }
+            cam[i] = temp;
+        }
+    }
+
+    temp = 0;
+    
+    for(int i = 0; i < 2; i++){
+        peaksval[i] = 0;
+    }
+    
+    for(int i = 0; i < 122; i++){
+        if(cam[i] > peaksval[0]){
+            peaksval[0] = cam[i];
+            peaks[0] = i;
+        }
+    }
+    
+    for(int i = 0; i < 122; i++){
+        if(cam[i] > peaksval[1] && peaks[0] != i){
+            peaksval[1] = cam[i];
+            peaks[1] = i;
+        }
+    }
+
+    temp = ((peaks[0]+1)+(peaks[1]+1))/2;
+    
+    return  temp;
+}
+
+float avgPos(){
+    float temp = 0;
+    
+    for(int i = 0; i < 100; i++){
+        temp += servBuf[i] * weight[i];
+    }
+    
+    return (temp / 19.8816);
+}
\ No newline at end of file