File content as of revision 15:47d949e2de1a:

/*
    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"
#include <Pulse.h>

#define PI           3.14159265358979323846

MODSERIAL pc(USBTX, USBRX);

bool demo = false;
enum State {CALIBRATE, INIT_0, IDLE, INIT_1, PHASE_1, INIT_2, PHASE_2, INIT_3, PHASE_3, PHASE_1EMG};
State currentState = CALIBRATE;

struct vec { // 3D vector(x,y,z) to store positions
    double x,y,z;
};

vec endPos{0,55,20};
vec ballPos{10,50,10};

// EMG CONTROLS
DigitalIn EMG_D(D15); // LDR connected, used to indicate which arm is sending atm, (left arm/right arm)
AnalogIn EMG_A(A5); // LDR connected, used to indicate what that arm is doing, (idle/move left/move right)
void moveWithEMG()
{
    int whichEMG = EMG_D.read(); // 0 or 1, left arm or right arm
    float EMGvalue = EMG_A.read();
    int EMGstate = (EMGvalue < 0.29) ? 0 : (EMGvalue < 0.7) ? 1 : 2;  // 0 if the led is off, 1 if it's half brightness, 2 if it's on completely
    float delta = 0.08; // How much to move in the xz direction
    if(whichEMG == 0) {
        if(EMGstate == 1) endPos.x -= delta; // Move the end position a bit to the left
        if(EMGstate == 2) endPos.x += delta; // ... to the right
    } else {
        if(EMGstate == 1) endPos.z -= delta; // ... away
        if(EMGstate == 2) endPos.z += delta; // ... towards us
    }
}

// JOYSTICK CONTROL //
AnalogIn joyX(A2), joyY(A1), slider(A0);
DigitalIn joyButton(PTB20);
void moveWithJoystick() // Move the endpositon based on joystick input.
{
    float delta = 0.04;
    if(joyX.read() < 0.2) endPos.x += delta;
    else if(joyX.read() > 0.8) endPos.x -= delta;
    if(joyY.read() < 0.2) endPos.z += delta;
    else if(joyY.read() > 0.8) endPos.z -= delta;
    if(slider.read() < 0.2) endPos.y += delta;
    else if(slider.read() > 0.9) endPos.y -= delta;
}

// DC MOTORS //
QEI encoder1(D10,D11,NC,32), encoder2(PTC5,PTC7, NC, 32), encoder3(D12,D13,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;

void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1) of a motor (1, 2, or 3)
{
    int motor_dir = (motor_spd >= 0) ? 0 : 1;
    motor_spd = fabs(motor_spd);

    if(motor == 1) {
        motor1_dir.write(1 - 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, always fully on or fully off
        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
{
    motor1_cur = 244 - float(encoder1.getPulses()) / 18530.00 * 360;
    if(motor1_cur > 360) motor1_cur -= 360;
    if(motor1_cur < 0) motor1_cur += 360;
    motor2_cur = float(encoder2.getPulses()) / 41920.00 + 14.3;
    motor3_cur = float(encoder3.getPulses()) / 41920.00 + 57.8;
}

void moveToTargets() // Move to the target positions. No PID. If the error(the fabs(.....)) is smaller than some threshold, then the motors are off, otherwise they are fully on for motor 2/3, and 0.08 speed for motor 1.
{
    setMotor(1, (fabs(motor1_cur - motor1_tar) < 0.5) ? 0 : (motor1_cur < motor1_tar) ? 0.08 : -0.08);
    setMotor(2, (fabs(motor2_cur - motor2_tar) < 0.04) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
    setMotor(3, (fabs(motor3_cur - motor3_tar) < 0.04) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
}

// KINEMATICS // (NOT USED ANYMORE, USED THE NEXT FUNCTION, WHICH TAKES THE SERVO POSITION AS ENDPOINT)
//void calculateKinematics(float x, float y, float z)  // y is up
//{
//    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (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;
//    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)
//}

void calculateKinematicsHand(float x, float y, float z)  // y is up, calculate target positions of the motors using the kinematics
{
    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI)); // Make sure the angle is between 0-360 degrees
    float xz = sqrt(x*x + z*z) - 15; // Offset 15cm because servo is 15cm away from endaffector in the 2D plane
    y -= 9; // Offset y with 9 cm because the servo is 9 cm below endaffector
    float angle2 = acos(sqrt(xz*xz + y*y) / 100.00) + atan2(xz, y);
    float angle3 = 2 * asin(sqrt(xz*xz +y*y) / 100.00);

    motor1_tar = angle1;
    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 motor has electromagnets inside, depending on which are on/off, the motor will move to a different position, this code does that.
    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) ? 0 : 1);
}

// SERVO //
AnalogOut servo(DAC0_OUT); // Write analog value to the Arduino
void setServo(float i) // Set servo to specified angle(0-180 degrees) signal measured by Arduino and decoded there.
{
    servo = i / 180.00;
}

// AIMING //
float aimAngle, aimTilt = 45;
float cueLength = 15;
void aim(float angle, float tilt)  // Moves both stepper and servo so the end affector points towards desired angle
{
    setServo(tilt + 90);

    // SPHERICAL TO CARTESIAN:
    float handX = ballPos.x + cueLength * sin(tilt / 180 * PI) * cos(angle / 180 * PI);
    float handY = ballPos.y - cueLength * cos(tilt / 180 * PI);
    float handZ = ballPos.z - cueLength * sin(tilt / 180 * PI) * sin(angle / 180 * PI);
    endPos.x = handX;
    endPos.y = handY;
    endPos.z = handZ;

    float stepperAngle = angle - fmod(2*PI - atan2(handZ, handX), 2*PI) * (180 / PI); // Make sure it stays between 0-360 degrees
    if(stepperAngle < 0) stepperAngle += 360;
    stepper_target = stepperAngle;
}

// SOLENOID //
DigitalOut solenoidA(PTC0), solenoidB(PTC9);
void setSolenoid(int dir) // 1 is out, 0 is in
{
    solenoidA = (dir == 0) ? 0 : 1; // IMPOSSIBLE TO CREATE SHORT CIRCUIT THIS WAY, A is always inverse of B!!
    solenoidB = (dir == 0) ? 1 : 0;
}

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

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

    while(!(switch1.read() < 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) { // When all switches have been pushed in, stop
        if(switch1.read() < 0.5) setMotor(1,0); // When one of the switches has been pushed in, stop that motor
        if(switch2.read() > 0.5) setMotor(2,0);
        if(switch3.read() > 0.5) setMotor(3,0);
        wait_ms(30);
    }
    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();
}


int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    motor1_pwm.period(0.020);
    motor3_pwm.period(0.020);
    stepper_moveToAngle.attach(&stepper_move, 0.0015);

    while (true) {
        switch(currentState) {
            case CALIBRATE: // Calibrate the hand
                setSolenoid(0);
                calibrate();
                setServo(90);
                currentState = INIT_0; // Go to next state after calibration
                break;
            case INIT_0: // Go to idle position (x=0, y=55, z=20);
                calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
                getMotorPositions(); // Calculate current positions
                moveToTargets(); // Set the motors speeds accordingly
                wait_ms(40);
                if(fabs(motor1_cur - motor1_tar) < 0.6 && fabs(motor2_cur - motor2_tar) < 0.05 && fabs(motor3_cur - motor3_tar) < 0.05) currentState = IDLE; // if idle position reached, go to next state
                break;
            case IDLE: // Wait for button press, stop all motors
                setMotor(1,0);
                setMotor(2,0);
                setMotor(3,0);
                wait_ms(40);
                if(joyButton.read() == 0) {
                    if(demo) currentState = PHASE_1;
                    else currentState = PHASE_1EMG;
                    setLaser(1);
                    wait_ms(1000); // Wait a second after the button has been pressed so the button check in the next phase doesn't trigger
                }
                break;
            case PHASE_1EMG: // Move endaffector with EMG
                moveWithEMG();
                setServo(80);
                calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
                getMotorPositions(); // Calculate current positions
                moveToTargets(); // Set the motors speeds accordingly
                if(joyButton.read() == 0) {
                    currentState = PHASE_1;
                    wait_ms(1000);
                }
                wait_ms(40);
                break;
            case PHASE_1: // MOVE endaffector with joystick
                moveWithJoystick();
                setServo(80);
                calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
                getMotorPositions(); // Calculate current positions
                moveToTargets(); // Set the motors speeds accordingly
                if(joyButton.read() == 0) {
                    currentState = PHASE_2;
                    wait_ms(1000);
                    ballPos.x = endPos.x;
                    ballPos.y = 75;
                    ballPos.z = endPos.z;
                    setLaser(0);
                }
                wait_ms(40);
                break;

            case PHASE_2: // Control angle/tilt of the cue with joystick/slider
                if(slider.read() > 0.9) aimTilt += 0.25;
                else if(slider.read() < 0.1) aimTilt -= 0.25;
                aimTilt = (aimTilt < 0) ? 0 : (aimTilt > 90) ? 90 : aimTilt;

                if(joyX.read() < 0.2) aimAngle -= 0.3;
                else if(joyX.read() > 0.8) aimAngle += 0.3;
                aimAngle = (aimAngle < 0) ? 360 : (aimAngle > 360) ? 0 : aimAngle;

                aim(aimAngle, aimTilt);

                calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
                getMotorPositions(); // Calculate current positions
                moveToTargets(); // Set the motors speeds accordingly
                wait_ms(40);

                if(joyButton.read() == 0) {
                    currentState = PHASE_3;
                }
                break;
            case PHASE_3: // Shoot, then reset position and go to idle
                setSolenoid(1);
                wait_ms(500);
                setSolenoid(0);
                currentState = INIT_0;
                endPos.x = 0;
                endPos.y = 55;
                endPos.z = 20;
                aimTilt = 45;
                aimAngle = 0;
                stepper_target = 0;
                break;
        }
        /*
        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)

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

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

        */
    }
}