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@32:2006977785f5, 2015-10-07 (annotated)

Committer:
annesteenbeek
Date:
Wed Oct 07 20:55:28 2015 +0200
Revision:
32:2006977785f5
Parent:
31:8fbee6c92753
Child:
34:f315b2b38555
added buttons and leds for case switching and added servo

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 31:8fbee6c92753 7 #include "servo.h"
annesteenbeek 32:2006977785f5 8 // functions for controlling the motors
annesteenbeek 32:2006977785f5 9 bool motorEnable = true;
annesteenbeek 6:b957d8809e7c 10
annesteenbeek 32:2006977785f5 11 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
annesteenbeek 32:2006977785f5 12 bool direction2 = false;
annesteenbeek 6:b957d8809e7c 13
annesteenbeek 32:2006977785f5 14 float motor1Pos = 0;
annesteenbeek 32:2006977785f5 15 float motor2Pos = 0;
annesteenbeek 6:b957d8809e7c 16
annesteenbeek 32:2006977785f5 17 float motorSpeed1 = 0;
annesteenbeek 32:2006977785f5 18 float motorSpeed2 = 0;
annesteenbeek 6:b957d8809e7c 19
annesteenbeek 32:2006977785f5 20 float motorSetSpeed1 = 0;
annesteenbeek 32:2006977785f5 21 float motorSetSpeed2 = 0;
annesteenbeek 6:b957d8809e7c 22
annesteenbeek 32:2006977785f5 23 float servoPos = 0;
annesteenbeek 6:b957d8809e7c 24
annesteenbeek 32:2006977785f5 25 float motorPWM1 = 0;
annesteenbeek 32:2006977785f5 26 float motorPWM2 = 0;
annesteenbeek 6:b957d8809e7c 27
annesteenbeek 32:2006977785f5 28 // Set PID values
annesteenbeek 32:2006977785f5 29 float Kp1 = 1;
annesteenbeek 32:2006977785f5 30 float Ki1 = 0;
annesteenbeek 32:2006977785f5 31 float Kd1 = 0;
annesteenbeek 0:525558a26464 32
annesteenbeek 32:2006977785f5 33 float Kp2 = 1;
annesteenbeek 32:2006977785f5 34 float Ki2 = 0;
annesteenbeek 32:2006977785f5 35 float Kd2 = 0;
annesteenbeek 20:3cba803cd771 36
annesteenbeek 32:2006977785f5 37 float PIDinterval = 0.2;
annesteenbeek 25:874675516927 38
annesteenbeek 32:2006977785f5 39 float prevMotor2Pos = 0;
annesteenbeek 32:2006977785f5 40 float prevMotor1Pos = 0;
annesteenbeek 32:2006977785f5 41 float prevTime = 0;
annesteenbeek 32:2006977785f5 42 float curTime = 0;
annesteenbeek 26:0a9e4147a31a 43
annesteenbeek 32:2006977785f5 44 // Create object instances
annesteenbeek 32:2006977785f5 45 // Initialze motors
annesteenbeek 32:2006977785f5 46 PwmOut motor1(motor1PWMPin);
annesteenbeek 32:2006977785f5 47 PwmOut motor2(motor2PWMPin);
annesteenbeek 32:2006977785f5 48
annesteenbeek 32:2006977785f5 49 // Initialize encoders
annesteenbeek 32:2006977785f5 50 Encoder encoder1(enc1A, enc1B);
annesteenbeek 32:2006977785f5 51 Encoder encoder2(enc2A, enc2B);
annesteenbeek 25:874675516927 52
annesteenbeek 32:2006977785f5 53 // Set direction pins
annesteenbeek 32:2006977785f5 54 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 32:2006977785f5 55 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 26:0a9e4147a31a 56
annesteenbeek 32:2006977785f5 57 // create PID instances
annesteenbeek 32:2006977785f5 58 PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
annesteenbeek 32:2006977785f5 59 PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
annesteenbeek 31:8fbee6c92753 60
annesteenbeek 32:2006977785f5 61 servo Servo(servoPin);
annesteenbeek 32:2006977785f5 62 Timer t;
annesteenbeek 25:874675516927 63
annesteenbeek 25:874675516927 64 void motorInit(){
annesteenbeek 25:874675516927 65
annesteenbeek 3:47c76be6d402 66 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 67 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 68
annesteenbeek 3:47c76be6d402 69 // Set motor PWM period
annesteenbeek 3:47c76be6d402 70 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 71 motor2.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 72
annesteenbeek 14:0c0d1bfd94ea 73 PIDmotor1.setSetPoint(motorSetSpeed1);
annesteenbeek 14:0c0d1bfd94ea 74 PIDmotor2.setSetPoint(motorSetSpeed2);
annesteenbeek 1:80f098c05d4b 75
annesteenbeek 20:3cba803cd771 76 PIDmotor1.setProcessValue(motorSpeed1);
annesteenbeek 20:3cba803cd771 77 PIDmotor2.setProcessValue(motorSpeed2);
annesteenbeek 30:a20f16bf8dda 78
annesteenbeek 30:a20f16bf8dda 79 // set limits for PID output to avoid integrator build up.
annesteenbeek 31:8fbee6c92753 80 PIDmotor1.setInputLimits(-300, 300);
annesteenbeek 31:8fbee6c92753 81 PIDmotor2.setInputLimits(-300, 300);
annesteenbeek 31:8fbee6c92753 82 PIDmotor1.setOutputLimits(-1, 1);
annesteenbeek 31:8fbee6c92753 83 PIDmotor2.setOutputLimits(-1, 1);
annesteenbeek 30:a20f16bf8dda 84
annesteenbeek 30:a20f16bf8dda 85 // Turn PID on
annesteenbeek 22:c562b9a4176d 86 PIDmotor1.setMode(1);
annesteenbeek 22:c562b9a4176d 87 PIDmotor2.setMode(1);
annesteenbeek 30:a20f16bf8dda 88
annesteenbeek 30:a20f16bf8dda 89 // start the timer
annesteenbeek 29:e4f3455aaa0b 90 t.start();
annesteenbeek 4:80e2280058ed 91 }
annesteenbeek 0:525558a26464 92
annesteenbeek 26:0a9e4147a31a 93
annesteenbeek 0:525558a26464 94 void motorControl(){
annesteenbeek 1:80f098c05d4b 95 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 30:a20f16bf8dda 96 // get encoder positions in degrees
annesteenbeek 30:a20f16bf8dda 97 // 131.25:1 gear ratio
annesteenbeek 30:a20f16bf8dda 98 // getPosition uses X2 configuration, so 32 counts per revolution
annesteenbeek 30:a20f16bf8dda 99 // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
annesteenbeek 30:a20f16bf8dda 100 motor1Pos = -((encoder1.getPosition()/32)/131.25)*360;
annesteenbeek 30:a20f16bf8dda 101 motor2Pos = -((encoder2.getPosition()/32)/131.25)*360;
annesteenbeek 3:47c76be6d402 102
annesteenbeek 0:525558a26464 103 // check if motor's are within rotational boundarys
annesteenbeek 30:a20f16bf8dda 104 // get encoder speeds in deg/sec
annesteenbeek 30:a20f16bf8dda 105 curTime = t.read();
annesteenbeek 29:e4f3455aaa0b 106 motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime);
annesteenbeek 29:e4f3455aaa0b 107 motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime);
annesteenbeek 29:e4f3455aaa0b 108 prevTime = curTime;
annesteenbeek 29:e4f3455aaa0b 109 prevMotor1Pos = motor1Pos;
annesteenbeek 29:e4f3455aaa0b 110 prevMotor2Pos = motor2Pos;
annesteenbeek 29:e4f3455aaa0b 111
annesteenbeek 31:8fbee6c92753 112 // calculate motor setpoint speed in deg/sec from setpoint x/y speed
annesteenbeek 30:a20f16bf8dda 113
annesteenbeek 32:2006977785f5 114 // compute new PID parameters using setpoint angle speeds and encoder speed
annesteenbeek 31:8fbee6c92753 115 writeMotors();
annesteenbeek 2:95ba9f6f0128 116
annesteenbeek 0:525558a26464 117 }else{
annesteenbeek 0:525558a26464 118 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 119 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 120 motor2.write(0);
annesteenbeek 0:525558a26464 121 }
annesteenbeek 0:525558a26464 122 }
annesteenbeek 0:525558a26464 123
annesteenbeek 31:8fbee6c92753 124 void writeMotors(){
annesteenbeek 31:8fbee6c92753 125 PIDmotor1.setSetPoint(motorSetSpeed1);
annesteenbeek 31:8fbee6c92753 126 PIDmotor2.setSetPoint(motorSetSpeed2);
annesteenbeek 31:8fbee6c92753 127
annesteenbeek 31:8fbee6c92753 128 PIDmotor1.setProcessValue(motorSpeed1);
annesteenbeek 31:8fbee6c92753 129 PIDmotor2.setProcessValue(motorSpeed2);
annesteenbeek 31:8fbee6c92753 130
annesteenbeek 31:8fbee6c92753 131 motorPWM1 = PIDmotor1.compute();
annesteenbeek 31:8fbee6c92753 132 motorPWM2 = PIDmotor2.compute();
annesteenbeek 31:8fbee6c92753 133 // write new values to motor's
annesteenbeek 31:8fbee6c92753 134 if (motorPWM1 > 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 135 direction1 = false;
annesteenbeek 31:8fbee6c92753 136 }else{
annesteenbeek 31:8fbee6c92753 137 direction1 = true; // CW rotation
annesteenbeek 31:8fbee6c92753 138 }
annesteenbeek 31:8fbee6c92753 139 if (motorPWM2 > 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 140 direction2 = false;
annesteenbeek 31:8fbee6c92753 141 }else{
annesteenbeek 31:8fbee6c92753 142 direction2 = true; // CW rotation
annesteenbeek 31:8fbee6c92753 143 }
annesteenbeek 31:8fbee6c92753 144 motor1Dir.write(direction1);
annesteenbeek 31:8fbee6c92753 145 motor2Dir.write(direction2);
annesteenbeek 31:8fbee6c92753 146
annesteenbeek 32:2006977785f5 147 motor1.write(abs(motorPWM1));
annesteenbeek 32:2006977785f5 148 motor2.write(abs(motorPWM2));
annesteenbeek 31:8fbee6c92753 149 }
annesteenbeek 31:8fbee6c92753 150
annesteenbeek 0:525558a26464 151 void servoControl(){
annesteenbeek 0:525558a26464 152 // use potMeter Value to set servo angle
annesteenbeek 31:8fbee6c92753 153 Servo.write(servoPos);
annesteenbeek 0:525558a26464 154 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 155 // calculate z position using angle
annesteenbeek 0:525558a26464 156 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 157 // find new x and y speed.
annesteenbeek 0:525558a26464 158
annesteenbeek 25:874675516927 159 }
annesteenbeek 25:874675516927 160
annesteenbeek 32:2006977785f5 161 void calibrateMotors(){
annesteenbeek 32:2006977785f5 162 // bool calibrating1 = true;
annesteenbeek 32:2006977785f5 163 // bool calibrating2 = true;
annesteenbeek 32:2006977785f5 164 // motor1Dir.write(false);
annesteenbeek 32:2006977785f5 165 // motor2Dir.write(false);
annesteenbeek 32:2006977785f5 166 // // move motors CCW until they reach outer most position
annesteenbeek 32:2006977785f5 167 // while (calibrating1 || calibrating2){
annesteenbeek 32:2006977785f5 168 // if(motor1AnglePin == 0){
annesteenbeek 32:2006977785f5 169 // motor1.write(0.3f);
annesteenbeek 32:2006977785f5 170 // }else{
annesteenbeek 32:2006977785f5 171 // motor1.write(0);
annesteenbeek 32:2006977785f5 172 // calibrating1 = false;
annesteenbeek 32:2006977785f5 173 // }
annesteenbeek 32:2006977785f5 174 // if(motor2AnglePin == 0){
annesteenbeek 32:2006977785f5 175 // motor2.write(0.3f);
annesteenbeek 32:2006977785f5 176 // }else{
annesteenbeek 32:2006977785f5 177 // motor2.write(0);
annesteenbeek 32:2006977785f5 178 // calibrating2 = false;
annesteenbeek 32:2006977785f5 179 // }
annesteenbeek 32:2006977785f5 180 // wait(0.2f);
annesteenbeek 32:2006977785f5 181 // }
annesteenbeek 32:2006977785f5 182 // // set the encoder values for angle.
annesteenbeek 32:2006977785f5 183 // encoder1.setValue(0);
annesteenbeek 32:2006977785f5 184 // encoder2.setValue(0);
annesteenbeek 32:2006977785f5 185 }