Diff: main.cpp

Revision:

13:76c9915db820

Parent:

11:bbddc908c78c

Child:

14:c638d4b9ee94

--- a/main.cpp	Fri Apr 05 17:47:06 2013 +0000
+++ b/main.cpp	Sat Apr 06 14:41:35 2013 +0000
@@ -65,8 +65,7 @@
 #include "Sensors/Encoders/Encoder.h"
 #include "Sensors/Colour/Colour.h"
 #include "Sensors/CakeSensor/CakeSensor.h"
-
-
+#include <algorithm>
 
 void motortest();
 void encodertest();
@@ -80,6 +79,7 @@
 void ledphototransistortest();
 void colourtest();
 void cakesensortest();
+void feedbacktest();
 
 int main() {
     
@@ -95,9 +95,29 @@
     //motortestline();
     //ledtest();
     //phototransistortest();
-    ledphototransistortest();
+    //ledphototransistortest();
     //colourtest(); // Red SnR too low
-    cakesensortest();
+    //cakesensortest();
+    //feedbacktest();
+}
+
+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(){
@@ -248,8 +268,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;