Diff: main.cpp

Revision:

31:cc08254ab7b5

Parent:

30:65f0c9ecf810

Child:

32:1bb406d2b3c3

--- a/main.cpp	Mon Oct 16 09:42:04 2017 +0000
+++ b/main.cpp	Tue Oct 17 09:52:12 2017 +0000
@@ -5,6 +5,7 @@
 #include "FastPWM.h"
 #include "refGen.h"
 #include "controller.h"
+#include "motorConfig.h"
 
 // Defining relevant constant parameters
 const float gearRatio = 131;
@@ -14,23 +15,10 @@
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
-enum robotStates {KILLED, ACTIVE};
-robotStates currentState = KILLED;
-
 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(5);
 
-// Defining outputs
-
-// Leds can be used to indicate status
-DigitalOut ledG(LED_GREEN);
-DigitalOut ledR(LED_RED);
-DigitalOut ledB(LED_BLUE);
-
-DigitalOut motor1_direction(D4);
-PwmOut motor1_pwm(D5);
-
 // Defining inputs
 InterruptIn sw2(SW2);
 InterruptIn sw3(SW3);
@@ -107,51 +95,11 @@
     e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
     }
 
-// motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
+// Generate a PID controller with the specified values of k_p, k_d and k_i
 controller motorController1(k_p, k_d, k_i);
     
 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
-void setMotor1(float motorValue){
-    switch(currentState){
-                case KILLED:
-                    motor1_pwm.write(0.0);
-                    // Set motor direction
-                    if (motorValue >=0){
-                        // corresponds to CW rotation of motor axle
-                        motor1_direction = 0;
-                        } else if(motorValue < 0){
-                        // corresponds to CCW rotation of motor axle
-                        motor1_direction = 1;
-                        pc.printf("Bah!");
-                        }
-                    ledR = 0;
-                    ledG = 1;
-                    ledB = 1;
-                    break;
-                case ACTIVE:
-                    pc.printf("Got into ACTIVE \r\n");
-                    // Set motor direction
-                    if (motorValue >=0){
-                        // corresponds to CW rotation of motor axle
-                        motor1_direction.write(0);
-                        } else if(motorValue < 0){
-                        // corresponds to CCW rotation of motor axle
-                        motor1_direction.write(1);
-                        }
-                        
-                    // Set motor speed
-                    if (fabs(motorValue)>1){ 
-                        motor1_pwm = 1;
-                        }
-                    else {
-                        motor1_pwm.write(fabs(motorValue) + 0.4);
-                        }
-                    ledR = 1;
-                    ledG = 1;
-                    ledB = 0;      
-                    break;
-            }
-    }
+motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
     
 void measureAndControl(){
     getError(e_pos, e_int, e_der);
@@ -161,18 +109,8 @@
     pc.printf("Motorvalue is %.2f \r\n", motorValue);
     pc.printf("Error is %.2f \r\n", e_pos);
     pc.printf("Reference is %.2f \r\n", r);
-    pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
-    setMotor1(motorValue);
-    }
-
-void killSwitch(){
-    pc.printf("Motors turned off");
-    currentState = KILLED;
-    }
-    
-void turnMotorsOn(){
-    pc.printf("Motors turned on");
-    currentState = ACTIVE; 
+    //pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
+    motor1.setMotor(motorValue);
     }
 
 void rSwitchDirection(){
@@ -184,10 +122,8 @@
 int main()
     {
     pc.printf("Main function");
-    motor1_direction = 0; // False = clockwise rotation
-    motor1_pwm.period(pwmPeriod);//T=1/f 
-    sw2.fall(&killSwitch);
-    sw3.fall(&turnMotorsOn);
+    sw2.fall(&motor1,&motorConfig::killSwitch);
+    sw3.fall(&motor1, &motorConfig::turnMotorOn);
     button2.rise(&rSwitchDirection);
     pc.baud(115200);
     controllerTicker.attach(measureAndControl, Ts);