File content as of revision 95:94f02d01ebdf:

#include "actuators.h"
#include "PID.h"
#include "mbed.h"
#include "config.h"
#include "encoder.h"
#include "HIDScope.h"
#include "Servo.h"
#include "buttons.h"
    // functions for controlling the motors
    bool motorsEnable = false;
    bool safetyOn = true; // start with safety off for calibration


    double encoder1Counts = 0;
    double encoder2Counts = 0;

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

    double motor1Pos = 0;
    double motor2Pos = 0;

    double motor1Speed = 0;
    double motor2Speed = 0;

    double motor1SetSpeed = 0;
    double motor2SetSpeed = 0;

    double servoPos = 0;

    double motor1PWM = 0;
    double motor2PWM = 0;

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

    double Kp2 = 0.008; 
    double Ki2 = 0.08; 
    double Kd2 = 0;

    double motor1PrevCounts = 0;
    double motor2PrevCounts = 0;
    double prevTime = 0;
    double now = 0;
    double timechange;
    bool pidOut = 0;

    // for Calibration:
    bool calibrating1 = true;
    bool calibrating2 = false;
    double motorCalSpeed = 10; // deg/sec
    double returnSpeed = -10;
    bool springHit = false;
    float lastCall = 0;


    // Create object instances
    // Safety Pin
    DigitalIn safetyIn(safetyPin);
    
    // 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 motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
    PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);

    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);
    
    motor1PID.SetSampleTime(motorCall);
    motor2PID.SetSampleTime(motorCall);
    
    motor1PID.SetOutputLimits(0,1);
    motor2PID.SetOutputLimits(0,1);
    
    // Turn PID on
    motor1PID.SetMode(AUTOMATIC);
    motor2PID.SetMode(AUTOMATIC);
    
    // start the timer
    t.start();
}


void motorControl(){
    // 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

        encoder1Counts = encoder1.getPosition();
        encoder2Counts = encoder2.getPosition();


        motor1Pos = -((encoder1Counts/32)/131.25)*360;
        motor2Pos = -((encoder2Counts/32)/131.25)*360;

        // check if motor's are within rotational boundarys
    // get  encoder speeds in deg/sec
        now = t.read(); 
        timechange = (now - prevTime);
        motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
        motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
        prevTime = now;
        motor1PrevCounts = encoder1Counts;
        motor2PrevCounts = encoder2Counts;
        
    // calculate motor setpoint speed in deg/sec from setpoint x/y speed

        
    if(motorsEnable){  // only run motors if switch is enabled
    // 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(){
    motor1PID.Compute(); // calculate PID outputs, output changes automatically
    motor2PID.Compute();
// write new values to motor's
    if (motor1SetSpeed > 0 ){ // CCW rotation 
        direction1 = false;
        motor1PID.SetOutputLimits(0,1); // change pid output direction
    }else{
        direction1 = true; // CW rotation
        motor1PID.SetOutputLimits(-1,0);
    }
    if (motor2SetSpeed > 0 ){ // CCW rotation 
        direction2 = false;
        motor2PID.SetOutputLimits(0,1);
    }else{
        direction2 = true; // CW rotation
        motor2PID.SetOutputLimits(-1,0);
    }
    motor1Dir.write(direction1);
    motor2Dir.write(direction2);

    motor1.write(abs(motor1PWM));
    motor2.write(abs(motor2PWM));
}

void servoControl(){
    // use potMeter Value to set servo angle
    if (motorsEnable){
        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(){
   safetyOn = false;
   redLed.write(0); greenLed.write(1); blueLed.write(1);
   while (calibrating1 || calibrating2){
       if (calibrating1){
            redLed.write(1); greenLed.write(0); blueLed.write(1);
           if(safetyIn.read() !=1){ // check if arm reached safety position
               encoder1.setPosition(0); // set motor 1 cal angle
               motor1SetSpeed = returnSpeed; // move away
               springHit = true;
           }else{ 
               if(springHit){ // if hit and after is no longer touching spring
                motor1SetSpeed = 0;
                springHit = false;
                calibrating1 = false;
                calibrating2 = true; // start calibrating 2
               }
           }
       }
       if (calibrating2){
            motor2SetSpeed = motorCalSpeed; 
            redLed.write(1); greenLed.write(1); blueLed.write(0);
           if(safetyIn.read() !=1){ // check if arm reached safety position
               encoder2.setPosition(0); // set motor 2 cal angle
               motor2SetSpeed = returnSpeed; // move away
               springHit = true;
           }else{ 
               if(springHit){ // if hit and after is no longer touching spring
                motor2SetSpeed = 0;
                springHit = false;
                calibrating2 = false; // stop calibrating 2
               }
           }
       }
       now = t.read(); // call motor using timer instead of wait
       if(now - lastCall > motorCall){
           writeMotors();
           lastCall = now;
       }

   }
    safetyOn = true; // turn safety on after callibration
}


void safety(){
    if (safetyOn){
        if (safetyIn.read() != 1){
            motorsEnable = false;
        }
    }
}