Code for the Poolinator
Dependencies: mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- sjoerd1999
- Date:
- 2019-10-31
- Revision:
- 10:6711ee2f1fa8
- Parent:
- 9:913a59894698
File content as of revision 10:6711ee2f1fa8:
/* 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 // COMMUNICATION MODSERIAL pc(USBTX, USBRX); PulseInOut pulsePin(D2); void moveWithEMG() { int i = int(pulsePin.read_high_us(1000) / 100.00 + 0.5); if(i > 0 && i < 10) { int EMG_A = (i > 6) ? 2 : (i > 3) ? 1 : 0; int EMG_B = (i == 1 || i == 4 || i == 7) ? 0 : (i == 2 || i == 5 || i == 8) ? 1 : 2; // Do something here } } struct vec { double x,y,z; }; vec endPos{0,60,10}; vec ballPos{10,50,10}; bool demo = true; enum State {CALIBRATE, INIT_0, IDLE, INIT_1, PHASE_1, INIT_2, PHASE_2, INIT_3, PHASE_3}; State currentState = CALIBRATE; // JOYSTICK CONTROL // AnalogIn joyX(A2), joyY(A1), slider(A0); DigitalIn joyButton(PTB20); void moveWithJoystick() { 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(D12,D13,NC,32), encoder2(PTC5,PTC7, NC, 32), encoder3(D10,D11,NC,32); PwmOut motor1_pwm(D6); DigitalOut motor1_dir(D7), motor3_A(D8), motor3_B(D9), 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(1 - motor_dir); motor1_pwm.write(motor_spd); } else if(motor == 3) { motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 3 has digital pins so no PWM motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0); } 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 { // 18530 pulses per rotation 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. { setMotor(1, (fabs(motor1_cur - motor1_tar) < 1) ? 0 : (motor1_cur < motor1_tar) ? 0.1 : -0.1); setMotor(2, (fabs(motor2_cur - motor2_tar) < 0.03) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1); setMotor(3, (fabs(motor3_cur - motor3_tar) < 0.03) ? 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) * (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 { float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI)); float xz = sqrt(x*x + z*z) - 15; y -= 9; 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_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) PWM signal measured by Arduino and decoded there. { servo = i / 180.00; } // AIMING // float aimAngle, aimTilt; float cueLength = 18; 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); 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; 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), 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.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); 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(); //setServo(0); setSolenoid(0); //setLaser(0); } int main() { pc.baud(115200); pc.printf("\r\nStarting...\r\n\r\n"); motor1_pwm.period(0.0001); //motor3_pwm.period(0.0001); stepper_moveToAngle.attach(&stepper_move, 0.0015); while (true) { switch(currentState) { case CALIBRATE: calibrate(); currentState = PHASE_1; setServo(90); break; case PHASE_1: if(demo) moveWithJoystick(); else moveWithEMG(); calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions getMotorPositions(); // Calculate current positions moveToTargets(); // Set the motors speeds accordingly pc.printf("x: %f\r\n",endPos.x); pc.printf("y: %f\r\n",endPos.y); pc.printf("z: %f\r\n",endPos.z); if(joyButton.read() == 0) { currentState = PHASE_2; wait_ms(1000); ballPos.x = endPos.x; ballPos.y = 65; ballPos.z = endPos.z; } wait_ms(40); break; case PHASE_2: if(slider.read() > 0.9) aimTilt += 0.5; else if(slider.read() < 0.1) aimTilt -= 0.5; if(aimTilt < 0) aimTilt = 0; else if(aimTilt > 90) aimTilt = 90; if(joyX.read() < 0.2) aimAngle -= 1; else if(joyX.read() > 0.8) aimAngle += 1; if(aimAngle < 0) aimAngle = 360; else if(aimAngle > 360) aimAngle = 0; aim(aimAngle, aimTilt); pc.printf("x: %f\r\n",endPos.x); pc.printf("y: %f\r\n",endPos.y); pc.printf("z: %f\r\n",endPos.z); pc.printf("angle: %f\r\n",aimAngle); pc.printf("tilt: %f\r\n",aimTilt); calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions getMotorPositions(); // Calculate current positions moveToTargets(); // Set the motors speeds accordingly wait_ms(40); 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) * 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; // // if(pot.read() < 0.2) setMotor(1,-0.2); // else if(pot.read() > 0.8) setMotor(1,0.2); // else setMotor(1,0); // wait_ms(30); // if(pot.read() > 0.8) setSolenoid(1); // else setSolenoid(0); // moveWithJoystick(); // calculateKinematics(endPos.x, endPos.y, endPos.z); // Calculate target positions // getMotorPositions(); // Calculate current positions // moveToTargets(); // Set the motors speeds accordingly // pc.printf("x: %f\r\n",endPos.x); // pc.printf("y: %f\r\n",endPos.y); // pc.printf("z: %f\r\n",endPos.z); // wait_ms(40); // // for(int i = 0; i < 360; i++) { // aim(i); // wait_ms(100); // pc.printf("angle: %d\r\n",i); // } // aimTilt = slider.read() * 180; // if(joyX.read() < 0.2) aimAngle -= 1; // else if(joyX.read() > 0.8) aimAngle += 1; // aim(aimAngle, aimTilt); // wait_ms(50); // getMotorPositions(); // pc.printf("jx: %f\r\n",joyX.read()); // pc.printf("jy: %f\r\n",joyY.read()); // pc.printf("sli: %d\r\n", joyButton.read()); // wait_ms(100); } }