Diff: actuators.cpp

Revision:

31:8fbee6c92753

Parent:

30:a20f16bf8dda

Child:

32:2006977785f5

--- a/actuators.cpp	Tue Oct 06 21:06:20 2015 +0000
+++ b/actuators.cpp	Wed Oct 07 15:09:52 2015 +0200
@@ -4,6 +4,7 @@
 #include "config.h"
 #include "encoder.h"
 #include "HIDScope.h"
+#include "servo.h"
 // functions for controlling the motors
 bool motorEnable = true;
 
@@ -56,6 +57,8 @@
 // create PID instances
 PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
 PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+
+servo Servo(servoPin);
 Timer t;  
     
 void motorInit(){
@@ -74,10 +77,10 @@
     PIDmotor2.setProcessValue(motorSpeed2);
 
     // set limits for PID output to avoid integrator build up.
-    PIDmotor1.setInputLimits(-100, 300);
-    PIDmotor2.setInputLimits(-100,300);
-    PIDmotor1.setOutputLimits(-100, 300);
-    PIDmotor2.setOutputLimits(-100, 300);
+    PIDmotor1.setInputLimits(-300, 300);
+    PIDmotor2.setInputLimits(-300, 300);
+    PIDmotor1.setOutputLimits(-1, 1);
+    PIDmotor2.setOutputLimits(-1, 1);
     
     // Turn PID on
     PIDmotor1.setMode(1);
@@ -107,36 +110,10 @@
         prevMotor1Pos = motor1Pos;
         prevMotor2Pos = motor2Pos;
         
-        
-    // translate to x/y speed
-    // compute new PID parameters using setpoint speeds and x/y speeds
-        PIDmotor1.setSetPoint(motorSetSpeed1);
-        PIDmotor2.setSetPoint(motorSetSpeed2);
+    // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
-        PIDmotor1.setProcessValue(motorSpeed1);
-        PIDmotor2.setProcessValue(motorSpeed2);
-    
-        motorPWM1 = PIDmotor1.compute();
-        motorPWM2 = PIDmotor2.compute();
-    // translate to motor rotation speed
-    // write new values to motor's
-        if (motorPWM1 > 0 ){ // CCW rotation 
-            direction1 = false;
-        }else{
-            direction1 = true; // CW rotation
-        }
-        if (motorPWM2 > 0 ){ // CCW rotation 
-            direction2 = false;
-        }else{
-            direction2 = true; // CW rotation
-        }
-        motor1Dir.write(direction1);
-        motor2Dir.write(direction2);
-        
-        motor1.write(abs(motorPWM1)/300);
-        motor2.write(abs(motorPWM2)/300);
-//        motor2.write(motorSetSpeed2);
-        pidError = PIDmotor2.getError();
+    // compute new PID parameters using setpoint speeds and x/y speeds
+        writeMotors();
 
     }else{
         // write 0 to motors
@@ -145,8 +122,36 @@
     }
 }
 
+void writeMotors(){
+    PIDmotor1.setSetPoint(motorSetSpeed1);
+    PIDmotor2.setSetPoint(motorSetSpeed2);
+
+    PIDmotor1.setProcessValue(motorSpeed1);
+    PIDmotor2.setProcessValue(motorSpeed2);
+
+    motorPWM1 = PIDmotor1.compute();
+    motorPWM2 = PIDmotor2.compute();
+// write new values to motor's
+    if (motorPWM1 > 0 ){ // CCW rotation 
+        direction1 = false;
+    }else{
+        direction1 = true; // CW rotation
+    }
+    if (motorPWM2 > 0 ){ // CCW rotation 
+        direction2 = false;
+    }else{
+        direction2 = true; // CW rotation
+    }
+    motor1Dir.write(direction1);
+    motor2Dir.write(direction2);
+    
+    motor1.write(abs(motorPWM1)/300);
+    motor2.write(abs(motorPWM2)/300);
+}
+
 void servoControl(){
     // use potMeter Value to set servo angle
+    Servo.write(servoPos);
     // (optionaly calculate xy position to keep balloon in position)
         // calculate z position using angle
         // calculate x y translation of endpoint