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@55:ee5257fb73df, 2015-10-09 (annotated)

Committer:
annesteenbeek
Date:
Fri Oct 09 12:59:00 2015 +0000
Revision:
55:ee5257fb73df
Parent:
54:c14c3bc48b8a
Child:
60:20945383ad1b
bug fixes for PID filter

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 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 55:ee5257fb73df 42 bool pidOut = 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 52:2ac9dee099ce 58 PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
annesteenbeek 52:2ac9dee099ce 59 PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
annesteenbeek 31:8fbee6c92753 60
annesteenbeek 34:f315b2b38555 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 55:ee5257fb73df 72
annesteenbeek 55:ee5257fb73df 73 motor1PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 74 motor2PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 75
annesteenbeek 30:a20f16bf8dda 76 // Turn PID on
annesteenbeek 55:ee5257fb73df 77 motor1PID.SetMode(AUTOMATIC);
annesteenbeek 55:ee5257fb73df 78 motor2PID.SetMode(AUTOMATIC);
annesteenbeek 30:a20f16bf8dda 79
annesteenbeek 30:a20f16bf8dda 80 // start the timer
annesteenbeek 29:e4f3455aaa0b 81 t.start();
annesteenbeek 4:80e2280058ed 82 }
annesteenbeek 0:525558a26464 83
annesteenbeek 26:0a9e4147a31a 84
annesteenbeek 0:525558a26464 85 void motorControl(){
annesteenbeek 30:a20f16bf8dda 86 // get encoder positions in degrees
annesteenbeek 30:a20f16bf8dda 87 // 131.25:1 gear ratio
annesteenbeek 30:a20f16bf8dda 88 // getPosition uses X2 configuration, so 32 counts per revolution
annesteenbeek 30:a20f16bf8dda 89 // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
annesteenbeek 30:a20f16bf8dda 90 motor1Pos = -((encoder1.getPosition()/32)/131.25)*360;
annesteenbeek 30:a20f16bf8dda 91 motor2Pos = -((encoder2.getPosition()/32)/131.25)*360;
annesteenbeek 3:47c76be6d402 92
annesteenbeek 0:525558a26464 93 // check if motor's are within rotational boundarys
annesteenbeek 30:a20f16bf8dda 94 // get encoder speeds in deg/sec
annesteenbeek 50:b0cf07ca53cf 95 now = t.read();
annesteenbeek 50:b0cf07ca53cf 96 timechange = (now - prevTime);
annesteenbeek 52:2ac9dee099ce 97 motor1Speed = (motor1Pos - motor1PrevPos)/timechange;
annesteenbeek 52:2ac9dee099ce 98 motor2Speed = (motor2Pos - motor2PrevPos)/timechange;
annesteenbeek 50:b0cf07ca53cf 99 prevTime = now;
annesteenbeek 52:2ac9dee099ce 100 motor1PrevPos = motor1Pos;
annesteenbeek 52:2ac9dee099ce 101 motor2PrevPos = motor2Pos;
annesteenbeek 29:e4f3455aaa0b 102
annesteenbeek 31:8fbee6c92753 103 // calculate motor setpoint speed in deg/sec from setpoint x/y speed
annesteenbeek 30:a20f16bf8dda 104
annesteenbeek 48:3cf1eaf34926 105
annesteenbeek 48:3cf1eaf34926 106 if(motorsEnable){ // only run motors if switch is enabled
annesteenbeek 32:2006977785f5 107 // compute new PID parameters using setpoint angle speeds and encoder speed
annesteenbeek 31:8fbee6c92753 108 writeMotors();
annesteenbeek 0:525558a26464 109 }else{
annesteenbeek 0:525558a26464 110 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 111 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 112 motor2.write(0);
annesteenbeek 0:525558a26464 113 }
annesteenbeek 0:525558a26464 114 }
annesteenbeek 0:525558a26464 115
annesteenbeek 31:8fbee6c92753 116 void writeMotors(){
annesteenbeek 54:c14c3bc48b8a 117 motor1PID.Compute(); // calculate PID outputs, output changes automatically
annesteenbeek 55:ee5257fb73df 118 pidOut = motor2PID.Compute();
annesteenbeek 31:8fbee6c92753 119 // write new values to motor's
annesteenbeek 52:2ac9dee099ce 120 if (motor1PWM >= 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 121 direction1 = false;
annesteenbeek 31:8fbee6c92753 122 }else{
annesteenbeek 31:8fbee6c92753 123 direction1 = true; // CW rotation
annesteenbeek 31:8fbee6c92753 124 }
annesteenbeek 52:2ac9dee099ce 125 if (motor2PWM >= 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 126 direction2 = false;
annesteenbeek 31:8fbee6c92753 127 }else{
annesteenbeek 31:8fbee6c92753 128 direction2 = true; // CW rotation
annesteenbeek 31:8fbee6c92753 129 }
annesteenbeek 31:8fbee6c92753 130 motor1Dir.write(direction1);
annesteenbeek 31:8fbee6c92753 131 motor2Dir.write(direction2);
annesteenbeek 31:8fbee6c92753 132
annesteenbeek 52:2ac9dee099ce 133 motor1.write(abs(motor1PWM));
annesteenbeek 52:2ac9dee099ce 134 motor2.write(abs(motor2PWM));
annesteenbeek 31:8fbee6c92753 135 }
annesteenbeek 31:8fbee6c92753 136
annesteenbeek 0:525558a26464 137 void servoControl(){
annesteenbeek 0:525558a26464 138 // use potMeter Value to set servo angle
annesteenbeek 34:f315b2b38555 139 servo.write(servoPos);
annesteenbeek 0:525558a26464 140 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 141 // calculate z position using angle
annesteenbeek 0:525558a26464 142 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 143 // find new x and y speed.
annesteenbeek 0:525558a26464 144
annesteenbeek 25:874675516927 145 }
annesteenbeek 25:874675516927 146
annesteenbeek 32:2006977785f5 147 void calibrateMotors(){
annesteenbeek 32:2006977785f5 148 // bool calibrating1 = true;
annesteenbeek 32:2006977785f5 149 // bool calibrating2 = true;
annesteenbeek 32:2006977785f5 150 // motor1Dir.write(false);
annesteenbeek 32:2006977785f5 151 // motor2Dir.write(false);
annesteenbeek 32:2006977785f5 152 // // move motors CCW until they reach outer most position
annesteenbeek 32:2006977785f5 153 // while (calibrating1 || calibrating2){
annesteenbeek 32:2006977785f5 154 // if(motor1AnglePin == 0){
annesteenbeek 32:2006977785f5 155 // motor1.write(0.3f);
annesteenbeek 32:2006977785f5 156 // }else{
annesteenbeek 32:2006977785f5 157 // motor1.write(0);
annesteenbeek 32:2006977785f5 158 // calibrating1 = false;
annesteenbeek 32:2006977785f5 159 // }
annesteenbeek 32:2006977785f5 160 // if(motor2AnglePin == 0){
annesteenbeek 32:2006977785f5 161 // motor2.write(0.3f);
annesteenbeek 32:2006977785f5 162 // }else{
annesteenbeek 32:2006977785f5 163 // motor2.write(0);
annesteenbeek 32:2006977785f5 164 // calibrating2 = false;
annesteenbeek 32:2006977785f5 165 // }
annesteenbeek 32:2006977785f5 166 // wait(0.2f);
annesteenbeek 32:2006977785f5 167 // }
annesteenbeek 32:2006977785f5 168 // // set the encoder values for angle.
annesteenbeek 32:2006977785f5 169 // encoder1.setValue(0);
annesteenbeek 32:2006977785f5 170 // encoder2.setValue(0);
annesteenbeek 32:2006977785f5 171 }