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

main.cpp@23:3f5d30b4784d, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 23:10:31 2015 +0200
Branch:
onefile
Revision:
23:3f5d30b4784d
Parent:
15:5fa388ba22cb
Child:
24:2d7e11441eee
new branch and moved functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
annesteenbeek 12:61759f94c07a 1 #include "mbed.h"
annesteenbeek 13:4837b36b9a68 2 #include "config.h" // settings and pin configurations
annesteenbeek 23:3f5d30b4784d 3 #include "encoder.h"
annesteenbeek 23:3f5d30b4784d 4 #include "PID.h"
annesteenbeek 15:5fa388ba22cb 5 #include "EMG.h"
annesteenbeek 12:61759f94c07a 6
annesteenbeek 12:61759f94c07a 7 //#define DEBUG // send debug data to HIDScope
annesteenbeek 6:b957d8809e7c 8 #ifdef DEBUG
annesteenbeek 15:5fa388ba22cb 9 #include "debug.h"
annesteenbeek 6:b957d8809e7c 10 dubugInit();
annesteenbeek 6:b957d8809e7c 11 #endif
annesteenbeek 6:b957d8809e7c 12
annesteenbeek 23:3f5d30b4784d 13 bool motorEnable = false;
annesteenbeek 23:3f5d30b4784d 14
annesteenbeek 23:3f5d30b4784d 15 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
annesteenbeek 23:3f5d30b4784d 16 bool direction2 = false;
annesteenbeek 23:3f5d30b4784d 17
annesteenbeek 23:3f5d30b4784d 18 float motor1Pos = 0;
annesteenbeek 23:3f5d30b4784d 19 float motor2Pos = 0;
annesteenbeek 23:3f5d30b4784d 20
annesteenbeek 23:3f5d30b4784d 21 float motorSpeed1 = 0;
annesteenbeek 23:3f5d30b4784d 22 float motorSpeed2 = 0;
annesteenbeek 23:3f5d30b4784d 23
annesteenbeek 23:3f5d30b4784d 24 float motorSetSpeed1 = 0;
annesteenbeek 23:3f5d30b4784d 25 float motorSetSpeed2 = 0;
annesteenbeek 23:3f5d30b4784d 26
annesteenbeek 23:3f5d30b4784d 27
annesteenbeek 23:3f5d30b4784d 28 float motorPWM1 = 0;
annesteenbeek 23:3f5d30b4784d 29 float motorPWM2 = 0;
annesteenbeek 23:3f5d30b4784d 30
annesteenbeek 23:3f5d30b4784d 31 // Set PID values
annesteenbeek 23:3f5d30b4784d 32 float Kp1 = 1;
annesteenbeek 23:3f5d30b4784d 33 float Ki1 = 1;
annesteenbeek 23:3f5d30b4784d 34 float Kd1 = 1;
annesteenbeek 23:3f5d30b4784d 35
annesteenbeek 23:3f5d30b4784d 36 float Kp2 = 1;
annesteenbeek 23:3f5d30b4784d 37 float Ki2 = 1;
annesteenbeek 23:3f5d30b4784d 38 float Kd2 = 1;
annesteenbeek 23:3f5d30b4784d 39
annesteenbeek 23:3f5d30b4784d 40 float PIDinterval = 0.2;
annesteenbeek 23:3f5d30b4784d 41
annesteenbeek 12:61759f94c07a 42
annesteenbeek 0:525558a26464 43
annesteenbeek 5:73bfad06b775 44 int main(){
annesteenbeek 12:61759f94c07a 45 setPins();
annesteenbeek 12:61759f94c07a 46 motorInit();
annesteenbeek 0:525558a26464 47 while (true) {
annesteenbeek 23:3f5d30b4784d 48 checkSwitches();
annesteenbeek 3:47c76be6d402 49 // readEMG();
annesteenbeek 0:525558a26464 50 motorControl();
annesteenbeek 3:47c76be6d402 51 // servoControl();
annesteenbeek 0:525558a26464 52 }
annesteenbeek 23:3f5d30b4784d 53 }
annesteenbeek 23:3f5d30b4784d 54
annesteenbeek 23:3f5d30b4784d 55
annesteenbeek 23:3f5d30b4784d 56 void motorInit(){
annesteenbeek 23:3f5d30b4784d 57 // Initialze motors
annesteenbeek 23:3f5d30b4784d 58 PwmOut motor1(motor1PWMPin);
annesteenbeek 23:3f5d30b4784d 59 PwmOut motor2(motor2PWMPin);
annesteenbeek 23:3f5d30b4784d 60
annesteenbeek 23:3f5d30b4784d 61 // Set motor direction pins.
annesteenbeek 23:3f5d30b4784d 62 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 23:3f5d30b4784d 63 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 23:3f5d30b4784d 64
annesteenbeek 23:3f5d30b4784d 65 // Set initial direction
annesteenbeek 23:3f5d30b4784d 66 motor1Dir.write(direction1);
annesteenbeek 23:3f5d30b4784d 67 motor2Dir.write(direction2);
annesteenbeek 23:3f5d30b4784d 68
annesteenbeek 23:3f5d30b4784d 69 // Set motor PWM period
annesteenbeek 23:3f5d30b4784d 70 motor1.period(1/pwm_frequency);
annesteenbeek 23:3f5d30b4784d 71 motor2.period(1/pwm_frequency);
annesteenbeek 23:3f5d30b4784d 72
annesteenbeek 23:3f5d30b4784d 73 // Initialize encoders (with speed calculation)
annesteenbeek 23:3f5d30b4784d 74 Encoder encoder1(enc1A, enc1B, true);
annesteenbeek 23:3f5d30b4784d 75 Encoder encoder2(enc2A, enc2B, true);
annesteenbeek 23:3f5d30b4784d 76
annesteenbeek 23:3f5d30b4784d 77 initPID();
annesteenbeek 23:3f5d30b4784d 78 }
annesteenbeek 23:3f5d30b4784d 79
annesteenbeek 23:3f5d30b4784d 80 void initPID(){
annesteenbeek 23:3f5d30b4784d 81 // create PID instances for motors
annesteenbeek 23:3f5d30b4784d 82 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 23:3f5d30b4784d 83 PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
annesteenbeek 23:3f5d30b4784d 84 PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
annesteenbeek 23:3f5d30b4784d 85 PIDmotor1.setSetPoint(motorSetSpeed1);
annesteenbeek 23:3f5d30b4784d 86 PIDmotor2.setSetPoint(motorSetSpeed2);
annesteenbeek 23:3f5d30b4784d 87
annesteenbeek 23:3f5d30b4784d 88 PIDmotor1.setProcessValue(motorSpeed1);
annesteenbeek 23:3f5d30b4784d 89 PIDmotor2.setProcessValue(motorSpeed2);
annesteenbeek 23:3f5d30b4784d 90 // set PID mode
annesteenbeek 23:3f5d30b4784d 91 PIDmotor1.setMode(1);
annesteenbeek 23:3f5d30b4784d 92 PIDmotor2.setMode(1);
annesteenbeek 23:3f5d30b4784d 93
annesteenbeek 23:3f5d30b4784d 94 // set limits for PID output to avoid integrator build up.
annesteenbeek 23:3f5d30b4784d 95 PIDmotor1.setOutputLimits(-1.0, 1.0);
annesteenbeek 23:3f5d30b4784d 96 PIDmotor2.setOutputLimits(-1.0, 1.0);
annesteenbeek 23:3f5d30b4784d 97 }
annesteenbeek 23:3f5d30b4784d 98
annesteenbeek 23:3f5d30b4784d 99
annesteenbeek 23:3f5d30b4784d 100 void motorControl(){
annesteenbeek 23:3f5d30b4784d 101 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 23:3f5d30b4784d 102
annesteenbeek 23:3f5d30b4784d 103 // get encoder positions
annesteenbeek 23:3f5d30b4784d 104 motor1Pos = encoder1.getPosition();
annesteenbeek 23:3f5d30b4784d 105 motor2Pos = encoder2.getPosition();
annesteenbeek 23:3f5d30b4784d 106
annesteenbeek 23:3f5d30b4784d 107 // check if motor's are within rotational boundarys
annesteenbeek 23:3f5d30b4784d 108 // get encoder speeds
annesteenbeek 23:3f5d30b4784d 109 motorSpeed1 = encoder1.getSpeed();
annesteenbeek 23:3f5d30b4784d 110 motorSpeed2 = encoder2.getSpeed();
annesteenbeek 23:3f5d30b4784d 111
annesteenbeek 23:3f5d30b4784d 112 // translate to x/y speed
annesteenbeek 23:3f5d30b4784d 113 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 23:3f5d30b4784d 114 motorPWM1 = PIDmotor1.compute();
annesteenbeek 23:3f5d30b4784d 115 motorPWM2 = PIDmotor2.compute();
annesteenbeek 23:3f5d30b4784d 116 // translate to motor rotation speed
annesteenbeek 23:3f5d30b4784d 117 // write new values to motor's
annesteenbeek 23:3f5d30b4784d 118 if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 23:3f5d30b4784d 119 direction1 = false;
annesteenbeek 23:3f5d30b4784d 120 }else{
annesteenbeek 23:3f5d30b4784d 121 direction1 = true; // CW rotation
annesteenbeek 23:3f5d30b4784d 122 }
annesteenbeek 23:3f5d30b4784d 123 if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 23:3f5d30b4784d 124 direction2 = false;
annesteenbeek 23:3f5d30b4784d 125 }else{
annesteenbeek 23:3f5d30b4784d 126 direction2 = true; // CW rotation
annesteenbeek 23:3f5d30b4784d 127 }
annesteenbeek 23:3f5d30b4784d 128 motor1.write(abs(motorPWM1));
annesteenbeek 23:3f5d30b4784d 129 motor2.write(abs(motorPWM2));
annesteenbeek 23:3f5d30b4784d 130
annesteenbeek 23:3f5d30b4784d 131 }else{
annesteenbeek 23:3f5d30b4784d 132 // write 0 to motors
annesteenbeek 23:3f5d30b4784d 133 motor1.write(0);
annesteenbeek 23:3f5d30b4784d 134 motor2.write(0);
annesteenbeek 23:3f5d30b4784d 135 }
annesteenbeek 23:3f5d30b4784d 136 }
annesteenbeek 23:3f5d30b4784d 137
annesteenbeek 23:3f5d30b4784d 138 void setPins(){
annesteenbeek 23:3f5d30b4784d 139 // set input/output pins
annesteenbeek 23:3f5d30b4784d 140 AnalogIn pot1(pot1Pin);
annesteenbeek 23:3f5d30b4784d 141 }
annesteenbeek 23:3f5d30b4784d 142
annesteenbeek 23:3f5d30b4784d 143
annesteenbeek 23:3f5d30b4784d 144 void checkSwitches(){
annesteenbeek 23:3f5d30b4784d 145 // read motor enable switch
annesteenbeek 23:3f5d30b4784d 146
annesteenbeek 23:3f5d30b4784d 147 // read pump enable switch
annesteenbeek 23:3f5d30b4784d 148
annesteenbeek 23:3f5d30b4784d 149 // read servo potmeter position
annesteenbeek 23:3f5d30b4784d 150
annesteenbeek 23:3f5d30b4784d 151 // read x speed potmeter position
annesteenbeek 23:3f5d30b4784d 152 float motorSetSpeed1 = pot1.read();
annesteenbeek 23:3f5d30b4784d 153
annesteenbeek 23:3f5d30b4784d 154 // read y speed potmeter position
annesteenbeek 23:3f5d30b4784d 155
annesteenbeek 23:3f5d30b4784d 156 // read killswitches
annesteenbeek 23:3f5d30b4784d 157
annesteenbeek 23:3f5d30b4784d 158 }