Diff: robot.cpp

Revision:

2:fc869e45e672

Child:

3:1f47375270c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Wed Nov 02 08:51:12 2016 +0000
@@ -0,0 +1,116 @@
+#include "robot.h"
+#include "geometry.h"
+
+/*
+    Pins
+*/
+
+const PinName upperMotorDir = D7;
+const PinName upperMotorPWM = D6;
+
+const PinName lowerMotorDir = D4;
+const PinName lowerMotorPWM = D5;
+
+const PinName upperEncoderPin1 = D13;
+const PinName upperEncoderPin2 = D12;
+
+const PinName lowerEncoderPin1 = D10;
+const PinName lowerEncoderPin2 = D9;
+
+const PinName buttonPin = D3;
+
+/*
+    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));
+    
+}
+
+/*
+    Methods
+*/
+
+void Robot::doTick() {
+    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);
+}
+
+// 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);
+}
+
+//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() {
+    upperArm.kill();
+    lowerArm.kill();
+}