Revision 13:2032db00f168, committed 2017-05-13

Comitter:

christine222

Date:

Sat May 13 02:40:49 2017 +0000

Parent:

12:5790e56a056f

Child:

14:9e7bb03ddccb

Commit message:

turning sort of works

Changed in this revision

--- a/main.cpp	Fri May 12 23:25:07 2017 +0000
+++ b/main.cpp	Sat May 13 02:40:49 2017 +0000
@@ -17,46 +17,209 @@
 */
 
 // Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 15
+#define IP_CONSTANT 13
 #define II_CONSTANT 0
-#define ID_CONSTANT 0
+#define ID_CONSTANT 1.
+
+const int desiredCountR = 1300;
+const int desiredCountL = 1400;
+
+void turnLeft(){
+    double speed0 = 0.15;
+    double speed1 = 0.15;
+    double kp = 0.01;
+
+    int counter = 0;
+    
+    int initialCount0 = encoder0.getPulses();
+    int initialCount1 = encoder1.getPulses();
+
+    double desiredCount0 = initialCount0 - desiredCountL;
+    double desiredCount1 = initialCount1 + desiredCountL;
+
+    int count0 = initialCount0;
+    int count1 = initialCount1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1){
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0; 
+            error1 = count1 - desiredCount1; 
+
+            speed0 = error0 * kp + speed0;
+            speed1 = error1 * kp + speed1;
+
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        }else{
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60){
+            break;
+        }
+     }
+
+    right_motor.brake();
+    left_motor.brake();
+}
 
 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 kp = 0.01;
+
+    int counter = 0;
+    
+    int initialCount0 = encoder0.getPulses();
+    int initialCount1 = encoder1.getPulses();
+
+    double desiredCount0 = initialCount0 + desiredCountR;
+    double desiredCount1 = initialCount1 - desiredCountR;
+
+    int count0 = initialCount0;
+    int count1 = initialCount1;
 
-    double desiredCount0 = 1500;
-    double desiredCount1 = -1500;
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1){
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0; // moves forward
+            error1 = count1 - desiredCount1; // moves backwards
+
+            speed0 = error0 * kp + speed0;
+            speed1 = error1 * kp + speed1;
 
-    double error0 = 10.1;
-    double error1 = 10.1;
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        }else{
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
 
-    //right_motor.move(speed0);  
-    //left_motor.move(speed1);
+        if (counter > 60){
+            break;
+        }
+        
+        // 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 turnLeft180(){
+    double speed0 = 0.15;
+    double speed1 = 0.15;
+    double kp = 0.01;
+
+    int counter = 0;
     
     int initialCount0 = encoder0.getPulses();
     int initialCount1 = encoder1.getPulses();
 
-    int count0 = encoder0.getPulses();
-    int count1 = encoder1.getPulses();
+    double desiredCount0 = initialCount0 - 3000;
+    double desiredCount1 = initialCount1 + 2700;
+
+    int count0 = initialCount0;
+    int count1 = initialCount1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1){
+
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0; 
+            error1 = count1 - desiredCount1; 
 
-    while(!(abs(error0) < 10) && !(abs(error1) < 10)){
+            speed0 = error0 * kp + speed0;
+            speed1 = error1 * kp + speed1;
 
-        count0 = encoder0.getPulses() - initialCount0;
-        count1 = encoder1.getPulses() - initialCount1;
+            right_motor.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        }else{
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60){
+            break;
+        }
+     }
+
+    right_motor.brake();
+    left_motor.brake();
+}
 
-        error0 = count0 + desiredCount0; // moves forward
-        error1 = count1 + desiredCount1; // moves backwards
+void turnRight180(){
+    double speed0 = 0.15;
+    double speed1 = 0.15;
+    double kp = 0.01;
+
+    int counter = 0;
+    
+    int initialCount0 = encoder0.getPulses();
+    int initialCount1 = encoder1.getPulses();
 
-        speed0 = error0 * kp + speed0;
-        speed1 = error1 * kp + speed1;
+    double desiredCount0 = initialCount0 + desiredCountR*2;
+    double desiredCount1 = initialCount1 - desiredCountR*2;
+
+    int count0 = initialCount0;
+    int count1 = initialCount1;
+
+    double error0 = count0 - desiredCount0;
+    double error1 = count1 - desiredCount1;
+
+    while(1){
 
-        right_motor.move(speed0);
-        left_motor.move(speed1);
+        if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+            count0 = encoder0.getPulses();
+            count1 = encoder1.getPulses();
+
+            error0 = count0 - desiredCount0; // moves forward
+            error1 = count1 - desiredCount1; // moves backwards
+
+            speed0 = error0 * kp + speed0;
+            speed1 = error1 * kp + 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.move(speed0);
+            left_motor.move(speed1);
+            counter = 0;
+        }else{
+            counter++;
+            right_motor.brake();
+            left_motor.brake();
+        }
+
+        if (counter > 60){
+            break;
+        }
+        
+        // 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();
@@ -121,10 +284,6 @@
     
     left_motor.brake();
     right_motor.brake();
-    while( 1 )
-    {
-        
-    }
 }
 
 void printDipFlag() {
@@ -194,10 +353,11 @@
 //    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
     
     // TODO: Setting all the registers and what not for Quadrature Encoders
-    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
+/*    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
     RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
     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
@@ -227,9 +387,27 @@
 
     while (1) {
         
-        //moveForwardUntilWallIr();
-        turnRight();
+        // moveForwardUntilWallIr();
+        // wait(2);
+        //turnRight180();
+        wait(0.5);
+        turnLeft180();
+        wait(1);
+        //turnRight180();
         wait(1);
+        //turnLeft();
+
+        // wait(1);
+        // turnLeft();
+        // wait(1);
+        // turnRight();
+        // wait(1);
+        // turnRight();
+        // turnRight180();
+        // wait(1);
+        // turnLeft180();
+        // wait(1);
+        break;
         //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) ) ;

--- a/motor.cpp	Fri May 12 23:25:07 2017 +0000
+++ b/motor.cpp	Sat May 13 02:40:49 2017 +0000
@@ -4,8 +4,8 @@
 void Motor::backward(double voltage) {
     if(voltage > maxSpeed){
         voltage = maxSpeed;
-    }else if(voltage < 0){
-        voltage = 0.0;
+    }else if(voltage < minSpeed){
+        voltage = minSpeed;
     }
     forw.write(voltage);
     back.write(0);
@@ -14,8 +14,8 @@
 void Motor::forward(double voltage) {
     if(voltage > maxSpeed){
         voltage = maxSpeed;
-    }else if(voltage < 0){
-        voltage = 0.0;
+    }else if(voltage < minSpeed){
+        voltage = minSpeed;
     }
     forw.write(0);
     back.write(voltage);
@@ -23,7 +23,7 @@
 
 void Motor::move(double voltage) {
     if(voltage < 0){
-        backward(voltage);
+        backward(-voltage);
     }
     if(voltage > 0){
         forward(voltage);

--- a/motor.h	Fri May 12 23:25:07 2017 +0000
+++ b/motor.h	Sat May 13 02:40:49 2017 +0000
@@ -19,7 +19,7 @@
 {
     public:
         Motor( PinName f, PinName b, PinName e ) : 
-            forw( f ), back( b ), enableMotor( e ), maxSpeed( 0.5 )
+            forw( f ), back( b ), enableMotor( e ), maxSpeed( 0.2 ), minSpeed(0.07)
         {
             enableMotor.write( 1 );
         }
@@ -35,6 +35,7 @@
         PwmOut back;
         DigitalOut enableMotor;  
         const double maxSpeed;
+        const double minSpeed;
 };
 
 //QEI leftWheel(