Diff: robot.cpp

Revision:

7:a80cb6b06320

Parent:

3:1f47375270c5

Child:

12:8295c02d740f

--- a/robot.cpp	Wed Nov 02 09:26:38 2016 +0000
+++ b/robot.cpp	Wed Nov 02 16:29:13 2016 +0000
@@ -1,5 +1,4 @@
 #include "robot.h"
-#include "geometry.h"
 
 /*
     Pins
@@ -17,107 +16,60 @@
 const PinName lowerEncoderPin1 = D10;
 const PinName lowerEncoderPin2 = D9;
 
-const PinName buttonPin = D3;
+const float initialArmLength = L_min;
 
 /*
     Constructor
 */
 
 Robot::Robot():
-    upperArm(L_min, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
-    lowerArm(L_min, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2),
-    killButton(buttonPin){
-    killButton.fall(this, &Robot::kill);
-    ticker.attach(this, &Robot::doTick, tick);
-    // x,y coordinate when both arms are of length L_min
-    y = 0.5 * d + h;
-    x = sqrt(pow(L_min,2) - pow(0.5 * d, 2));
-    
+    upperArm(initialArmLength, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
+    lowerArm(initialArmLength, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2) {
+    killed = false;
 }
 
 /*
     Methods
 */
 
-void Robot::doTick() {
+// Update the arms
+void Robot::update() {
     upperArm.update();
     lowerArm.update();
 }
 
-void Robot::moveY(float dy) {
-    float goal = y + dy;
-    float upperArmLengthGoal;
-    float lowerArmLengthGoal;
-    
-    const float n = 10;
-    float dy_step = dy/n;
-    
-}
-
 // Set upper arm velocity (rad/s)
 void Robot::setUpperArmVelocity(float referenceVelocity) {
-    upperArm.setVelocity(referenceVelocity);
-}
-
-float Robot::getUpperArmLength(){
-    return upperArm.getLength();
-}
-
-float Robot::getLowerArmLength(){
-    return lowerArm.getLength();    
+    if (!killed) {
+        upperArm.setVelocity(referenceVelocity);
+    }
 }
 
 // Set lower arm velocity (rad/s) (multiplied with -1 because for some reason the directions are different for the two arms)
 void Robot::setLowerArmVelocity(float referenceVelocity) {
-    lowerArm.setVelocity(-1 * referenceVelocity);
+    if (!killed) {
+        lowerArm.setVelocity(-1 * referenceVelocity);
+    }
 }
 
-//void Robot::setArms(float upper, float lower)   {
-//    const float referenceVelocity = 1;
-//    
-//    bool upperGoalReached = false;
-//    bool lowerGoalReached = false;
-//    
-//    float d_upper = upper-upperArm.getLength();
-//    float d_lower = lower-lowerArm.getLength();
-//    
-//    bool upperDir = d_upper >= 0;
-//    bool lowerDir = d_lower >= 0;
-//    
-//    setUpperArmVelocity(upperDir? -1*referenceVelocity:referenceVelocity);
-//    setLowerArmVelocity(lowerDir? referenceVelocity:-1*referenceVelocity);
-//    
-//    while (!(upperGoalReached && lowerGoalReached)) {
-//        if (!upperGoalReached) {
-//            d_upper = upper-upperArm.getLength();
-//            if (upperDir) {
-//                upperGoalReached = d_upper < 0;
-//            } else {
-//                upperGoalReached = d_upper >= 0;
-//            }
-//            if (upperGoalReached) {
-//                setUpperArmVelocity(0);
-//            }
-//        }
-//        if (!lowerGoalReached) {
-//            d_lower = lower-lowerArm.getLength();
-//            if (lowerDir) {
-//                lowerGoalReached = d_lower < 0;
-//            } else {
-//                lowerGoalReached = d_lower >= 0;
-//            }
-//            if (lowerGoalReached) {
-//                setLowerArmVelocity(0);
-//            }
-//        }
-//    }
-//    
-//    // TODO -- update x/y values?
-//    
-//}
-
 // Turn off both arms
 void Robot::kill() {
+    killed = true;
     upperArm.kill();
     lowerArm.kill();
 }
+
+// Indicates if the robot has been killed
+bool Robot::isKilled() {
+    return killed;
+}
+
+// Get upper arm length
+float Robot::getUpperArmLength(){
+    return upperArm.getLength();
+}
+
+// Get lower arm length
+float Robot::getLowerArmLength(){
+    return lowerArm.getLength();    
+}