File content as of revision 104:750d7e13137d:

#include "actuators.h"
#include "PID.h"
#include "mbed.h"
#include "config.h"
#include "encoder.h"
#include "HIDScope.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 servoPulsewidth;
double servoPos = 0;

double motor1PWM = 0;
double motor2PWM = 0;

// Set PID values
double Kp1 = 0.008;
double Ki1 = 0.08;
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;

// Set servo values
const double servoPeriod = 0.020;
const double servo_range = 20;  // Servo range (-range/ range) [deg]
const double servo_vel = 15;    // Servo velocity [deg/s]
const double servo_inc = servo_vel * servoCall;     // Servo postion increment per cycle
double servo_pos = 0;
double servoSpeed = -1;
double scaleXSpeed = 10;
double scaleYSpeed = 20;
double scaleZSpeed = 1;



// Set calibration values
double motorCalSpeed = 10; // deg/sec
double returnSpeed = -10;
bool springHit = false;
float lastCall = 0;
bool calibrating1 = true;
bool calibrating2 = false;

// Create object instances
// Safety Pin
DigitalIn safetyIn(safetyPin);

// Initialze motors
PwmOut motor1(motor1PWMPin);
PwmOut motor2(motor2PWMPin);

// initialize Servo
PwmOut servo(servoPin);


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

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

    // set servo period
    servo.period(servoPeriod);


    // start the timer
    t.start();
}


void motorControl()
{
    // EMG signals to motor speeds
//        motor1SetSpeed = x_velocity*scaleXSpeed;
//        motor2SetSpeed = y_velocity*scaleYSpeed;
//        servoSpeed = z_velocity*scaleZSpeed;


    // 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()
{
    if (servoSpeed > 0) {
        if((servo_pos + servo_inc) <= servo_range) {       // If increment step does not exceed maximum range
            servo_pos += servo_inc;
        }
    } else if (servoSpeed < 0) {
        if((servo_pos - servo_inc) >= -servo_range) {       // If increment step does not exceed maximum range
            servo_pos -= servo_inc;
        }
    }
    servoPulsewidth = 0.0015 + (servo_pos/90)*0.001;
    if(motorsEnable) {
        servo.pulsewidth(servoPulsewidth);
    } else if (!motorsEnable) {
        servo.pulsewidth(0.0017);
    }
}

void calibrateMotors()
{
    safetyOn = false; // safety springs off
    motorsEnable = true; // motors on
    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) {
            motorControl();
            lastCall = now;
        }

    }
    motorsEnable = false; // turn motor's off again
    safetyOn = true; // turn safety on after callibration
}


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