File content as of revision 7:bfcb74384f46:

/*
    The Poolinator - A pool playing robot for people with DMD

GROUP 10
Sjoerd de Jong              - s1949950
Joost Loohuis               - s1969633
Viktor Edlund               - s2430878
Giuseppina Pinky Diatmiko   - s1898841
Daan v.d Veen               - s2003171
*/

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"

MODSERIAL pc(USBTX, USBRX);

AnalogIn pot(A1);

// DC MOTORS CODE //
QEI encoder1(D10,D11,NC,32), encoder2(D12,D13,NC,32), encoder3(PTC5,PTC7, NC, 32);
PwmOut motor1_pwm(D5), motor2_pwm(D6);
DigitalOut motor1_dir(D4), motor2_dir(D7), motor3_A(D2), motor3_B(D3);

int frequency_pwm = 10000;

int motor1_targetPos = 0, motor2_targetPos = 0, motor3_targetPos = 0;

void setMotor(int motor, float motor_spd) // Between -1 and 1;
{
    int motor_dir = (motor_spd >= 0) ? 0 : 1;
    motor_spd = fabs(motor_spd);

    if(motor == 1) {
        motor1_dir.write(motor_dir);
        motor1_pwm.write(motor_spd);
    } else if(motor == 2) {
        motor2_dir.write(motor_dir);
        motor2_pwm.write(motor_spd);
    } else if(motor == 3) {
        motor3_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 3 has digital pins so no PWM
        motor3_B.write((motor_dir == 0) ? int(motor_spd) : 0);
    }
}

// STEPPER MOTOR CODE //
DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);

int stepper_steps = 0;
float stepper_angle = 0;

void stepper_move(int direction_) // Requires ~1.5ms wait time between each step
{
    STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
    STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
    STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
    STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
    stepper_steps += (direction_ == 0) ? - 1 : 1;
    stepper_steps = (stepper_steps + 8) % 8;
    stepper_angle += ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
}

// SERVO CODE //
PwmOut servo(D9);
void setServo(float angle)
{
    float i = angle / 1.800;
    float duty = (i + 20) / 1000.00;
    servo.write(duty);
}

// SOLENOID CODE //
DigitalOut solenoidA(PTC0), solenoidB(PTC9);
void setSolenoid(int dir)
{
    solenoidA = (dir == 0) ? 1 : 0;
    solenoidB = (dir == 0) ? 0 : 1;
}

// LASER CODE //
DigitalOut laserPin(D8);
void setLaser(bool on)
{
    if(on) laserPin.write(1);
    else laserPin.write(0);
}

// CALIBRATION //
AnalogIn switch1(A2), switch2(A3), switch3(A4);
void calibrate()
{
    setMotor(1,0.2);
    setMotor(2,1);
    setMotor(3,1);

    while(!(switch1.read() > 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) {
        wait_ms(40);
        if(switch1.read() > 0.5) setMotor(1,0);
        if(switch2.read() > 0.5) setMotor(2,0);
        if(switch3.read() > 0.5) setMotor(3,0);
    }
    for(int i = 1; i <= 3; i++) setMotor(i,0);
    
    encoder1.reset();
    encoder2.reset();
    encoder3.reset();
    
    setServo(0);
}


int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    motor1_pwm.period(1.0 / frequency_pwm);
    motor2_pwm.period(1.0 / frequency_pwm);
    servo.period(0.020);

    //calibrate();

    while (true) {
//        float value1 = switch1.read();
//        pc.printf("s1: %f\r\n",value1);
//        float value2 = switch2.read();
//        pc.printf("s2: %f\r\n",value2);
//        float value3 = switch3.read();
//        pc.printf("s3: %f\r\n",value3);
//        wait_ms(100);
        /*
        SOME EXAPLE CODE

        * MOTOR
        setMotor(..., ...) // which motor (1,2,3), and speed (-1, +1)

        * PID
        double currentPos1 = encoder1.getPulses();
        double targetPos1 = 10000;
        double error1 = targetPos1 - currentPos1;
        float PID_speed1 = pid(error) * PID_gain;
        setMotor(1, PID_speed1);

        * STEPPER
        stepper_move(1); // direction (0 or 1)
        wait_ms(1.5); // requires some time between steps

        * SERVO
        setServo(...) // value between 0.0 and 180.0 (= 0 and 180 degrees)

        * SOLENOID
        setSolenoid(1); // shooting
        wait_ms(1000);
        setSolenoid(0);

        */

        // DEMO
//        for(int i = 0; i < 180; i++) {
//            moveServo(i);
//            wait_ms(20);
//        }
//        for(int i = 180; i > 0; i--) {
//            moveServo(i);
//            wait_ms(20);
//        }
//        for(int i = 0; i < 2000; i++) {
//            stepper_move(1); // direction (0 or 1)
//            wait_ms(1.5); // requires some time between steps
//        }
//        for(int i = 0; i < 2000; i++) {
//            stepper_move(0); // direction (0 or 1)
//            wait_ms(1.5); // requires some time between steps
//        }
//        setSolenoid(1);
//        wait_ms(1000);
//        setSolenoid(0);

        float value = pot.read();
        if(value < 0.4) setMotor(3, 1);
        else if (value > 0.6) setMotor(3, -1);
        else setMotor(3,0);

        wait_ms(30);

    }
}