Diff: main.cpp

Revision:

12:5790e56a056f

Parent:

11:8fc2b703086b

Child:

13:2032db00f168

--- a/main.cpp	Sun May 07 01:13:42 2017 +0000
+++ b/main.cpp	Fri May 12 23:25:07 2017 +0000
@@ -21,6 +21,48 @@
 #define II_CONSTANT 0
 #define ID_CONSTANT 0
 
+void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
+    double speed0 = 0.15;
+    double speed1 = 0.15;
+    double kp = 0.00001;
+
+    double desiredCount0 = 1500;
+    double desiredCount1 = -1500;
+
+    double error0 = 10.1;
+    double error1 = 10.1;
+
+    //right_motor.move(speed0);  
+    //left_motor.move(speed1);
+    
+    int initialCount0 = encoder0.getPulses();
+    int initialCount1 = encoder1.getPulses();
+
+    int count0 = encoder0.getPulses();
+    int count1 = encoder1.getPulses();
+
+    while(!(abs(error0) < 10) && !(abs(error1) < 10)){
+
+        count0 = encoder0.getPulses() - initialCount0;
+        count1 = encoder1.getPulses() - initialCount1;
+
+        error0 = count0 + desiredCount0; // moves forward
+        error1 = count1 + desiredCount1; // moves backwards
+
+        speed0 = error0 * kp + speed0;
+        speed1 = error1 * kp + speed1;
+
+        right_motor.move(speed0);
+        left_motor.move(speed1);
+
+        serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
+        serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
+    }
+
+    right_motor.brake();
+    left_motor.brake();
+}
+
 void moveForwardUntilWallIr() {    
     double currentError = 0;
     double previousError = 0;
@@ -157,6 +199,21 @@
     GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
     GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
     
+    // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
+    // of alternate function specified
+    // 4 modes sets AHB1ENR
+    // Now TMR: enable clock with timer, APB1ENR
+    // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
+    //
+    // Encoder mode: disable timer before changing timer to encoder
+    // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
+    // CCMR sets sample rate and set the channel to input
+    // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
+    // SMCR - encoder mode
+    // CR1 reenabales
+    // then read CNT reg of timer
+    
+    
     dipButton1.rise(&enableButton1);
     dipButton2.rise(&enableButton2);
     dipButton3.rise(&enableButton3);
@@ -170,8 +227,9 @@
 
     while (1) {
         
-        moveForwardUntilWallIr();
-        wait(0.1);
+        //moveForwardUntilWallIr();
+        turnRight();
+        wait(1);
         //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
 //        serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
         //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;