File content as of revision 11:5c5bd574c01a:

/*
    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"

#define PI           3.14159265358979323846

MODSERIAL pc(USBTX, USBRX);

AnalogIn pot(A1); // Potentiometer on BioRobotics shield

struct vec {
    double x,y,z;
};

vec endPos{0,0,0};

// DC MOTORS //
QEI encoder1(D10,D11,NC,32), encoder3(D12,D13,NC,32), encoder2(PTC5,PTC7, NC, 32);
PwmOut motor1_pwm(D5), motor3_pwm(D6);
DigitalOut motor1_dir(D4), motor3_dir(D7), motor2_A(D2), motor2_B(D3);

float motor1_cur = 0, motor2_cur = 0, motor3_cur = 0;
float motor1_tar = 0, motor2_tar = 0, motor3_tar = 0;
float motor1_zero = 37.5, motor2_zero = 14.3, motor3_zero = 57.8; // in degrees(M1) / cm(M2/M3)

void setMotor(int motor, float motor_spd) // Set the motor speed (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 == 3) {
        motor3_dir.write(motor_dir);
        motor3_pwm.write(motor_spd);
    } else if(motor == 2) {
        motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM
        motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0);
    }
}

void getMotorPositions() // Calculate current motor positions (M1:angle, M2/M3:length) based on encoder pulses
{
    // SOME SCALING NEEDED HERE //
    motor1_cur = encoder1.getPulses();
    motor2_cur = encoder2.getPulses();
    motor3_cur = encoder3.getPulses();
}

void moveToTargets() // Move to the target positions. No PID.
{
    float threshold = 100; // If close to the target pos, don't move
    setMotor(1, (fabs(motor1_cur - motor1_tar) < threshold) ? 0 : (motor1_cur < motor1_tar) ? 0.2 : -0.2);
    setMotor(2, (fabs(motor2_cur - motor2_tar) < threshold) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
    setMotor(3, (fabs(motor3_cur - motor3_tar) < threshold) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
}

// KINEMATICS //
void calculateKinematics(float x, float y, float z)  // y is up
{
    float angle1 = fmod(2*PI - atan2(z, x), 2*PI);
    float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
    float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);

    motor1_tar = (angle1 < 30) ? 30 : (angle1 > 330) ? 330 : angle1; // constrain between 30 - 330
    motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
    motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
}

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

int stepper_steps = 0;
float stepper_angle = 0, stepper_target = 0;

void stepper_step(int direction_) // Requires ~1.5ms wait time between each step, 4096 steps is one rotation
{
    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);
}

Ticker stepper_moveToAngle;
void stepper_move() // Move toward desired angle with threshold. In Ticker function because requires wait otherwise
{
    if(fabs(stepper_angle - stepper_target) > 1) stepper_step((stepper_angle < stepper_target) ? 1 : 0);
}

// SERVO //
PwmOut servo(D9);
void setServo(float angle) // Set servo to specified angle(0-180 degrees)
{
    float i = angle / 1.800;
    float duty = (i + 20) / 1000.00;
    servo.write(duty);
}

// AIMING //
void aim(float angle)  // Moves both stepper and servo so the end affector points towards desired angle
{
    if(angle < 180) {
        servo.write(0);
        stepper_target = angle;
    } else {
        servo.write(180);
        stepper_target = angle - 180;
    }
}

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

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

// CALIBRATION //
AnalogIn switch1(A3), switch2(A4), switch3(A5);
void calibrate() // Calibrates all 3 motors simultaniously
{
    setMotor(1,-0.2); // Set all motors to move towards the switches
    setMotor(2,1);
    setMotor(3,1);

    /*while(!(switch1.read() < 0.5f && switch2.read() > 0.5f && switch3.read() > 0.5f)) { // When all switches have been pushed in, stop
        if(switch1.read() < 0.5f) setMotor(1,0);
        if(switch2.read() > 0.5f) setMotor(2,0);
        if(switch3.read() > 0.5f) setMotor(3,0);
        wait_ms(40);
    }*/
    
    bool doneCalibrating = false;
    while(!doneCalibrating) {
        bool motor1_hit = switch1.read() < 0.5f;
        bool motor2_hit = switch2.read() > 0.5f;
        bool motor3_hit = switch3.read() > 0.5f;
        
        pc.printf("Switches: %d %d %d\r\n",motor1_hit,motor2_hit,motor3_hit);
        
        if(motor1_hit) {
            setMotor(1,0);
        }
        if(motor2_hit) {
            setMotor(2,0);
        }
        if(motor3_hit) {
            setMotor(3,0);
        }
        
        wait_ms(40);
    }
    
    for(int i = 1; i <= 3; i++) setMotor(i,0); // Make sure they've all stopped

    encoder1.reset(); // Reset encoder positions
    encoder2.reset();
    encoder3.reset();

    //setServo(0);
}


int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    motor1_pwm.period(0.0001);
    motor3_pwm.period(0.0001);
    servo.period(0.020);
    //stepper_moveToAngle.attach(&stepper_move, 0.0015);
    //setMotor(1,0.2);
    while(true){
        pc.printf("m1: %f\r\n",switch1.read());
        pc.printf("m2: %f\r\n",switch2.read());
        pc.printf("m3: %f\r\n",switch3.read());
        
        wait_ms(100);
        
        }
    calibrate();

    while (true) {
        /*
        SOME EXAPLE CODE

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

        * KINEMATICS (this should be done every ~30 ms)
        calculateKinematics(x, y, z); // Calculate target positions
        getMotorPositions(); // Calculate current positions
        moveToTargets(); // Set the motors speeds accordingly

        * STEPPER
        stepper_target = ...; // angle (between 0.0 and 180.0)

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

        * AIMING
        aim(...) // value between 0 and 360

        * SOLENOID
        setSolenoid(...); // position, 0(in) or 1(out)

        * LASER
        setLaser(...) // 0(off) or 1(on)

        */

        //  The EMG MBED sends one of these values: 11 12 13 21 22 23 31 32 33 (first digit is GOLEFT(1),STOP(2),GORIGHT(3), same for second digit but z-axis.
        //  endPos.z += (n > 30) ? 1 : (n > 20)  ? 0 : -1;
        //  endPos.x += (n % 10 == 1) ? 1 : (n % 10 == 2) ? 0 : -1;


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

        //getMotorPositions();
        motor1_cur = encoder1.getPulses();
        motor2_cur = encoder2.getPulses();
        motor3_cur = encoder3.getPulses();


        pc.printf("m1: %d\r\n",motor1_cur);
        pc.printf("m2: %d\r\n",motor2_cur);
        pc.printf("m3: %d\r\n",motor3_cur);
    }
}