Diff: main.cpp

Branch:

onefile

Revision:

23:3f5d30b4784d

Parent:

15:5fa388ba22cb

Child:

24:2d7e11441eee

diff -r c562b9a4176d -r 3f5d30b4784d main.cpp
--- a/main.cpp	Mon Oct 05 21:27:52 2015 +0200
+++ b/main.cpp	Mon Oct 05 23:10:31 2015 +0200
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "config.h"  // settings and pin configurations
-#include "actuators.h"
-#include "buttons.h"
+#include "encoder.h"
+#include "PID.h"
 #include "EMG.h"
 
 //#define DEBUG // send debug data to HIDScope
@@ -10,15 +10,149 @@
     dubugInit();
 #endif
 
+bool motorEnable = false;
+
+bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
+bool direction2 = false;
+
+float motor1Pos = 0;
+float motor2Pos = 0;
+
+float motorSpeed1 = 0;
+float motorSpeed2 = 0;
+
+float motorSetSpeed1 = 0;
+float motorSetSpeed2 = 0;
+
+
+float motorPWM1 = 0;
+float motorPWM2 = 0;
+
+// Set PID values
+float Kp1 = 1; 
+float Ki1 = 1; 
+float Kd1 = 1;
+
+float Kp2 = 1; 
+float Ki2 = 1; 
+float Kd2 = 1;
+
+float PIDinterval = 0.2;
+
 
 
 int main(){
     setPins();
     motorInit(); 
     while (true) {
-        // checkSwitches();
+        checkSwitches();
         // readEMG();
         motorControl();
         // servoControl();
     }
-}
\ No newline at end of file
+}
+
+
+void motorInit(){
+    // Initialze motors
+    PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+
+    // Set motor direction pins.
+    DigitalOut motor1Dir(motor1DirPin);
+    DigitalOut motor2Dir(motor2DirPin);
+
+    // Set initial direction
+    motor1Dir.write(direction1);
+    motor2Dir.write(direction2);
+
+    // Set motor PWM period
+    motor1.period(1/pwm_frequency);
+    motor2.period(1/pwm_frequency);
+
+    // Initialize encoders (with speed calculation)
+    Encoder encoder1(enc1A, enc1B, true);
+    Encoder encoder2(enc2A, enc2B, true);
+
+    initPID();
+}
+
+void initPID(){
+    // create PID instances for motors
+        // PID pidname(input, output, setpoint, kp, ki, kd, direction)
+    PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
+    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+    PIDmotor1.setSetPoint(motorSetSpeed1);
+    PIDmotor2.setSetPoint(motorSetSpeed2);
+
+    PIDmotor1.setProcessValue(motorSpeed1);
+    PIDmotor2.setProcessValue(motorSpeed2);
+    // set PID mode
+    PIDmotor1.setMode(1);
+    PIDmotor2.setMode(1);
+
+    // set limits for PID output to avoid integrator build up.
+    PIDmotor1.setOutputLimits(-1.0, 1.0);
+    PIDmotor2.setOutputLimits(-1.0, 1.0);
+}
+
+
+void motorControl(){
+    if(motorEnable){  // only run motors if switch is enabled
+
+    // get encoder positions
+        motor1Pos = encoder1.getPosition();
+        motor2Pos = encoder2.getPosition();
+
+        // check if motor's are within rotational boundarys
+    // get  encoder speeds
+        motorSpeed1 = encoder1.getSpeed();
+        motorSpeed2 = encoder2.getSpeed();
+
+    // translate to x/y speed
+    // compute new PID parameters using setpoint speeds and x/y speeds
+        motorPWM1 = PIDmotor1.compute();
+        motorPWM2 = PIDmotor2.compute();
+    // translate to motor rotation speed
+    // write new values to motor's
+        if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
+            direction1 = false;
+        }else{
+            direction1 = true; // CW rotation
+        }
+        if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
+            direction2 = false;
+        }else{
+            direction2 = true; // CW rotation
+        }
+        motor1.write(abs(motorPWM1));
+        motor2.write(abs(motorPWM2));
+
+    }else{
+        // write 0 to motors
+        motor1.write(0);
+        motor2.write(0);
+    }
+}
+
+void setPins(){
+    // set input/output pins
+    AnalogIn pot1(pot1Pin);
+}
+
+
+void checkSwitches(){
+    // read motor enable switch
+    
+    // read pump enable switch
+    
+    // read servo potmeter position
+    
+    // read x speed potmeter position
+    float motorSetSpeed1 = pot1.read();
+    
+    // read y speed potmeter position
+    
+    // read killswitches
+    
+    }
\ No newline at end of file