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@25:874675516927, 2015-10-06 (annotated)

Committer:
annesteenbeek
Date:
Tue Oct 06 14:24:40 2015 +0000
Revision:
25:874675516927
Parent:
22:c562b9a4176d
Child:
26:0a9e4147a31a
correct header file pointer declaration. Buildable code.

Who changed what in which revision?

UserRevisionLine numberNew 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 6:b957d8809e7c 8 bool motorEnable = false;
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 14:0c0d1bfd94ea 28 float Ki1 = 1;
annesteenbeek 14:0c0d1bfd94ea 29 float Kd1 = 1;
annesteenbeek 6:b957d8809e7c 30
annesteenbeek 14:0c0d1bfd94ea 31 float Kp2 = 1;
annesteenbeek 14:0c0d1bfd94ea 32 float Ki2 = 1;
annesteenbeek 14:0c0d1bfd94ea 33 float Kd2 = 1;
annesteenbeek 0:525558a26464 34
annesteenbeek 20:3cba803cd771 35 float PIDinterval = 0.2;
annesteenbeek 20:3cba803cd771 36
annesteenbeek 25:874675516927 37
annesteenbeek 25:874675516927 38 Encoder encoder1(enc1A, enc1B, true);
annesteenbeek 25:874675516927 39 Encoder encoder2(enc2A, enc2B, true);
annesteenbeek 25:874675516927 40
annesteenbeek 3:47c76be6d402 41 PwmOut motor1(motor1PWMPin);
annesteenbeek 3:47c76be6d402 42 PwmOut motor2(motor2PWMPin);
annesteenbeek 25:874675516927 43
annesteenbeek 25:874675516927 44 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 25:874675516927 45 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 25:874675516927 46 PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
annesteenbeek 25:874675516927 47 PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
annesteenbeek 25:874675516927 48
annesteenbeek 25:874675516927 49
annesteenbeek 25:874675516927 50 void motorInit(){
annesteenbeek 25:874675516927 51 // Initialze motors
annesteenbeek 3:47c76be6d402 52
annesteenbeek 25:874675516927 53
annesteenbeek 25:874675516927 54 // Initialize encoders (with speed calculation)
annesteenbeek 25:874675516927 55
annesteenbeek 3:47c76be6d402 56
annesteenbeek 25:874675516927 57 // Set motor direction pins.
annesteenbeek 25:874675516927 58
annesteenbeek 25:874675516927 59
annesteenbeek 3:47c76be6d402 60 // Set initial direction
annesteenbeek 3:47c76be6d402 61 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 62 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 63
annesteenbeek 3:47c76be6d402 64 // Set motor PWM period
annesteenbeek 3:47c76be6d402 65 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 66 motor2.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 67
annesteenbeek 25:874675516927 68
annesteenbeek 1:80f098c05d4b 69
annesteenbeek 4:80e2280058ed 70 initPID();
annesteenbeek 4:80e2280058ed 71 }
annesteenbeek 4:80e2280058ed 72
annesteenbeek 4:80e2280058ed 73 void initPID(){
annesteenbeek 5:73bfad06b775 74 // create PID instances for motors
annesteenbeek 5:73bfad06b775 75 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 25:874675516927 76
annesteenbeek 25:874675516927 77
annesteenbeek 14:0c0d1bfd94ea 78 PIDmotor1.setSetPoint(motorSetSpeed1);
annesteenbeek 14:0c0d1bfd94ea 79 PIDmotor2.setSetPoint(motorSetSpeed2);
annesteenbeek 1:80f098c05d4b 80
annesteenbeek 20:3cba803cd771 81 PIDmotor1.setProcessValue(motorSpeed1);
annesteenbeek 20:3cba803cd771 82 PIDmotor2.setProcessValue(motorSpeed2);
annesteenbeek 1:80f098c05d4b 83 // set PID mode
annesteenbeek 22:c562b9a4176d 84 PIDmotor1.setMode(1);
annesteenbeek 22:c562b9a4176d 85 PIDmotor2.setMode(1);
annesteenbeek 1:80f098c05d4b 86
annesteenbeek 2:95ba9f6f0128 87 // set limits for PID output to avoid integrator build up.
annesteenbeek 21:3e6ebec86438 88 PIDmotor1.setOutputLimits(-1.0, 1.0);
annesteenbeek 21:3e6ebec86438 89 PIDmotor2.setOutputLimits(-1.0, 1.0);
annesteenbeek 4:80e2280058ed 90 }
annesteenbeek 0:525558a26464 91
annesteenbeek 0:525558a26464 92 void motorControl(){
annesteenbeek 1:80f098c05d4b 93 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 94
annesteenbeek 0:525558a26464 95 // get encoder positions
annesteenbeek 3:47c76be6d402 96 motor1Pos = encoder1.getPosition();
annesteenbeek 3:47c76be6d402 97 motor2Pos = encoder2.getPosition();
annesteenbeek 3:47c76be6d402 98
annesteenbeek 0:525558a26464 99 // check if motor's are within rotational boundarys
annesteenbeek 4:80e2280058ed 100 // get encoder speeds
annesteenbeek 4:80e2280058ed 101 motorSpeed1 = encoder1.getSpeed();
annesteenbeek 4:80e2280058ed 102 motorSpeed2 = encoder2.getSpeed();
annesteenbeek 2:95ba9f6f0128 103
annesteenbeek 0:525558a26464 104 // translate to x/y speed
annesteenbeek 0:525558a26464 105 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 14:0c0d1bfd94ea 106 motorPWM1 = PIDmotor1.compute();
annesteenbeek 14:0c0d1bfd94ea 107 motorPWM2 = PIDmotor2.compute();
annesteenbeek 2:95ba9f6f0128 108 // translate to motor rotation speed
annesteenbeek 0:525558a26464 109 // write new values to motor's
annesteenbeek 5:73bfad06b775 110 if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 111 direction1 = false;
annesteenbeek 5:73bfad06b775 112 }else{
annesteenbeek 5:73bfad06b775 113 direction1 = true; // CW rotation
annesteenbeek 5:73bfad06b775 114 }
annesteenbeek 5:73bfad06b775 115 if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 116 direction2 = false;
annesteenbeek 5:73bfad06b775 117 }else{
annesteenbeek 5:73bfad06b775 118 direction2 = true; // CW rotation
annesteenbeek 5:73bfad06b775 119 }
annesteenbeek 5:73bfad06b775 120 motor1.write(abs(motorPWM1));
annesteenbeek 5:73bfad06b775 121 motor2.write(abs(motorPWM2));
annesteenbeek 2:95ba9f6f0128 122
annesteenbeek 0:525558a26464 123 }else{
annesteenbeek 0:525558a26464 124 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 125 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 126 motor2.write(0);
annesteenbeek 0:525558a26464 127 }
annesteenbeek 0:525558a26464 128 }
annesteenbeek 0:525558a26464 129
annesteenbeek 0:525558a26464 130 void servoControl(){
annesteenbeek 0:525558a26464 131 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 132 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 133 // calculate z position using angle
annesteenbeek 0:525558a26464 134 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 135 // find new x and y speed.
annesteenbeek 0:525558a26464 136
annesteenbeek 25:874675516927 137 }
annesteenbeek 25:874675516927 138