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@24:2d7e11441eee, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 21:46:51 2015 +0000
Branch:
onefile
Revision:
24:2d7e11441eee
Parent:
23:3f5d30b4784d 
Moved subfiles into main.cpp

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