Diff: actuators.cpp

Revision:

122:1a5df0765790

Parent:

121:6d8f1bdcda05

Child:

123:815ace09b753

--- a/actuators.cpp	Wed Oct 28 13:38:36 2015 +0100
+++ b/actuators.cpp	Thu Oct 29 12:26:50 2015 +0000
@@ -56,17 +56,6 @@
 double servoSpeed = 0;
 const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg
 
-// Set calibration values
-double motor1CalSpeed = -5; // deg/sec
-double motor2CalSpeed = 5;
-bool springHit = false;
-float lastCall = 0;
-bool calibrating1 = true;
-bool calibrating2 = false;
-double looseTime = 0;
-
-//bool calReady = false;
-
 // Create object instances
 // Safety Pin
 DigitalIn safetyIn(safetyPin);
@@ -151,13 +140,16 @@
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
     // exclude kinematics when still calibrating
-    if (calReady){
-      kinematics();
-    }
+//    if (calReady){
+//      kinematics();
+//    }
 
     if(motorsEnable){  // only run motors if switch is enabled
     // compute new PID parameters using setpoint angle speeds and encoder speed
-        writeMotors();
+        motor1Dir.write(true);
+        motor2Dir.write(true);
+        motor1.write(0);
+        motor2.write(0.05f);
         servoControl();
     }else{
         // write 0 to motors
@@ -196,7 +188,16 @@
 }
 
 const double motor1StartPos = (-10*32*131.25)/360; // angle to encoder counts
-const double motor2StartPos = (114*32*131.25)/360; 
+const double motor2StartPos = (120*32*131.25)/360; 
+
+// Set calibration values
+double motor1CalSpeed = -10; // deg/sec
+double motor2CalSpeed = 5;
+bool springHit = false;
+float lastCall = 0;
+bool calibrating1 = true;
+bool calibrating2 = false;
+double looseTime = 0;
 
 bool calibrateMotors(){
    safetyOn = false; // safety springs off
@@ -269,7 +270,7 @@
 const double Ymax = 0.645;
 const double Ymin = 0.33;
 double Xpos = 0; // set values
-double Ypos =0;
+double Ypos = 0;
 
 
 
@@ -291,12 +292,12 @@
         motor2SetSpeed = 0;
         return false;
     }
-motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
-    setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180));
-motor2SetSpeed = -(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \
+motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \
+    setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)));
+motor2SetSpeed = (180/PI)*(-(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \
     setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \
     setXSpeed*L1*cos(motor1Pos*PI/180) + \
-    setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180));
+    setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180)));
     return true;
 
 }
\ No newline at end of file