File content as of revision 24:2d7e11441eee:

#include "mbed.h"
#include "config.h"  // settings and pin configurations
#include "encoder.h"
#include "PID.h"
#include "EMG.h"



bool motorEnable = false;

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 motorPWM1 = 0;
float motorPWM2 = 0;

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

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

float PIDinterval = 0.2;
    Encoder encoder1(enc1A, enc1B, true);
    Encoder encoder2(enc2A, enc2B, true);
        PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
    PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
        PwmOut motor1(motor1PWMPin);
    PwmOut motor2(motor2PWMPin);
        AnalogIn pot1(pot1Pin);
    
void initPID(){
    // create PID instances for motors
        // PID pidname(input, output, setpoint, kp, ki, kd, direction)

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

    PIDmotor1.setProcessValue(motorSpeed1);
    PIDmotor2.setProcessValue(motorSpeed2);
    // set PID mode
    PIDmotor1.setMode(1);
    PIDmotor2.setMode(1);

    // set limits for PID output to avoid integrator build up.
    PIDmotor1.setOutputLimits(-1.0, 1.0);
    PIDmotor2.setOutputLimits(-1.0, 1.0);
}

void motorInit(){
    // Initialze motors


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

    // Set initial direction
    motor1Dir.write(direction1);
    motor2Dir.write(direction2);

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

    // Initialize encoders (with speed calculation)

    initPID();
}




void motorControl(){
    if(motorEnable){  // only run motors if switch is enabled

    // get encoder positions
        motor1Pos = encoder1.getPosition();
        motor2Pos = encoder2.getPosition();

        // check if motor's are within rotational boundarys
    // get  encoder speeds
        motorSpeed1 = encoder1.getSpeed();
        motorSpeed2 = encoder2.getSpeed();

    // translate to x/y speed
    // compute new PID parameters using setpoint speeds and x/y speeds
        motorPWM1 = PIDmotor1.compute();
        motorPWM2 = PIDmotor2.compute();
    // translate to motor rotation speed
    // write new values to motor's
        if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
            direction1 = false;
        }else{
            direction1 = true; // CW rotation
        }
        if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
            direction2 = false;
        }else{
            direction2 = true; // CW rotation
        }
        motor1.write(abs(motorPWM1));
        motor2.write(abs(motorPWM2));

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

void setPins(){
    // set input/output pins

}


void checkSwitches(){
    // read motor enable switch
    
    // read pump enable switch
    
    // read servo potmeter position
    
    // read x speed potmeter position
    float motorSetSpeed1 = pot1.read();
    
    // read y speed potmeter position
    
    // read killswitches
    
    }

int main(){
    setPins();
    motorInit(); 
    while (true) {
        checkSwitches();
        // readEMG();
        motorControl();
        // servoControl();
    }
}