Dammenisvoorcasuals / Mbed 2 deprecated chessRobot

control for robotic arm that can play chess using a granular gripper

Dependencies:   Encoder mbed HIDScope Servo MODSERIAL

Fork of chessRobot by a steenbeek

actuators.cpp@1:80f098c05d4b, 2015-10-01 (annotated)

Committer:
annesteenbeek
Date:
Thu Oct 01 15:09:24 2015 +0200
Revision:
1:80f098c05d4b
Parent:
0:525558a26464
Child:
2:95ba9f6f0128
Added motor control functions

Who changed what in which revision?

UserRevisionLine numberNew 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 1:80f098c05d4b 16 PIDmotor1.SetOutputLimits(-1f, 1f);
annesteenbeek 1:80f098c05d4b 17 PIDmotor2.SetOutputLimits(-1f, 1f);
annesteenbeek 0:525558a26464 18 }
annesteenbeek 0:525558a26464 19
annesteenbeek 0:525558a26464 20
annesteenbeek 0:525558a26464 21 void motorControl(){
annesteenbeek 1:80f098c05d4b 22 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 23
annesteenbeek 0:525558a26464 24 // get encoder positions
annesteenbeek 0:525558a26464 25 // check if motor's are within rotational boundarys
annesteenbeek 0:525558a26464 26 // calculate encoder speeds
annesteenbeek 0:525558a26464 27 // translate to x/y speed
annesteenbeek 1:80f098c05d4b 28
annesteenbeek 0:525558a26464 29 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 1:80f098c05d4b 30 PIDmotor1.compute();
annesteenbeek 1:80f098c05d4b 31 PIDmotor2.compute();
annesteenbeek 0:525558a26464 32 // write new values to motor's
annesteenbeek 0:525558a26464 33
annesteenbeek 0:525558a26464 34 }else{
annesteenbeek 0:525558a26464 35 // write 0 to motors
annesteenbeek 0:525558a26464 36 }
annesteenbeek 0:525558a26464 37 }
annesteenbeek 0:525558a26464 38
annesteenbeek 0:525558a26464 39 void servoControl(){
annesteenbeek 0:525558a26464 40 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 41 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 42 // calculate z position using angle
annesteenbeek 0:525558a26464 43 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 44 // find new x and y speed.
annesteenbeek 0:525558a26464 45
annesteenbeek 0:525558a26464 46 }
annesteenbeek 0:525558a26464 47
annesteenbeek 0:525558a26464 48 void pumpControl(){
annesteenbeek 0:525558a26464 49 if (pumpButton == HIGH){
annesteenbeek 0:525558a26464 50 // write pumpPin High
annesteenbeek 0:525558a26464 51 }else{
annesteenbeek 0:525558a26464 52 // write pumpPin Low
annesteenbeek 0:525558a26464 53 }
annesteenbeek 0:525558a26464 54 }