Revision 12:5790e56a056f, committed 2017-05-12

Comitter:

kyleliangus

Date:

Fri May 12 23:25:07 2017 +0000

Parent:

11:8fc2b703086b

Child:

13:2032db00f168

Commit message:

Right Turn is wrong, need fix

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPIO.h	Fri May 12 23:25:07 2017 +0000
@@ -0,0 +1,23 @@
+#include "stm32f4xx.h" 
+
+    // PA_1 is A of right
+    // PA_0 is B of right
+    // PA_5 is A of left
+    // PB_3 is B of left
+
+#define ALT_FUNC_MODE 0x03
+//#define 
+ 
+// 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
\ No newline at end of file

--- 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) ) ;

--- a/motor.cpp	Sun May 07 01:13:42 2017 +0000
+++ b/motor.cpp	Fri May 12 23:25:07 2017 +0000
@@ -2,15 +2,34 @@
 #include "motor.h"
 
 void Motor::backward(double voltage) {
+    if(voltage > maxSpeed){
+        voltage = maxSpeed;
+    }else if(voltage < 0){
+        voltage = 0.0;
+    }
     forw.write(voltage);
     back.write(0);
 }
 
 void Motor::forward(double voltage) {
+    if(voltage > maxSpeed){
+        voltage = maxSpeed;
+    }else if(voltage < 0){
+        voltage = 0.0;
+    }
     forw.write(0);
     back.write(voltage);
 }
 
+void Motor::move(double voltage) {
+    if(voltage < 0){
+        backward(voltage);
+    }
+    if(voltage > 0){
+        forward(voltage);
+    }
+}
+
 
 void Motor::brake() {
     forw.write(BRAKE_VOLTAGE);

--- a/motor.h	Sun May 07 01:13:42 2017 +0000
+++ b/motor.h	Fri May 12 23:25:07 2017 +0000
@@ -19,7 +19,7 @@
 {
     public:
         Motor( PinName f, PinName b, PinName e ) : 
-            forw( f ), back( b ), enableMotor( e )
+            forw( f ), back( b ), enableMotor( e ), maxSpeed( 0.5 )
         {
             enableMotor.write( 1 );
         }
@@ -28,11 +28,13 @@
         void forward( double voltage );
         void brake();
         void coast();
+        void move( double voltage );
         
     private:
         PwmOut forw;
         PwmOut back;
         DigitalOut enableMotor;  
+        const double maxSpeed;
 };
 
 //QEI leftWheel(