Martijn Grootens / Mbed 2 deprecated mbr_2017_robot_gr20

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?

UserRevisionLine numberNew 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