Diff: actuators.cpp

Revision:

32:2006977785f5

Parent:

31:8fbee6c92753

Child:

34:f315b2b38555

--- a/actuators.cpp	Wed Oct 07 15:09:52 2015 +0200
+++ b/actuators.cpp	Wed Oct 07 20:55:28 2015 +0200
@@ -5,61 +5,61 @@
 #include "encoder.h"
 #include "HIDScope.h"
 #include "servo.h"
-// functions for controlling the motors
-bool motorEnable = true;
+    // functions for controlling the motors
+    bool motorEnable = true;
 
-bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
-bool direction2 = false;
+    bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
+    bool direction2 = false;
 
-float motor1Pos = 0;
-float motor2Pos = 0;
+    float motor1Pos = 0;
+    float motor2Pos = 0;
 
-float motorSpeed1 = 0;
-float motorSpeed2 = 0;
+    float motorSpeed1 = 0;
+    float motorSpeed2 = 0;
 
-float motorSetSpeed1 = 0;
-float motorSetSpeed2 = 0;
+    float motorSetSpeed1 = 0;
+    float motorSetSpeed2 = 0;
 
-
-float motorPWM1 = 0;
-float motorPWM2 = 0;
+    float servoPos = 0;
 
-// Set PID values
-float Kp1 = 1; 
-float Ki1 = 0; 
-float Kd1 = 0;
+    float motorPWM1 = 0;
+    float motorPWM2 = 0;
 
-float Kp2 = 1; 
-float Ki2 = 0; 
-float Kd2 = 0;
+    // Set PID values
+    float Kp1 = 1; 
+    float Ki1 = 0; 
+    float Kd1 = 0;
 
-float PIDinterval = 0.2;
+    float Kp2 = 1; 
+    float Ki2 = 0; 
+    float Kd2 = 0;
 
-float prevMotor2Pos = 0;
-float prevMotor1Pos = 0;
-float prevTime = 0;
-float curTime = 0;
+    float PIDinterval = 0.2;
 
-float pidError = 0;
-// Create object instances
-// Initialze motors
-PwmOut motor1(motor1PWMPin);
-PwmOut motor2(motor2PWMPin);
+    float prevMotor2Pos = 0;
+    float prevMotor1Pos = 0;
+    float prevTime = 0;
+    float curTime = 0;
 
-// Initialize encoders
-Encoder encoder1(enc1A, enc1B);
-Encoder encoder2(enc2A, enc2B);
+    // Create object instances
+    // Initialze motors
+    PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+
+    // Initialize encoders
+    Encoder encoder1(enc1A, enc1B);
+    Encoder encoder2(enc2A, enc2B);
 
-// Set direction pins     
-DigitalOut motor1Dir(motor1DirPin);
-DigitalOut motor2Dir(motor2DirPin);  
+    // Set direction pins     
+    DigitalOut motor1Dir(motor1DirPin);
+    DigitalOut motor2Dir(motor2DirPin);  
 
-// create PID instances
-PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
-PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
+    // create PID instances
+    PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
+    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
 
-servo Servo(servoPin);
-Timer t;  
+    servo Servo(servoPin);
+    Timer t;  
     
 void motorInit(){
     
@@ -93,7 +93,6 @@
 
 void motorControl(){
     if(motorEnable){  // only run motors if switch is enabled
-
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -112,7 +111,7 @@
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
-    // compute new PID parameters using setpoint speeds and x/y speeds
+    // compute new PID parameters using setpoint angle speeds and encoder speed
         writeMotors();
 
     }else{
@@ -145,8 +144,8 @@
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
     
-    motor1.write(abs(motorPWM1)/300);
-    motor2.write(abs(motorPWM2)/300);
+    motor1.write(abs(motorPWM1));
+    motor2.write(abs(motorPWM2));
 }
 
 void servoControl(){
@@ -159,3 +158,28 @@
     
 }
 
+void calibrateMotors(){
+    // bool calibrating1 = true;
+    // bool calibrating2 = true;
+    // motor1Dir.write(false);
+    // motor2Dir.write(false);
+    // // move motors CCW until they reach outer most position
+    // while (calibrating1 || calibrating2){
+    //     if(motor1AnglePin == 0){
+    //         motor1.write(0.3f);
+    //     }else{
+    //         motor1.write(0);
+    //         calibrating1 = false;
+    //     }
+    //     if(motor2AnglePin == 0){
+    //         motor2.write(0.3f);
+    //     }else{
+    //         motor2.write(0);
+    //         calibrating2 = false;
+    //     }
+    //     wait(0.2f);
+    // }
+    // // set the encoder values for angle.
+    // encoder1.setValue(0);
+    // encoder2.setValue(0);
+}
\ No newline at end of file