Diff: actuators.cpp

Revision:

124:f67ce69557db

Parent:

121:6d8f1bdcda05

Child:

127:831f03471efb

--- a/actuators.cpp	Wed Oct 28 13:38:36 2015 +0100
+++ b/actuators.cpp	Thu Oct 29 18:23:08 2015 +0100
@@ -38,8 +38,8 @@
 double motor2PWM = 0;
 
 // Set PID values
-double Kp1 = 1; 
-double Ki1 = 0; 
+double Kp1 = 0.008; 
+double Ki1 = 0.08; 
 double Kd1 = 0;
 
 double Kp2 = 0.008; 
@@ -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);
@@ -122,10 +111,18 @@
 
 void motorControl(){
         // EMG signals to motor speeds
-    //  const double scaleVel = 20;
-    //  motor1SetSpeed = x_velocity*scaleVel;
-    //  motor2SetSpeed = y_velocity*scaleVel;
-    //  servoSpeed = z_velocity*scaleVel;
+  if(!usePotmeters && controlAngle){
+      double scaleVel = 20;
+      motor1SetSpeed = x_velocity*scaleVel;
+      motor2SetSpeed = y_velocity*scaleVel;
+      servoSpeed = z_velocity*scaleVel;
+  }
+  if(!usePotmeters && controlDirection){
+      double scaleVel = 0.1;
+      setXSpeed = x_velocity*scaleVel;
+      setYSpeed = y_velocity*scaleVel;
+      servoSpeed = z_velocity*scaleVel;
+    }
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -134,7 +131,6 @@
         encoder1Counts = encoder1.getPosition();
         encoder2Counts = encoder2.getPosition();
 
-
         motor1Pos = ((encoder1Counts/32)/131.25)*360;
         motor2Pos = ((encoder2Counts/32)/131.25)*360;
 
@@ -151,7 +147,7 @@
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
     // exclude kinematics when still calibrating
-    if (calReady){
+    if (calReady && !controlAngle){
       kinematics();
     }
 
@@ -170,6 +166,9 @@
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
 // write new values to motor's
+    if (motor1SetSpeed ==0 ){
+      motor1PID.SetOutputLimits(0,0);
+    }
     if (motor1SetSpeed > 0 ){ // CCW rotation 
         direction1 = true;
         motor1PID.SetOutputLimits(0,1); // change pid output direction
@@ -177,6 +176,9 @@
         direction1 = false; // CW rotation
         motor1PID.SetOutputLimits(-1,0);
     }
+    if (motor2SetSpeed ==0 ){
+      motor2PID.SetOutputLimits(0,0);
+    }
     if (motor2SetSpeed > 0 ){ // CCW rotation 
         direction2 = true;
         motor2PID.SetOutputLimits(0,1);
@@ -196,7 +198,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 +280,7 @@
 const double Ymax = 0.645;
 const double Ymin = 0.33;
 double Xpos = 0; // set values
-double Ypos =0;
+double Ypos = 0;
 
 
 
@@ -291,12 +302,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