Revision 109:026abd708dce, committed 2015-10-26

Comitter:

annesteenbeek

Date:

Mon Oct 26 12:45:41 2015 +0000

Parent:

108:b7beb67a1313

Child:

110:a6439e13be8b

Commit message:

tabbed back in main, commented kinematics;

Changed in this revision

--- a/actuators.cpp	Mon Oct 26 12:41:37 2015 +0000
+++ b/actuators.cpp	Mon Oct 26 12:45:41 2015 +0000
@@ -7,84 +7,84 @@
 #include "buttons.h"
 #include "Servo.h"
 
-    // Motor control constants
-    #define pwm_frequency 50000 // still High, could be lowered
-    #define PI 3.14159265
+// Motor control constants
+#define pwm_frequency 50000 // still High, could be lowered
+#define PI 3.14159265
 
-    // functions for controlling the motors
-    bool motorsEnable = false;
-    bool safetyOn = true; 
+// functions for controlling the motors
+bool motorsEnable = false;
+bool safetyOn = true; 
 
 
-    double encoder1Counts = 0;
-    double encoder2Counts = 0;
+double encoder1Counts = 0;
+double encoder2Counts = 0;
 
-    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;
 
-    double motor1Pos = 0;
-    double motor2Pos = 0;
+double motor1Pos = 0;
+double motor2Pos = 0;
 
-    double motor1Speed = 0;
-    double motor2Speed = 0;
+double motor1Speed = 0;
+double motor2Speed = 0;
 
-    double motor1SetSpeed = 0;
-    double motor2SetSpeed = 0;
+double motor1SetSpeed = 0;
+double motor2SetSpeed = 0;
 
-    double motor1PWM = 0;
-    double motor2PWM = 0;
+double motor1PWM = 0;
+double motor2PWM = 0;
 
-    // Set PID values
-    double Kp1 = 1; 
-    double Ki1 = 0; 
-    double Kd1 = 0;
+// Set PID values
+double Kp1 = 1; 
+double Ki1 = 0; 
+double Kd1 = 0;
 
-    double Kp2 = 0.008; 
-    double Ki2 = 0.08; 
-    double Kd2 = 0;
+double Kp2 = 0.008; 
+double Ki2 = 0.08; 
+double Kd2 = 0;
 
-    double motor1PrevCounts = 0;
-    double motor2PrevCounts = 0;
-    double prevTime = 0;
-    double now = 0;
-    double timechange;
+double motor1PrevCounts = 0;
+double motor2PrevCounts = 0;
+double prevTime = 0;
+double now = 0;
+double timechange;
+
+// Set servo values
+const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg
 
-    // Set servo values
-    const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg
-    
-    // Set calibration values
-    double motorCalSpeed = 10; // deg/sec
-    double returnSpeed = -10;
-    bool springHit = false;
-    float lastCall = 0;
-    bool calibrating1 = true;
-    bool calibrating2 = false;
+// Set calibration values
+double motorCalSpeed = 10; // deg/sec
+double returnSpeed = -10;
+bool springHit = false;
+float lastCall = 0;
+bool calibrating1 = true;
+bool calibrating2 = false;
 
-    // Create object instances
-    // Safety Pin
-    DigitalIn safetyIn(safetyPin);
-    
-    // Initialze motors
-    PwmOut motor1(motor1PWMPin);
-    PwmOut motor2(motor2PWMPin);
+// Create object instances
+// Safety Pin
+DigitalIn safetyIn(safetyPin);
 
-    // initialize Servo
-    Servo servo(servoPin);
+// Initialze motors
+PwmOut motor1(motor1PWMPin);
+PwmOut motor2(motor2PWMPin);
+
+// initialize Servo
+Servo servo(servoPin);
 
 
-    // Initialize encoders
-    Encoder encoder1(enc1A, enc1B);
-    Encoder encoder2(enc2A, enc2B);
+// 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 motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
-    PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
+// create PID instances
+PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
+PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
 
-    Timer t;  
+Timer t;  
     
 void motorInit(){
     
@@ -243,7 +243,7 @@
 }
 
 
-bool kinematics(){
+//bool kinematics(){
 //     // calculate current x and Y
 //     X = L2*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
 //     Y = L2*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
@@ -269,4 +269,4 @@
 //     setXSpeed*L1*cos(motor1Pos*PI/180) + \
 //     setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180));
 
-}
\ No newline at end of file
+//}
\ No newline at end of file