control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp@52:2ac9dee099ce, 2015-10-09 (annotated)
- Committer:
- annesteenbeek
- Date:
- Fri Oct 09 13:42:41 2015 +0200
- Revision:
- 52:2ac9dee099ce
- Parent:
- 51:1e6334b993a3
- Child:
- 54:c14c3bc48b8a
changed motor parameters to more constant names, implemented personal PID
Who changed what in which revision?
User | Revision | Line number | New 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 | 34:f315b2b38555 | 7 | #include "Servo.h" |
annesteenbeek |
32:2006977785f5 | 8 | // functions for controlling the motors |
annesteenbeek | 34:f315b2b38555 | 9 | bool motorsEnable = false; |
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 | 52:2ac9dee099ce | 17 | float motor1Speed = 0; |
annesteenbeek | 52:2ac9dee099ce | 18 | float motor2Speed = 0; |
annesteenbeek | 6:b957d8809e7c | 19 | |
annesteenbeek | 52:2ac9dee099ce | 20 | float motor1SetSpeed = 0; |
annesteenbeek | 52:2ac9dee099ce | 21 | float motor2SetSpeed = 0; |
annesteenbeek | 6:b957d8809e7c | 22 | |
annesteenbeek |
32:2006977785f5 | 23 | float servoPos = 0; |
annesteenbeek | 6:b957d8809e7c | 24 | |
annesteenbeek | 52:2ac9dee099ce | 25 | float motor1PWM = 0; |
annesteenbeek | 52:2ac9dee099ce | 26 | float motor2PWM = 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 | 52:2ac9dee099ce | 37 | float motor1PrevPos = 0; |
annesteenbeek | 52:2ac9dee099ce | 38 | float motor2PrevPos = 0; |
annesteenbeek |
32:2006977785f5 | 39 | float prevTime = 0; |
annesteenbeek | 50:b0cf07ca53cf | 40 | float now = 0; |
annesteenbeek | 50:b0cf07ca53cf | 41 | float timechange; |
annesteenbeek | 26:0a9e4147a31a | 42 | |
annesteenbeek |
32:2006977785f5 | 43 | // Create object instances |
annesteenbeek |
32:2006977785f5 | 44 | // Initialze motors |
annesteenbeek |
32:2006977785f5 | 45 | PwmOut motor1(motor1PWMPin); |
annesteenbeek |
32:2006977785f5 | 46 | PwmOut motor2(motor2PWMPin); |
annesteenbeek |
32:2006977785f5 | 47 | |
annesteenbeek |
32:2006977785f5 | 48 | // Initialize encoders |
annesteenbeek |
32:2006977785f5 | 49 | Encoder encoder1(enc1A, enc1B); |
annesteenbeek |
32:2006977785f5 | 50 | Encoder encoder2(enc2A, enc2B); |
annesteenbeek | 25:874675516927 | 51 | |
annesteenbeek |
32:2006977785f5 | 52 | // Set direction pins |
annesteenbeek |
32:2006977785f5 | 53 | DigitalOut motor1Dir(motor1DirPin); |
annesteenbeek |
32:2006977785f5 | 54 | DigitalOut motor2Dir(motor2DirPin); |
annesteenbeek | 26:0a9e4147a31a | 55 | |
annesteenbeek |
32:2006977785f5 | 56 | // create PID instances |
annesteenbeek | 52:2ac9dee099ce | 57 | PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); |
annesteenbeek | 52:2ac9dee099ce | 58 | PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); |
annesteenbeek | 31:8fbee6c92753 | 59 | |
annesteenbeek | 34:f315b2b38555 | 60 | Servo servo(servoPin); |
annesteenbeek |
32:2006977785f5 | 61 | Timer t; |
annesteenbeek | 25:874675516927 | 62 | |
annesteenbeek | 25:874675516927 | 63 | void motorInit(){ |
annesteenbeek | 25:874675516927 | 64 | |
annesteenbeek | 3:47c76be6d402 | 65 | motor1Dir.write(direction1); |
annesteenbeek | 3:47c76be6d402 | 66 | motor2Dir.write(direction2); |
annesteenbeek | 3:47c76be6d402 | 67 | |
annesteenbeek | 3:47c76be6d402 | 68 | // Set motor PWM period |
annesteenbeek | 3:47c76be6d402 | 69 | motor1.period(1/pwm_frequency); |
annesteenbeek | 3:47c76be6d402 | 70 | motor2.period(1/pwm_frequency); |
annesteenbeek | 3:47c76be6d402 | 71 | |
annesteenbeek | 30:a20f16bf8dda | 72 | // Turn PID on |
annesteenbeek | 52:2ac9dee099ce | 73 | motor1PID.SetMode(1); |
annesteenbeek | 52:2ac9dee099ce | 74 | motor2PID.SetMode(1); |
annesteenbeek | 30:a20f16bf8dda | 75 | |
annesteenbeek | 30:a20f16bf8dda | 76 | // start the timer |
annesteenbeek | 29:e4f3455aaa0b | 77 | t.start(); |
annesteenbeek | 4:80e2280058ed | 78 | } |
annesteenbeek | 0:525558a26464 | 79 | |
annesteenbeek | 26:0a9e4147a31a | 80 | |
annesteenbeek | 0:525558a26464 | 81 | void motorControl(){ |
annesteenbeek | 30:a20f16bf8dda | 82 | // get encoder positions in degrees |
annesteenbeek | 30:a20f16bf8dda | 83 | // 131.25:1 gear ratio |
annesteenbeek | 30:a20f16bf8dda | 84 | // getPosition uses X2 configuration, so 32 counts per revolution |
annesteenbeek | 30:a20f16bf8dda | 85 | // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive |
annesteenbeek | 30:a20f16bf8dda | 86 | motor1Pos = -((encoder1.getPosition()/32)/131.25)*360; |
annesteenbeek | 30:a20f16bf8dda | 87 | motor2Pos = -((encoder2.getPosition()/32)/131.25)*360; |
annesteenbeek | 3:47c76be6d402 | 88 | |
annesteenbeek | 0:525558a26464 | 89 | // check if motor's are within rotational boundarys |
annesteenbeek | 30:a20f16bf8dda | 90 | // get encoder speeds in deg/sec |
annesteenbeek | 50:b0cf07ca53cf | 91 | now = t.read(); |
annesteenbeek | 50:b0cf07ca53cf | 92 | timechange = (now - prevTime); |
annesteenbeek | 52:2ac9dee099ce | 93 | motor1Speed = (motor1Pos - motor1PrevPos)/timechange; |
annesteenbeek | 52:2ac9dee099ce | 94 | motor2Speed = (motor2Pos - motor2PrevPos)/timechange; |
annesteenbeek | 50:b0cf07ca53cf | 95 | prevTime = now; |
annesteenbeek | 52:2ac9dee099ce | 96 | motor1PrevPos = motor1Pos; |
annesteenbeek | 52:2ac9dee099ce | 97 | motor2PrevPos = motor2Pos; |
annesteenbeek | 29:e4f3455aaa0b | 98 | |
annesteenbeek | 31:8fbee6c92753 | 99 | // calculate motor setpoint speed in deg/sec from setpoint x/y speed |
annesteenbeek | 30:a20f16bf8dda | 100 | |
annesteenbeek | 48:3cf1eaf34926 | 101 | |
annesteenbeek | 48:3cf1eaf34926 | 102 | if(motorsEnable){ // only run motors if switch is enabled |
annesteenbeek |
32:2006977785f5 | 103 | // compute new PID parameters using setpoint angle speeds and encoder speed |
annesteenbeek | 31:8fbee6c92753 | 104 | writeMotors(); |
annesteenbeek | 0:525558a26464 | 105 | }else{ |
annesteenbeek | 0:525558a26464 | 106 | // write 0 to motors |
annesteenbeek | 2:95ba9f6f0128 | 107 | motor1.write(0); |
annesteenbeek | 2:95ba9f6f0128 | 108 | motor2.write(0); |
annesteenbeek | 0:525558a26464 | 109 | } |
annesteenbeek | 0:525558a26464 | 110 | } |
annesteenbeek | 0:525558a26464 | 111 | |
annesteenbeek | 31:8fbee6c92753 | 112 | void writeMotors(){ |
annesteenbeek | 52:2ac9dee099ce | 113 | motor1PID.compute(); // calculate PID outputs, output changes automatically |
annesteenbeek | 52:2ac9dee099ce | 114 | motor2PID.compute(); |
annesteenbeek | 31:8fbee6c92753 | 115 | // write new values to motor's |
annesteenbeek | 52:2ac9dee099ce | 116 | if (motor1PWM >= 0 ){ // CCW rotation |
annesteenbeek | 31:8fbee6c92753 | 117 | direction1 = false; |
annesteenbeek | 31:8fbee6c92753 | 118 | }else{ |
annesteenbeek | 31:8fbee6c92753 | 119 | direction1 = true; // CW rotation |
annesteenbeek | 31:8fbee6c92753 | 120 | } |
annesteenbeek | 52:2ac9dee099ce | 121 | if (motor2PWM >= 0 ){ // CCW rotation |
annesteenbeek | 31:8fbee6c92753 | 122 | direction2 = false; |
annesteenbeek | 31:8fbee6c92753 | 123 | }else{ |
annesteenbeek | 31:8fbee6c92753 | 124 | direction2 = true; // CW rotation |
annesteenbeek | 31:8fbee6c92753 | 125 | } |
annesteenbeek | 31:8fbee6c92753 | 126 | motor1Dir.write(direction1); |
annesteenbeek | 31:8fbee6c92753 | 127 | motor2Dir.write(direction2); |
annesteenbeek | 31:8fbee6c92753 | 128 | |
annesteenbeek | 52:2ac9dee099ce | 129 | motor1.write(abs(motor1PWM)); |
annesteenbeek | 52:2ac9dee099ce | 130 | motor2.write(abs(motor2PWM)); |
annesteenbeek | 31:8fbee6c92753 | 131 | } |
annesteenbeek | 31:8fbee6c92753 | 132 | |
annesteenbeek | 0:525558a26464 | 133 | void servoControl(){ |
annesteenbeek | 0:525558a26464 | 134 | // use potMeter Value to set servo angle |
annesteenbeek | 34:f315b2b38555 | 135 | servo.write(servoPos); |
annesteenbeek | 0:525558a26464 | 136 | // (optionaly calculate xy position to keep balloon in position) |
annesteenbeek | 0:525558a26464 | 137 | // calculate z position using angle |
annesteenbeek | 0:525558a26464 | 138 | // calculate x y translation of endpoint |
annesteenbeek | 0:525558a26464 | 139 | // find new x and y speed. |
annesteenbeek | 0:525558a26464 | 140 | |
annesteenbeek | 25:874675516927 | 141 | } |
annesteenbeek | 25:874675516927 | 142 | |
annesteenbeek |
32:2006977785f5 | 143 | void calibrateMotors(){ |
annesteenbeek |
32:2006977785f5 | 144 | // bool calibrating1 = true; |
annesteenbeek |
32:2006977785f5 | 145 | // bool calibrating2 = true; |
annesteenbeek |
32:2006977785f5 | 146 | // motor1Dir.write(false); |
annesteenbeek |
32:2006977785f5 | 147 | // motor2Dir.write(false); |
annesteenbeek |
32:2006977785f5 | 148 | // // move motors CCW until they reach outer most position |
annesteenbeek |
32:2006977785f5 | 149 | // while (calibrating1 || calibrating2){ |
annesteenbeek |
32:2006977785f5 | 150 | // if(motor1AnglePin == 0){ |
annesteenbeek |
32:2006977785f5 | 151 | // motor1.write(0.3f); |
annesteenbeek |
32:2006977785f5 | 152 | // }else{ |
annesteenbeek |
32:2006977785f5 | 153 | // motor1.write(0); |
annesteenbeek |
32:2006977785f5 | 154 | // calibrating1 = false; |
annesteenbeek |
32:2006977785f5 | 155 | // } |
annesteenbeek |
32:2006977785f5 | 156 | // if(motor2AnglePin == 0){ |
annesteenbeek |
32:2006977785f5 | 157 | // motor2.write(0.3f); |
annesteenbeek |
32:2006977785f5 | 158 | // }else{ |
annesteenbeek |
32:2006977785f5 | 159 | // motor2.write(0); |
annesteenbeek |
32:2006977785f5 | 160 | // calibrating2 = false; |
annesteenbeek |
32:2006977785f5 | 161 | // } |
annesteenbeek |
32:2006977785f5 | 162 | // wait(0.2f); |
annesteenbeek |
32:2006977785f5 | 163 | // } |
annesteenbeek |
32:2006977785f5 | 164 | // // set the encoder values for angle. |
annesteenbeek |
32:2006977785f5 | 165 | // encoder1.setValue(0); |
annesteenbeek |
32:2006977785f5 | 166 | // encoder2.setValue(0); |
annesteenbeek |
32:2006977785f5 | 167 | } |