main.cpp

00001 /*
00002     The Poolinator - A pool playing robot for people with DMD
00003 
00004 GROUP 10
00005 Sjoerd de Jong              - s1949950
00006 Joost Loohuis               - s1969633
00007 Viktor Edlund               - s2430878
00008 Giuseppina Pinky Diatmiko   - s1898841
00009 Daan v.d Veen               - s2003171
00010 */
00011 
00012 #include "mbed.h"
00013 #include "HIDScope.h"
00014 #include "QEI.h"
00015 #include "MODSERIAL.h"
00016 #include "BiQuad.h"
00017 #include "FastPWM.h"
00018 #include <Pulse.h>
00019 
00020 #define PI           3.14159265358979323846
00021 
00022 MODSERIAL pc(USBTX, USBRX);
00023 
00024 bool demo = false;
00025 enum State {CALIBRATE, INIT_0, IDLE, INIT_1, PHASE_1, INIT_2, PHASE_2, INIT_3, PHASE_3, PHASE_1EMG};
00026 State currentState = CALIBRATE;
00027 
00028 struct vec { // 3D vector(x,y,z) to store positions
00029     double x,y,z;
00030 };
00031 
00032 vec endPos{0,55,20};
00033 vec ballPos{10,50,10};
00034 
00035 // EMG CONTROLS
00036 DigitalIn EMG_D(D15); // LDR connected, used to indicate which arm is sending atm, (left arm/right arm)
00037 AnalogIn EMG_A(A5); // LDR connected, used to indicate what that arm is doing, (idle/move left/move right)
00038 void moveWithEMG()
00039 {
00040     int whichEMG = EMG_D.read(); // 0 or 1, left arm or right arm
00041     float EMGvalue = EMG_A.read();
00042     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
00043     float delta = 0.08; // How much to move in the xz direction
00044     if(whichEMG == 0) {
00045         if(EMGstate == 1) endPos.x -= delta; // Move the end position a bit to the left
00046         if(EMGstate == 2) endPos.x += delta; // ... to the right
00047     } else {
00048         if(EMGstate == 1) endPos.z -= delta; // ... away
00049         if(EMGstate == 2) endPos.z += delta; // ... towards us
00050     }
00051 }
00052 
00053 // JOYSTICK CONTROL //
00054 AnalogIn joyX(A2), joyY(A1), slider(A0);
00055 DigitalIn joyButton(PTB20);
00056 void moveWithJoystick() // Move the endpositon based on joystick input.
00057 {
00058     float delta = 0.04;
00059     if(joyX.read() < 0.2) endPos.x += delta;
00060     else if(joyX.read() > 0.8) endPos.x -= delta;
00061     if(joyY.read() < 0.2) endPos.z += delta;
00062     else if(joyY.read() > 0.8) endPos.z -= delta;
00063     if(slider.read() < 0.2) endPos.y += delta;
00064     else if(slider.read() > 0.9) endPos.y -= delta;
00065 }
00066 
00067 // DC MOTORS //
00068 QEI encoder1(D10,D11,NC,32), encoder2(PTC5,PTC7, NC, 32), encoder3(D12,D13,NC,32);
00069 PwmOut motor1_pwm(D5), motor3_pwm(D6);
00070 DigitalOut motor1_dir(D4), motor3_dir(D7), motor2_A(D2), motor2_B(D3);
00071 
00072 float motor1_cur = 0, motor2_cur = 0, motor3_cur = 0;
00073 float motor1_tar = 0, motor2_tar = 0, motor3_tar = 0;
00074 
00075 void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1) of a motor (1, 2, or 3)
00076 {
00077     int motor_dir = (motor_spd >= 0) ? 0 : 1;
00078     motor_spd = fabs(motor_spd);
00079 
00080     if(motor == 1) {
00081         motor1_dir.write(1 - motor_dir);
00082         motor1_pwm.write(motor_spd);
00083     } else if(motor == 3) {
00084         motor3_dir.write(motor_dir);
00085         motor3_pwm.write(motor_spd);
00086     } else if(motor == 2) {
00087         motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM, always fully on or fully off
00088         motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0);
00089     }
00090 }
00091 
00092 void getMotorPositions() // Calculate current motor positions (M1:angle, M2/M3:length) based on encoder pulses
00093 {
00094     motor1_cur = 244 - float(encoder1.getPulses()) / 18530.00 * 360;
00095     if(motor1_cur > 360) motor1_cur -= 360;
00096     if(motor1_cur < 0) motor1_cur += 360;
00097     motor2_cur = float(encoder2.getPulses()) / 41920.00 + 14.3;
00098     motor3_cur = float(encoder3.getPulses()) / 41920.00 + 57.8;
00099 }
00100 
00101 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.
00102 {
00103     setMotor(1, (fabs(motor1_cur - motor1_tar) < 0.5) ? 0 : (motor1_cur < motor1_tar) ? 0.08 : -0.08);
00104     setMotor(2, (fabs(motor2_cur - motor2_tar) < 0.04) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
00105     setMotor(3, (fabs(motor3_cur - motor3_tar) < 0.04) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
00106 }
00107 
00108 // KINEMATICS // (NOT USED ANYMORE, USED THE NEXT FUNCTION, WHICH TAKES THE SERVO POSITION AS ENDPOINT)
00109 //void calculateKinematics(float x, float y, float z)  // y is up
00110 //{
00111 //    float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
00112 //    float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
00113 //    float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);
00114 //
00115 //    motor1_tar = angle1;
00116 //    motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
00117 //    motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
00118 //}
00119 
00120 void calculateKinematicsHand(float x, float y, float z)  // y is up, calculate target positions of the motors using the kinematics
00121 {
00122     float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI)); // Make sure the angle is between 0-360 degrees
00123     float xz = sqrt(x*x + z*z) - 15; // Offset 15cm because servo is 15cm away from endaffector in the 2D plane
00124     y -= 9; // Offset y with 9 cm because the servo is 9 cm below endaffector
00125     float angle2 = acos(sqrt(xz*xz + y*y) / 100.00) + atan2(xz, y);
00126     float angle3 = 2 * asin(sqrt(xz*xz +y*y) / 100.00);
00127 
00128     motor1_tar = angle1;
00129     motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
00130     motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
00131 }
00132 
00133 // STEPPER MOTOR // 
00134 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
00135 
00136 int stepper_steps = 0;
00137 float stepper_angle = 0, stepper_target = 0;
00138 
00139 void stepper_step(int direction_) // Requires ~1.5ms wait time between each step, 4096 steps is one rotation
00140 { // Stepper motor has electromagnets inside, depending on which are on/off, the motor will move to a different position, this code does that.
00141     STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
00142     STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
00143     STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
00144     STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
00145     stepper_steps += (direction_ == 0) ? - 1 : 1;
00146     stepper_steps = (stepper_steps + 8) % 8;
00147     stepper_angle -= ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
00148 }
00149 
00150 Ticker stepper_moveToAngle;
00151 void stepper_move() // Move toward desired angle with threshold. In Ticker function because requires wait otherwise
00152 {
00153     if(fabs(stepper_angle - stepper_target) > 1) stepper_step((stepper_angle < stepper_target) ? 0 : 1);
00154 }
00155 
00156 // SERVO //
00157 AnalogOut servo(DAC0_OUT); // Write analog value to the Arduino
00158 void setServo(float i) // Set servo to specified angle(0-180 degrees) signal measured by Arduino and decoded there.
00159 {
00160     servo = i / 180.00;
00161 }
00162 
00163 // AIMING //
00164 float aimAngle, aimTilt = 45;
00165 float cueLength = 15;
00166 void aim(float angle, float tilt)  // Moves both stepper and servo so the end affector points towards desired angle
00167 {
00168     setServo(tilt + 90);
00169 
00170     // SPHERICAL TO CARTESIAN:
00171     float handX = ballPos.x + cueLength * sin(tilt / 180 * PI) * cos(angle / 180 * PI);
00172     float handY = ballPos.y - cueLength * cos(tilt / 180 * PI);
00173     float handZ = ballPos.z - cueLength * sin(tilt / 180 * PI) * sin(angle / 180 * PI);
00174     endPos.x = handX;
00175     endPos.y = handY;
00176     endPos.z = handZ;
00177 
00178     float stepperAngle = angle - fmod(2*PI - atan2(handZ, handX), 2*PI) * (180 / PI); // Make sure it stays between 0-360 degrees
00179     if(stepperAngle < 0) stepperAngle += 360;
00180     stepper_target = stepperAngle;
00181 }
00182 
00183 // SOLENOID //
00184 DigitalOut solenoidA(PTC0), solenoidB(PTC9);
00185 void setSolenoid(int dir) // 1 is out, 0 is in
00186 {
00187     solenoidA = (dir == 0) ? 0 : 1; // IMPOSSIBLE TO CREATE SHORT CIRCUIT THIS WAY, A is always inverse of B!!
00188     solenoidB = (dir == 0) ? 1 : 0;
00189 }
00190 
00191 // LASER //
00192 DigitalOut laserPin(D8);
00193 void setLaser(bool on)
00194 {
00195     if(on) laserPin.write(1);
00196     else laserPin.write(0);
00197 }
00198 
00199 // CALIBRATION // a3 a4 A5
00200 AnalogIn switch1(A3), switch2(A4);
00201 DigitalIn switch3(D14);
00202 void calibrate() // Calibrates all 3 motors simultaniously
00203 {
00204     setMotor(1,0.1); // Set all motors to move towards the switches
00205     setMotor(2,1);
00206     setMotor(3,1);
00207 
00208     while(!(switch1.read() < 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) { // When all switches have been pushed in, stop
00209         if(switch1.read() < 0.5) setMotor(1,0); // When one of the switches has been pushed in, stop that motor
00210         if(switch2.read() > 0.5) setMotor(2,0);
00211         if(switch3.read() > 0.5) setMotor(3,0);
00212         wait_ms(30);
00213     }
00214     for(int i = 1; i <= 3; i++) setMotor(i,0); // Make sure they've all stopped
00215 
00216     encoder1.reset(); // Reset encoder positions
00217     encoder2.reset();
00218     encoder3.reset();
00219 }
00220 
00221 
00222 int main()
00223 {
00224     pc.baud(115200);
00225     pc.printf("\r\nStarting...\r\n\r\n");
00226     motor1_pwm.period(0.020);
00227     motor3_pwm.period(0.020);
00228     stepper_moveToAngle.attach(&stepper_move, 0.0015);
00229 
00230     while (true) {
00231         switch(currentState) {
00232             case CALIBRATE: // Calibrate the hand
00233                 setSolenoid(0);
00234                 calibrate();
00235                 setServo(90);
00236                 currentState = INIT_0; // Go to next state after calibration
00237                 break;
00238             case INIT_0: // Go to idle position (x=0, y=55, z=20);
00239                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
00240                 getMotorPositions(); // Calculate current positions
00241                 moveToTargets(); // Set the motors speeds accordingly
00242                 wait_ms(40);
00243                 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
00244                 break;
00245             case IDLE: // Wait for button press, stop all motors
00246                 setMotor(1,0);
00247                 setMotor(2,0);
00248                 setMotor(3,0);
00249                 wait_ms(40);
00250                 if(joyButton.read() == 0) {
00251                     if(demo) currentState = PHASE_1;
00252                     else currentState = PHASE_1EMG;
00253                     setLaser(1);
00254                     wait_ms(1000); // Wait a second after the button has been pressed so the button check in the next phase doesn't trigger
00255                 }
00256                 break;
00257             case PHASE_1EMG: // Move endaffector with EMG
00258                 moveWithEMG();
00259                 setServo(80);
00260                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
00261                 getMotorPositions(); // Calculate current positions
00262                 moveToTargets(); // Set the motors speeds accordingly
00263                 if(joyButton.read() == 0) {
00264                     currentState = PHASE_1;
00265                     wait_ms(1000);
00266                 }
00267                 wait_ms(40);
00268                 break;
00269             case PHASE_1: // MOVE endaffector with joystick
00270                 moveWithJoystick();
00271                 setServo(80);
00272                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
00273                 getMotorPositions(); // Calculate current positions
00274                 moveToTargets(); // Set the motors speeds accordingly
00275                 if(joyButton.read() == 0) {
00276                     currentState = PHASE_2;
00277                     wait_ms(1000);
00278                     ballPos.x = endPos.x;
00279                     ballPos.y = 75;
00280                     ballPos.z = endPos.z;
00281                     setLaser(0);
00282                 }
00283                 wait_ms(40);
00284                 break;
00285 
00286             case PHASE_2: // Control angle/tilt of the cue with joystick/slider
00287                 if(slider.read() > 0.9) aimTilt += 0.25;
00288                 else if(slider.read() < 0.1) aimTilt -= 0.25;
00289                 aimTilt = (aimTilt < 0) ? 0 : (aimTilt > 90) ? 90 : aimTilt;
00290 
00291                 if(joyX.read() < 0.2) aimAngle -= 0.3;
00292                 else if(joyX.read() > 0.8) aimAngle += 0.3;
00293                 aimAngle = (aimAngle < 0) ? 360 : (aimAngle > 360) ? 0 : aimAngle;
00294 
00295                 aim(aimAngle, aimTilt);
00296 
00297                 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
00298                 getMotorPositions(); // Calculate current positions
00299                 moveToTargets(); // Set the motors speeds accordingly
00300                 wait_ms(40);
00301 
00302                 if(joyButton.read() == 0) {
00303                     currentState = PHASE_3;
00304                 }
00305                 break;
00306             case PHASE_3: // Shoot, then reset position and go to idle
00307                 setSolenoid(1);
00308                 wait_ms(500);
00309                 setSolenoid(0);
00310                 currentState = INIT_0;
00311                 endPos.x = 0;
00312                 endPos.y = 55;
00313                 endPos.z = 20;
00314                 aimTilt = 45;
00315                 aimAngle = 0;
00316                 stepper_target = 0;
00317                 break;
00318         }
00319         /*
00320         SOME EXAPLE CODE
00321 
00322         * MOTOR
00323         setMotor(..., ...) // which motor (1,2,3), and speed (-1.0, +1.0)
00324 
00325         * KINEMATICS (this should be done every ~30 ms)
00326         calculateKinematics(x, y, z); // Calculate target positions
00327         getMotorPositions(); // Calculate current positions
00328         moveToTargets(); // Set the motors speeds accordingly
00329 
00330         * STEPPER
00331         stepper_target = ...; // angle (between 0.0 and 180.0)
00332 
00333         * SERVO
00334         setServo(...) // value between 0.0 and 180.0 (= 0 and 180 degrees)
00335 
00336         * SOLENOID
00337         setSolenoid(...); // position, 0(in) or 1(out)
00338 
00339         * LASER
00340         setLaser(...) // 0(off) or 1(on)
00341 
00342         */
00343     }
00344 }