Code to let Gr20's BioRobotics2017 robot come to live.
Dependencies: FastPWM MODSERIAL QEI mbed
robot.cpp@9:4bf2034d37a3, 2017-11-13 (annotated)
- Committer:
- megrootens
- Date:
- Mon Nov 13 18:23:41 2017 +0000
- Revision:
- 9:4bf2034d37a3
- Parent:
- 7:b9a209f889f5
check on robot: code runs without any problems.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
megrootens | 7:b9a209f889f5 | 1 | #include "robot.h" |
megrootens | 7:b9a209f889f5 | 2 | #include "ui.h" |
megrootens | 7:b9a209f889f5 | 3 | #include "ref.h" |
megrootens | 7:b9a209f889f5 | 4 | #include "tools.h" |
megrootens | 7:b9a209f889f5 | 5 | |
megrootens | 7:b9a209f889f5 | 6 | |
megrootens | 7:b9a209f889f5 | 7 | /** |
megrootens | 7:b9a209f889f5 | 8 | * Robot implementation |
megrootens | 7:b9a209f889f5 | 9 | */ |
megrootens | 7:b9a209f889f5 | 10 | namespace robot |
megrootens | 7:b9a209f889f5 | 11 | { |
megrootens | 7:b9a209f889f5 | 12 | Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV); |
megrootens | 7:b9a209f889f5 | 13 | Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true); |
megrootens | 7:b9a209f889f5 | 14 | |
megrootens | 7:b9a209f889f5 | 15 | Controller c1(.500,.200,0.070,kSampleTime); |
megrootens | 7:b9a209f889f5 | 16 | Controller c2(.400,.200,0.050,kSampleTime); |
megrootens | 7:b9a209f889f5 | 17 | |
megrootens | 7:b9a209f889f5 | 18 | State state = OFF; |
megrootens | 7:b9a209f889f5 | 19 | bool is_calibrated = false; |
megrootens | 7:b9a209f889f5 | 20 | |
megrootens | 7:b9a209f889f5 | 21 | /** |
megrootens | 7:b9a209f889f5 | 22 | * Switching ROBOT state to a new state |
megrootens | 7:b9a209f889f5 | 23 | */ |
megrootens | 7:b9a209f889f5 | 24 | void SwitchState(State new_state) |
megrootens | 7:b9a209f889f5 | 25 | { |
megrootens | 7:b9a209f889f5 | 26 | ui::serial.printf("* Switching ROBOT State to: %s\r\n",StateNames[new_state]); |
megrootens | 7:b9a209f889f5 | 27 | state = new_state; |
megrootens | 7:b9a209f889f5 | 28 | } |
megrootens | 7:b9a209f889f5 | 29 | |
megrootens | 7:b9a209f889f5 | 30 | /** |
megrootens | 7:b9a209f889f5 | 31 | * State flow // MANUAL state not implemented |
megrootens | 7:b9a209f889f5 | 32 | */ |
megrootens | 7:b9a209f889f5 | 33 | void GoToNextState() |
megrootens | 7:b9a209f889f5 | 34 | { |
megrootens | 7:b9a209f889f5 | 35 | switch (state) { |
megrootens | 7:b9a209f889f5 | 36 | case OFF: |
megrootens | 7:b9a209f889f5 | 37 | SwitchState(CALIBRATION); |
megrootens | 7:b9a209f889f5 | 38 | ui::SwitchState(ui::IGNORE); |
megrootens | 7:b9a209f889f5 | 39 | Start(); |
megrootens | 7:b9a209f889f5 | 40 | break; |
megrootens | 7:b9a209f889f5 | 41 | case CALIBRATION: |
megrootens | 7:b9a209f889f5 | 42 | SwitchState(HOMING); |
megrootens | 7:b9a209f889f5 | 43 | Start(); |
megrootens | 7:b9a209f889f5 | 44 | break; |
megrootens | 7:b9a209f889f5 | 45 | case HOMING: |
megrootens | 7:b9a209f889f5 | 46 | SwitchState(DEMO); |
megrootens | 7:b9a209f889f5 | 47 | //ui::SwitchState(ui::STATE_SWITCHING); |
megrootens | 7:b9a209f889f5 | 48 | break; |
megrootens | 7:b9a209f889f5 | 49 | case READY: |
megrootens | 7:b9a209f889f5 | 50 | SwitchState(DEMO); |
megrootens | 7:b9a209f889f5 | 51 | break; |
megrootens | 7:b9a209f889f5 | 52 | default: |
megrootens | 7:b9a209f889f5 | 53 | SwitchState(OFF); |
megrootens | 7:b9a209f889f5 | 54 | is_calibrated = false; |
megrootens | 7:b9a209f889f5 | 55 | Stop(); |
megrootens | 7:b9a209f889f5 | 56 | } |
megrootens | 7:b9a209f889f5 | 57 | } |
megrootens | 7:b9a209f889f5 | 58 | |
megrootens | 7:b9a209f889f5 | 59 | /** |
megrootens | 7:b9a209f889f5 | 60 | * Check whether any of the motors is running |
megrootens | 7:b9a209f889f5 | 61 | */ |
megrootens | 7:b9a209f889f5 | 62 | bool has_power() |
megrootens | 7:b9a209f889f5 | 63 | { |
megrootens | 7:b9a209f889f5 | 64 | return m1.has_power() or m2.has_power(); |
megrootens | 7:b9a209f889f5 | 65 | } |
megrootens | 7:b9a209f889f5 | 66 | |
megrootens | 7:b9a209f889f5 | 67 | /** |
megrootens | 7:b9a209f889f5 | 68 | * Get motor angle in [deg] for motor i=1,2 |
megrootens | 7:b9a209f889f5 | 69 | */ |
megrootens | 7:b9a209f889f5 | 70 | double get_angle(int i) |
megrootens | 7:b9a209f889f5 | 71 | { |
megrootens | 7:b9a209f889f5 | 72 | switch(i) { |
megrootens | 7:b9a209f889f5 | 73 | case 1: |
megrootens | 7:b9a209f889f5 | 74 | return m1.get_angle(); |
megrootens | 7:b9a209f889f5 | 75 | case 2: |
megrootens | 7:b9a209f889f5 | 76 | return m2.get_angle(); |
megrootens | 7:b9a209f889f5 | 77 | default: |
megrootens | 7:b9a209f889f5 | 78 | return NULL; |
megrootens | 7:b9a209f889f5 | 79 | } |
megrootens | 7:b9a209f889f5 | 80 | } |
megrootens | 7:b9a209f889f5 | 81 | |
megrootens | 7:b9a209f889f5 | 82 | /** |
megrootens | 7:b9a209f889f5 | 83 | * Compute end-effector x-coordinate [m] for given motor angles [deg]. |
megrootens | 7:b9a209f889f5 | 84 | */ |
megrootens | 7:b9a209f889f5 | 85 | double ForwardKinematicsX(double theta_1, double theta_2) |
megrootens | 7:b9a209f889f5 | 86 | { |
megrootens | 7:b9a209f889f5 | 87 | return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2)); |
megrootens | 7:b9a209f889f5 | 88 | } |
megrootens | 7:b9a209f889f5 | 89 | |
megrootens | 7:b9a209f889f5 | 90 | /** |
megrootens | 7:b9a209f889f5 | 91 | * Compute end-effector y-coordinate [m] for given motor angles [deg]. |
megrootens | 7:b9a209f889f5 | 92 | */ |
megrootens | 7:b9a209f889f5 | 93 | double ForwardKinematicsY(double theta_1, double theta_2) |
megrootens | 7:b9a209f889f5 | 94 | { |
megrootens | 7:b9a209f889f5 | 95 | return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2)); |
megrootens | 7:b9a209f889f5 | 96 | } |
megrootens | 7:b9a209f889f5 | 97 | |
megrootens | 7:b9a209f889f5 | 98 | /** |
megrootens | 7:b9a209f889f5 | 99 | * Compute end-effector x-coordinate [m] for current motor angles. |
megrootens | 7:b9a209f889f5 | 100 | */ |
megrootens | 7:b9a209f889f5 | 101 | double get_x() |
megrootens | 7:b9a209f889f5 | 102 | { |
megrootens | 7:b9a209f889f5 | 103 | return ForwardKinematicsX(get_angle(1),get_angle(2)); |
megrootens | 7:b9a209f889f5 | 104 | } |
megrootens | 7:b9a209f889f5 | 105 | |
megrootens | 7:b9a209f889f5 | 106 | /** |
megrootens | 7:b9a209f889f5 | 107 | * Compute end-effector y-coordinate [m] for current motor angles. |
megrootens | 7:b9a209f889f5 | 108 | */ |
megrootens | 7:b9a209f889f5 | 109 | double get_y() |
megrootens | 7:b9a209f889f5 | 110 | { |
megrootens | 7:b9a209f889f5 | 111 | return ForwardKinematicsY(get_angle(1),get_angle(2)); |
megrootens | 7:b9a209f889f5 | 112 | } |
megrootens | 7:b9a209f889f5 | 113 | |
megrootens | 7:b9a209f889f5 | 114 | /** |
megrootens | 7:b9a209f889f5 | 115 | * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y |
megrootens | 7:b9a209f889f5 | 116 | * coordinate [m] |
megrootens | 7:b9a209f889f5 | 117 | */ |
megrootens | 7:b9a209f889f5 | 118 | double InverseKinematicsTheta(double x, double y, int i) |
megrootens | 7:b9a209f889f5 | 119 | { |
megrootens | 7:b9a209f889f5 | 120 | // vector to end point |
megrootens | 7:b9a209f889f5 | 121 | double r = sqrt(pow(x,2) + pow(y,2)); |
megrootens | 7:b9a209f889f5 | 122 | double dir_angle = rad2deg( atan2(y,x) ); |
megrootens | 7:b9a209f889f5 | 123 | |
megrootens | 7:b9a209f889f5 | 124 | // law of cosines for internal angles |
megrootens | 7:b9a209f889f5 | 125 | double int_angle_1 = |
megrootens | 7:b9a209f889f5 | 126 | rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) ); |
megrootens | 7:b9a209f889f5 | 127 | double int_angle_2 = |
megrootens | 7:b9a209f889f5 | 128 | rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) ); |
megrootens | 7:b9a209f889f5 | 129 | |
megrootens | 7:b9a209f889f5 | 130 | // absolute motor angles w.r.t x,y frame |
megrootens | 7:b9a209f889f5 | 131 | double theta_1 = dir_angle + int_angle_1; |
megrootens | 7:b9a209f889f5 | 132 | double theta_2 = dir_angle - int_angle_2; |
megrootens | 7:b9a209f889f5 | 133 | |
megrootens | 7:b9a209f889f5 | 134 | switch (i) { |
megrootens | 7:b9a209f889f5 | 135 | case 1: return theta_1; |
megrootens | 7:b9a209f889f5 | 136 | case 2: return theta_2; |
megrootens | 7:b9a209f889f5 | 137 | default: return NULL; |
megrootens | 7:b9a209f889f5 | 138 | } |
megrootens | 7:b9a209f889f5 | 139 | } |
megrootens | 7:b9a209f889f5 | 140 | |
megrootens | 7:b9a209f889f5 | 141 | /** |
megrootens | 7:b9a209f889f5 | 142 | * Start robot: |
megrootens | 7:b9a209f889f5 | 143 | * - set reference position to current position s.t. no 'explosion' |
megrootens | 7:b9a209f889f5 | 144 | * - start motors |
megrootens | 7:b9a209f889f5 | 145 | * - reset controller |
megrootens | 7:b9a209f889f5 | 146 | */ |
megrootens | 7:b9a209f889f5 | 147 | void Start() |
megrootens | 7:b9a209f889f5 | 148 | { |
megrootens | 7:b9a209f889f5 | 149 | ui::serial.printf("! Starting robot.\r\n"); |
megrootens | 7:b9a209f889f5 | 150 | ui::serial.printf(" - Setting current position as reference.\r\n"); |
megrootens | 7:b9a209f889f5 | 151 | ref::SetPositionAsReference(); |
megrootens | 7:b9a209f889f5 | 152 | |
megrootens | 7:b9a209f889f5 | 153 | ui::serial.printf(" - Resetting controllers.\r\n"); |
megrootens | 7:b9a209f889f5 | 154 | c1.Reset(); |
megrootens | 7:b9a209f889f5 | 155 | c2.Reset(); |
megrootens | 7:b9a209f889f5 | 156 | |
megrootens | 7:b9a209f889f5 | 157 | ui::serial.printf(" - Starting motors.\r\n"); |
megrootens | 7:b9a209f889f5 | 158 | m1.Start(); |
megrootens | 7:b9a209f889f5 | 159 | m2.Start(); |
megrootens | 7:b9a209f889f5 | 160 | } |
megrootens | 7:b9a209f889f5 | 161 | |
megrootens | 7:b9a209f889f5 | 162 | void Stop() |
megrootens | 7:b9a209f889f5 | 163 | { |
megrootens | 7:b9a209f889f5 | 164 | ui::serial.printf("! Stopping robot.\r\n"); |
megrootens | 7:b9a209f889f5 | 165 | ui::serial.printf(" - Stopping motors.\r\n"); |
megrootens | 7:b9a209f889f5 | 166 | m1.Stop(); |
megrootens | 7:b9a209f889f5 | 167 | m2.Stop(); |
megrootens | 7:b9a209f889f5 | 168 | } |
megrootens | 7:b9a209f889f5 | 169 | |
megrootens | 7:b9a209f889f5 | 170 | /** |
megrootens | 7:b9a209f889f5 | 171 | * Control loop; negative feedback control on error with reference postion |
megrootens | 7:b9a209f889f5 | 172 | */ |
megrootens | 7:b9a209f889f5 | 173 | void ControlLoop() |
megrootens | 7:b9a209f889f5 | 174 | { |
megrootens | 7:b9a209f889f5 | 175 | m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) )); |
megrootens | 7:b9a209f889f5 | 176 | m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) )); |
megrootens | 7:b9a209f889f5 | 177 | } |
megrootens | 7:b9a209f889f5 | 178 | |
megrootens | 7:b9a209f889f5 | 179 | /** |
megrootens | 7:b9a209f889f5 | 180 | * Set the motor angles to the values corresponding to the end-stops. |
megrootens | 7:b9a209f889f5 | 181 | * Only call this method when the robot is at its end-stops! |
megrootens | 7:b9a209f889f5 | 182 | */ |
megrootens | 7:b9a209f889f5 | 183 | void SetCalibrationAngles() |
megrootens | 7:b9a209f889f5 | 184 | { |
megrootens | 7:b9a209f889f5 | 185 | m1.set_angle(kCalibAngleMotor1); |
megrootens | 7:b9a209f889f5 | 186 | m2.set_angle(kCalibAngleMotor2); |
megrootens | 7:b9a209f889f5 | 187 | is_calibrated = true; |
megrootens | 7:b9a209f889f5 | 188 | } |
megrootens | 7:b9a209f889f5 | 189 | |
megrootens | 7:b9a209f889f5 | 190 | } // end namespace robot |