control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp@2:95ba9f6f0128, 2015-10-01 (annotated)
- Committer:
- annesteenbeek
- Date:
- Thu Oct 01 15:36:55 2015 +0200
- Revision:
- 2:95ba9f6f0128
- Parent:
- 1:80f098c05d4b
- Child:
- 3:47c76be6d402
Changed motor configuration
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
annesteenbeek | 0:525558a26464 | 1 | // functions for controlling the motors |
annesteenbeek | 0:525558a26464 | 2 | |
annesteenbeek | 0:525558a26464 | 3 | void motorInit(){ |
annesteenbeek | 1:80f098c05d4b | 4 | PwmOut motor1(motorpin1); |
annesteenbeek | 1:80f098c05d4b | 5 | PwmOut motor2(motorpin2); |
annesteenbeek | 1:80f098c05d4b | 6 | |
annesteenbeek | 1:80f098c05d4b | 7 | // create PID instances for motors |
annesteenbeek | 1:80f098c05d4b | 8 | // PID pidname(input, output, setpoint, kp, ki, kd, direction) |
annesteenbeek | 1:80f098c05d4b | 9 | PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT); |
annesteenbeek | 1:80f098c05d4b | 10 | PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT); |
annesteenbeek | 1:80f098c05d4b | 11 | |
annesteenbeek | 1:80f098c05d4b | 12 | // set PID mode |
annesteenbeek | 1:80f098c05d4b | 13 | PIDmotor1.SetMode(AUTOMATIC); |
annesteenbeek | 1:80f098c05d4b | 14 | PIDmotor2.SetMode(AUTOMATIC); |
annesteenbeek | 1:80f098c05d4b | 15 | |
annesteenbeek | 2:95ba9f6f0128 | 16 | // set limits for PID output to avoid integrator build up. |
annesteenbeek | 1:80f098c05d4b | 17 | PIDmotor1.SetOutputLimits(-1f, 1f); |
annesteenbeek | 1:80f098c05d4b | 18 | PIDmotor2.SetOutputLimits(-1f, 1f); |
annesteenbeek | 0:525558a26464 | 19 | } |
annesteenbeek | 0:525558a26464 | 20 | |
annesteenbeek | 0:525558a26464 | 21 | |
annesteenbeek | 0:525558a26464 | 22 | void motorControl(){ |
annesteenbeek | 1:80f098c05d4b | 23 | if(motorEnable){ // only run motors if switch is enabled |
annesteenbeek | 1:80f098c05d4b | 24 | |
annesteenbeek | 0:525558a26464 | 25 | // get encoder positions |
annesteenbeek | 0:525558a26464 | 26 | // check if motor's are within rotational boundarys |
annesteenbeek | 0:525558a26464 | 27 | // calculate encoder speeds |
annesteenbeek | 2:95ba9f6f0128 | 28 | motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime); |
annesteenbeek | 2:95ba9f6f0128 | 29 | motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime); |
annesteenbeek | 2:95ba9f6f0128 | 30 | |
annesteenbeek | 2:95ba9f6f0128 | 31 | // store current positions and time |
annesteenbeek | 2:95ba9f6f0128 | 32 | prevMotorPos1 = motorPos1; |
annesteenbeek | 2:95ba9f6f0128 | 33 | prevMotorPos2 = motorPos2; |
annesteenbeek | 2:95ba9f6f0128 | 34 | prevTime = time.read(); |
annesteenbeek | 0:525558a26464 | 35 | // translate to x/y speed |
annesteenbeek | 0:525558a26464 | 36 | // compute new PID parameters using setpoint speeds and x/y speeds |
annesteenbeek | 1:80f098c05d4b | 37 | PIDmotor1.compute(); |
annesteenbeek | 1:80f098c05d4b | 38 | PIDmotor2.compute(); |
annesteenbeek | 2:95ba9f6f0128 | 39 | // translate to motor rotation speed |
annesteenbeek | 0:525558a26464 | 40 | // write new values to motor's |
annesteenbeek | 2:95ba9f6f0128 | 41 | motor1.write(motorPWM1); |
annesteenbeek | 2:95ba9f6f0128 | 42 | motor2.write(motorPWM2); |
annesteenbeek | 2:95ba9f6f0128 | 43 | |
annesteenbeek | 0:525558a26464 | 44 | }else{ |
annesteenbeek | 0:525558a26464 | 45 | // write 0 to motors |
annesteenbeek | 2:95ba9f6f0128 | 46 | motor1.write(0); |
annesteenbeek | 2:95ba9f6f0128 | 47 | motor2.write(0); |
annesteenbeek | 0:525558a26464 | 48 | } |
annesteenbeek | 0:525558a26464 | 49 | } |
annesteenbeek | 0:525558a26464 | 50 | |
annesteenbeek | 0:525558a26464 | 51 | void servoControl(){ |
annesteenbeek | 0:525558a26464 | 52 | // use potMeter Value to set servo angle |
annesteenbeek | 0:525558a26464 | 53 | // (optionaly calculate xy position to keep balloon in position) |
annesteenbeek | 0:525558a26464 | 54 | // calculate z position using angle |
annesteenbeek | 0:525558a26464 | 55 | // calculate x y translation of endpoint |
annesteenbeek | 0:525558a26464 | 56 | // find new x and y speed. |
annesteenbeek | 0:525558a26464 | 57 | |
annesteenbeek | 0:525558a26464 | 58 | } |
annesteenbeek | 0:525558a26464 | 59 | |
annesteenbeek | 0:525558a26464 | 60 | void pumpControl(){ |
annesteenbeek | 0:525558a26464 | 61 | if (pumpButton == HIGH){ |
annesteenbeek | 0:525558a26464 | 62 | // write pumpPin High |
annesteenbeek | 0:525558a26464 | 63 | }else{ |
annesteenbeek | 0:525558a26464 | 64 | // write pumpPin Low |
annesteenbeek | 0:525558a26464 | 65 | } |
annesteenbeek | 0:525558a26464 | 66 | } |