File content as of revision 3:1f47375270c5:

#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);
}

float Robot::getUpperArmLength(){
    return upperArm.getLength();
}

float Robot::getLowerArmLength(){
    return lowerArm.getLength();    
}

// 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();
}