control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp@30:a20f16bf8dda, 2015-10-06 (annotated)
- Committer:
- annesteenbeek
- Date:
- Tue Oct 06 21:06:20 2015 +0000
- Revision:
- 30:a20f16bf8dda
- Parent:
- 29:e4f3455aaa0b
- Child:
- 31:8fbee6c92753
divided functions with Tickers and working PID;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
annesteenbeek | 13:4837b36b9a68 | 1 | #include "actuators.h" |
annesteenbeek | 13:4837b36b9a68 | 2 | #include "PID.h" |
annesteenbeek | 13:4837b36b9a68 | 3 | #include "mbed.h" |
annesteenbeek | 13:4837b36b9a68 | 4 | #include "config.h" |
annesteenbeek | 13:4837b36b9a68 | 5 | #include "encoder.h" |
annesteenbeek | 25:874675516927 | 6 | #include "HIDScope.h" |
annesteenbeek | 0:525558a26464 | 7 | // functions for controlling the motors |
annesteenbeek | 26:0a9e4147a31a | 8 | bool motorEnable = true; |
annesteenbeek | 6:b957d8809e7c | 9 | |
annesteenbeek | 6:b957d8809e7c | 10 | bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) |
annesteenbeek | 6:b957d8809e7c | 11 | bool direction2 = false; |
annesteenbeek | 6:b957d8809e7c | 12 | |
annesteenbeek | 14:0c0d1bfd94ea | 13 | float motor1Pos = 0; |
annesteenbeek | 14:0c0d1bfd94ea | 14 | float motor2Pos = 0; |
annesteenbeek | 6:b957d8809e7c | 15 | |
annesteenbeek | 14:0c0d1bfd94ea | 16 | float motorSpeed1 = 0; |
annesteenbeek | 14:0c0d1bfd94ea | 17 | float motorSpeed2 = 0; |
annesteenbeek | 6:b957d8809e7c | 18 | |
annesteenbeek | 14:0c0d1bfd94ea | 19 | float motorSetSpeed1 = 0; |
annesteenbeek | 14:0c0d1bfd94ea | 20 | float motorSetSpeed2 = 0; |
annesteenbeek | 6:b957d8809e7c | 21 | |
annesteenbeek | 6:b957d8809e7c | 22 | |
annesteenbeek | 14:0c0d1bfd94ea | 23 | float motorPWM1 = 0; |
annesteenbeek | 14:0c0d1bfd94ea | 24 | float motorPWM2 = 0; |
annesteenbeek | 6:b957d8809e7c | 25 | |
annesteenbeek | 6:b957d8809e7c | 26 | // Set PID values |
annesteenbeek | 14:0c0d1bfd94ea | 27 | float Kp1 = 1; |
annesteenbeek | 30:a20f16bf8dda | 28 | float Ki1 = 0; |
annesteenbeek | 30:a20f16bf8dda | 29 | float Kd1 = 0; |
annesteenbeek | 6:b957d8809e7c | 30 | |
annesteenbeek | 14:0c0d1bfd94ea | 31 | float Kp2 = 1; |
annesteenbeek | 30:a20f16bf8dda | 32 | float Ki2 = 0; |
annesteenbeek | 30:a20f16bf8dda | 33 | float Kd2 = 0; |
annesteenbeek | 0:525558a26464 | 34 | |
annesteenbeek | 20:3cba803cd771 | 35 | float PIDinterval = 0.2; |
annesteenbeek | 20:3cba803cd771 | 36 | |
annesteenbeek | 29:e4f3455aaa0b | 37 | float prevMotor2Pos = 0; |
annesteenbeek | 29:e4f3455aaa0b | 38 | float prevMotor1Pos = 0; |
annesteenbeek | 29:e4f3455aaa0b | 39 | float prevTime = 0; |
annesteenbeek | 29:e4f3455aaa0b | 40 | float curTime = 0; |
annesteenbeek | 25:874675516927 | 41 | |
annesteenbeek | 30:a20f16bf8dda | 42 | float pidError = 0; |
annesteenbeek | 26:0a9e4147a31a | 43 | // Create object instances |
annesteenbeek | 26:0a9e4147a31a | 44 | // Initialze motors |
annesteenbeek | 26:0a9e4147a31a | 45 | PwmOut motor1(motor1PWMPin); |
annesteenbeek | 26:0a9e4147a31a | 46 | PwmOut motor2(motor2PWMPin); |
annesteenbeek | 26:0a9e4147a31a | 47 | |
annesteenbeek | 26:0a9e4147a31a | 48 | // Initialize encoders |
annesteenbeek | 29:e4f3455aaa0b | 49 | Encoder encoder1(enc1A, enc1B); |
annesteenbeek | 29:e4f3455aaa0b | 50 | Encoder encoder2(enc2A, enc2B); |
annesteenbeek | 25:874675516927 | 51 | |
annesteenbeek | 26:0a9e4147a31a | 52 | // Set direction pins |
annesteenbeek | 26:0a9e4147a31a | 53 | DigitalOut motor1Dir(motor1DirPin); |
annesteenbeek | 26:0a9e4147a31a | 54 | DigitalOut motor2Dir(motor2DirPin); |
annesteenbeek | 26:0a9e4147a31a | 55 | |
annesteenbeek | 26:0a9e4147a31a | 56 | // create PID instances |
annesteenbeek | 26:0a9e4147a31a | 57 | PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); |
annesteenbeek | 26:0a9e4147a31a | 58 | PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); |
annesteenbeek | 29:e4f3455aaa0b | 59 | Timer t; |
annesteenbeek | 25:874675516927 | 60 | |
annesteenbeek | 25:874675516927 | 61 | void motorInit(){ |
annesteenbeek | 25:874675516927 | 62 | |
annesteenbeek | 3:47c76be6d402 | 63 | motor1Dir.write(direction1); |
annesteenbeek | 3:47c76be6d402 | 64 | motor2Dir.write(direction2); |
annesteenbeek | 3:47c76be6d402 | 65 | |
annesteenbeek | 3:47c76be6d402 | 66 | // Set motor PWM period |
annesteenbeek | 3:47c76be6d402 | 67 | motor1.period(1/pwm_frequency); |
annesteenbeek | 3:47c76be6d402 | 68 | motor2.period(1/pwm_frequency); |
annesteenbeek | 3:47c76be6d402 | 69 | |
annesteenbeek | 14:0c0d1bfd94ea | 70 | PIDmotor1.setSetPoint(motorSetSpeed1); |
annesteenbeek | 14:0c0d1bfd94ea | 71 | PIDmotor2.setSetPoint(motorSetSpeed2); |
annesteenbeek | 1:80f098c05d4b | 72 | |
annesteenbeek | 20:3cba803cd771 | 73 | PIDmotor1.setProcessValue(motorSpeed1); |
annesteenbeek | 20:3cba803cd771 | 74 | PIDmotor2.setProcessValue(motorSpeed2); |
annesteenbeek | 30:a20f16bf8dda | 75 | |
annesteenbeek | 30:a20f16bf8dda | 76 | // set limits for PID output to avoid integrator build up. |
annesteenbeek | 30:a20f16bf8dda | 77 | PIDmotor1.setInputLimits(-100, 300); |
annesteenbeek | 30:a20f16bf8dda | 78 | PIDmotor2.setInputLimits(-100,300); |
annesteenbeek | 30:a20f16bf8dda | 79 | PIDmotor1.setOutputLimits(-100, 300); |
annesteenbeek | 30:a20f16bf8dda | 80 | PIDmotor2.setOutputLimits(-100, 300); |
annesteenbeek | 30:a20f16bf8dda | 81 | |
annesteenbeek | 30:a20f16bf8dda | 82 | // Turn PID on |
annesteenbeek | 22:c562b9a4176d | 83 | PIDmotor1.setMode(1); |
annesteenbeek | 22:c562b9a4176d | 84 | PIDmotor2.setMode(1); |
annesteenbeek | 30:a20f16bf8dda | 85 | |
annesteenbeek | 30:a20f16bf8dda | 86 | // start the timer |
annesteenbeek | 29:e4f3455aaa0b | 87 | t.start(); |
annesteenbeek | 4:80e2280058ed | 88 | } |
annesteenbeek | 0:525558a26464 | 89 | |
annesteenbeek | 26:0a9e4147a31a | 90 | |
annesteenbeek | 0:525558a26464 | 91 | void motorControl(){ |
annesteenbeek | 1:80f098c05d4b | 92 | if(motorEnable){ // only run motors if switch is enabled |
annesteenbeek | 1:80f098c05d4b | 93 | |
annesteenbeek | 30:a20f16bf8dda | 94 | // get encoder positions in degrees |
annesteenbeek | 30:a20f16bf8dda | 95 | // 131.25:1 gear ratio |
annesteenbeek | 30:a20f16bf8dda | 96 | // getPosition uses X2 configuration, so 32 counts per revolution |
annesteenbeek | 30:a20f16bf8dda | 97 | // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive |
annesteenbeek | 30:a20f16bf8dda | 98 | motor1Pos = -((encoder1.getPosition()/32)/131.25)*360; |
annesteenbeek | 30:a20f16bf8dda | 99 | motor2Pos = -((encoder2.getPosition()/32)/131.25)*360; |
annesteenbeek | 3:47c76be6d402 | 100 | |
annesteenbeek | 0:525558a26464 | 101 | // check if motor's are within rotational boundarys |
annesteenbeek | 30:a20f16bf8dda | 102 | // get encoder speeds in deg/sec |
annesteenbeek | 30:a20f16bf8dda | 103 | curTime = t.read(); |
annesteenbeek | 29:e4f3455aaa0b | 104 | motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime); |
annesteenbeek | 29:e4f3455aaa0b | 105 | motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime); |
annesteenbeek | 29:e4f3455aaa0b | 106 | prevTime = curTime; |
annesteenbeek | 29:e4f3455aaa0b | 107 | prevMotor1Pos = motor1Pos; |
annesteenbeek | 29:e4f3455aaa0b | 108 | prevMotor2Pos = motor2Pos; |
annesteenbeek | 29:e4f3455aaa0b | 109 | |
annesteenbeek | 29:e4f3455aaa0b | 110 | |
annesteenbeek | 0:525558a26464 | 111 | // translate to x/y speed |
annesteenbeek | 0:525558a26464 | 112 | // compute new PID parameters using setpoint speeds and x/y speeds |
annesteenbeek | 30:a20f16bf8dda | 113 | PIDmotor1.setSetPoint(motorSetSpeed1); |
annesteenbeek | 30:a20f16bf8dda | 114 | PIDmotor2.setSetPoint(motorSetSpeed2); |
annesteenbeek | 30:a20f16bf8dda | 115 | |
annesteenbeek | 30:a20f16bf8dda | 116 | PIDmotor1.setProcessValue(motorSpeed1); |
annesteenbeek | 30:a20f16bf8dda | 117 | PIDmotor2.setProcessValue(motorSpeed2); |
annesteenbeek | 30:a20f16bf8dda | 118 | |
annesteenbeek | 14:0c0d1bfd94ea | 119 | motorPWM1 = PIDmotor1.compute(); |
annesteenbeek | 14:0c0d1bfd94ea | 120 | motorPWM2 = PIDmotor2.compute(); |
annesteenbeek | 2:95ba9f6f0128 | 121 | // translate to motor rotation speed |
annesteenbeek | 0:525558a26464 | 122 | // write new values to motor's |
annesteenbeek | 30:a20f16bf8dda | 123 | if (motorPWM1 > 0 ){ // CCW rotation |
annesteenbeek | 5:73bfad06b775 | 124 | direction1 = false; |
annesteenbeek | 5:73bfad06b775 | 125 | }else{ |
annesteenbeek | 5:73bfad06b775 | 126 | direction1 = true; // CW rotation |
annesteenbeek | 5:73bfad06b775 | 127 | } |
annesteenbeek | 30:a20f16bf8dda | 128 | if (motorPWM2 > 0 ){ // CCW rotation |
annesteenbeek | 5:73bfad06b775 | 129 | direction2 = false; |
annesteenbeek | 5:73bfad06b775 | 130 | }else{ |
annesteenbeek | 5:73bfad06b775 | 131 | direction2 = true; // CW rotation |
annesteenbeek | 5:73bfad06b775 | 132 | } |
annesteenbeek | 30:a20f16bf8dda | 133 | motor1Dir.write(direction1); |
annesteenbeek | 30:a20f16bf8dda | 134 | motor2Dir.write(direction2); |
annesteenbeek | 30:a20f16bf8dda | 135 | |
annesteenbeek | 30:a20f16bf8dda | 136 | motor1.write(abs(motorPWM1)/300); |
annesteenbeek | 30:a20f16bf8dda | 137 | motor2.write(abs(motorPWM2)/300); |
annesteenbeek | 30:a20f16bf8dda | 138 | // motor2.write(motorSetSpeed2); |
annesteenbeek | 30:a20f16bf8dda | 139 | pidError = PIDmotor2.getError(); |
annesteenbeek | 2:95ba9f6f0128 | 140 | |
annesteenbeek | 0:525558a26464 | 141 | }else{ |
annesteenbeek | 0:525558a26464 | 142 | // write 0 to motors |
annesteenbeek | 2:95ba9f6f0128 | 143 | motor1.write(0); |
annesteenbeek | 2:95ba9f6f0128 | 144 | motor2.write(0); |
annesteenbeek | 0:525558a26464 | 145 | } |
annesteenbeek | 0:525558a26464 | 146 | } |
annesteenbeek | 0:525558a26464 | 147 | |
annesteenbeek | 0:525558a26464 | 148 | void servoControl(){ |
annesteenbeek | 0:525558a26464 | 149 | // use potMeter Value to set servo angle |
annesteenbeek | 0:525558a26464 | 150 | // (optionaly calculate xy position to keep balloon in position) |
annesteenbeek | 0:525558a26464 | 151 | // calculate z position using angle |
annesteenbeek | 0:525558a26464 | 152 | // calculate x y translation of endpoint |
annesteenbeek | 0:525558a26464 | 153 | // find new x and y speed. |
annesteenbeek | 0:525558a26464 | 154 | |
annesteenbeek | 25:874675516927 | 155 | } |
annesteenbeek | 25:874675516927 | 156 |