Diff: main.cpp

Revision:

14:c638d4b9ee94

Parent:

13:d4b5851742a3

Parent:

12:76c9915db820

Child:

15:9c5aaeda36dc

--- a/main.cpp	Fri Apr 05 21:49:23 2013 +0000
+++ b/main.cpp	Sat Apr 06 15:43:47 2013 +0000
@@ -67,8 +67,7 @@
 #include "Sensors/Colour/Colour.h"
 #include "Sensors/CakeSensor/CakeSensor.h"
 #include "Processes/Printing/Printing.h"
-
-
+#include <algorithm>
 
 void motortest();
 void encodertest();
@@ -84,6 +83,7 @@
 void cakesensortest();
 void printingtestthread(void const*);
 void printingtestthread2(void const*);
+void feedbacktest();
 
 int main() {
     
@@ -102,6 +102,7 @@
     //ledphototransistortest();
     //colourtest(); // Red SnR too low
     //cakesensortest();
+    //feedbacktest();
     DigitalOut l1(LED1);
     Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -125,6 +126,7 @@
         Thread::wait(200);
     }
 }
+
 void printingtestthread2(void const*){
     const char ID = 2;
     float buffer[5] = {ID};
@@ -138,6 +140,26 @@
     }
 }
 
+
+void feedbacktest(){
+    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
+    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+    
+    float Pgain = -0.002;
+    float fwdspeed = -200/3.0f;
+    Timer timer;
+    timer.start();
+    
+    while(true){
+        float expecdist = fwdspeed * timer.read();
+        float errleft = Eleft.getPoint() - (expecdist*1.07f);
+        float errright = Eright.getPoint() - expecdist;
+        
+        mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
+        mright(max(min(errright*Pgain, 0.4f), -0.4f));
+    }
+}
+
 void cakesensortest(){
     wait(1);
     pc.printf("cakesensortest");
@@ -286,8 +308,8 @@
 }
 
 void motorencodetestline(){
-    Encoder Eleft(p27, p28), Eright(p30, p29);
-    MainMotor mleft(p24,p23), mright(p21,p22);
+    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
+    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.2;
     const float dspeed = 0.1;