File content as of revision 32:2006977785f5:

#include "actuators.h"
#include "PID.h"
#include "mbed.h"
#include "config.h"
#include "encoder.h"
#include "HIDScope.h"
#include "servo.h"
    // functions for controlling the motors
    bool motorEnable = true;

    bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
    bool direction2 = false;

    float motor1Pos = 0;
    float motor2Pos = 0;

    float motorSpeed1 = 0;
    float motorSpeed2 = 0;

    float motorSetSpeed1 = 0;
    float motorSetSpeed2 = 0;

    float servoPos = 0;

    float motorPWM1 = 0;
    float motorPWM2 = 0;

    // Set PID values
    float Kp1 = 1; 
    float Ki1 = 0; 
    float Kd1 = 0;

    float Kp2 = 1; 
    float Ki2 = 0; 
    float Kd2 = 0;

    float PIDinterval = 0.2;

    float prevMotor2Pos = 0;
    float prevMotor1Pos = 0;
    float prevTime = 0;
    float curTime = 0;

    // Create object instances
    // Initialze motors
    PwmOut motor1(motor1PWMPin);
    PwmOut motor2(motor2PWMPin);

    // Initialize encoders
    Encoder encoder1(enc1A, enc1B);
    Encoder encoder2(enc2A, enc2B);

    // Set direction pins     
    DigitalOut motor1Dir(motor1DirPin);
    DigitalOut motor2Dir(motor2DirPin);  

    // create PID instances
    PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);

    servo Servo(servoPin);
    Timer t;  
    
void motorInit(){
    
    motor1Dir.write(direction1);
    motor2Dir.write(direction2);

    // Set motor PWM period
    motor1.period(1/pwm_frequency);
    motor2.period(1/pwm_frequency);

    PIDmotor1.setSetPoint(motorSetSpeed1);
    PIDmotor2.setSetPoint(motorSetSpeed2);

    PIDmotor1.setProcessValue(motorSpeed1);
    PIDmotor2.setProcessValue(motorSpeed2);

    // set limits for PID output to avoid integrator build up.
    PIDmotor1.setInputLimits(-300, 300);
    PIDmotor2.setInputLimits(-300, 300);
    PIDmotor1.setOutputLimits(-1, 1);
    PIDmotor2.setOutputLimits(-1, 1);
    
    // Turn PID on
    PIDmotor1.setMode(1);
    PIDmotor2.setMode(1);
    
    // start the timer
    t.start();
}


void motorControl(){
    if(motorEnable){  // only run motors if switch is enabled
    // get encoder positions in degrees
        // 131.25:1 gear ratio
        // getPosition uses X2 configuration, so 32 counts per revolution
        // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
        motor1Pos = -((encoder1.getPosition()/32)/131.25)*360;
        motor2Pos = -((encoder2.getPosition()/32)/131.25)*360;

        // check if motor's are within rotational boundarys
    // get  encoder speeds in deg/sec
        curTime = t.read();
        motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime);
        motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime);
        prevTime = curTime;
        prevMotor1Pos = motor1Pos;
        prevMotor2Pos = motor2Pos;
        
    // calculate motor setpoint speed in deg/sec from setpoint x/y speed

    // compute new PID parameters using setpoint angle speeds and encoder speed
        writeMotors();

    }else{
        // write 0 to motors
        motor1.write(0);
        motor2.write(0);
    }
}

void writeMotors(){
    PIDmotor1.setSetPoint(motorSetSpeed1);
    PIDmotor2.setSetPoint(motorSetSpeed2);

    PIDmotor1.setProcessValue(motorSpeed1);
    PIDmotor2.setProcessValue(motorSpeed2);

    motorPWM1 = PIDmotor1.compute();
    motorPWM2 = PIDmotor2.compute();
// write new values to motor's
    if (motorPWM1 > 0 ){ // CCW rotation 
        direction1 = false;
    }else{
        direction1 = true; // CW rotation
    }
    if (motorPWM2 > 0 ){ // CCW rotation 
        direction2 = false;
    }else{
        direction2 = true; // CW rotation
    }
    motor1Dir.write(direction1);
    motor2Dir.write(direction2);
    
    motor1.write(abs(motorPWM1));
    motor2.write(abs(motorPWM2));
}

void servoControl(){
    // use potMeter Value to set servo angle
    Servo.write(servoPos);
    // (optionaly calculate xy position to keep balloon in position)
        // calculate z position using angle
        // calculate x y translation of endpoint
        // find new x and y speed.
    
}

void calibrateMotors(){
    // bool calibrating1 = true;
    // bool calibrating2 = true;
    // motor1Dir.write(false);
    // motor2Dir.write(false);
    // // move motors CCW until they reach outer most position
    // while (calibrating1 || calibrating2){
    //     if(motor1AnglePin == 0){
    //         motor1.write(0.3f);
    //     }else{
    //         motor1.write(0);
    //         calibrating1 = false;
    //     }
    //     if(motor2AnglePin == 0){
    //         motor2.write(0.3f);
    //     }else{
    //         motor2.write(0);
    //         calibrating2 = false;
    //     }
    //     wait(0.2f);
    // }
    // // set the encoder values for angle.
    // encoder1.setValue(0);
    // encoder2.setValue(0);
}