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

Committer:
annesteenbeek
Date:
Tue Oct 06 21:06:20 2015 +0000
Revision:
30:a20f16bf8dda
Parent:
29:e4f3455aaa0b
Child:
31:8fbee6c92753
divided functions with Tickers and working PID;

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