Code to let Gr20's BioRobotics2017 robot come to live.
Dependencies: FastPWM MODSERIAL QEI mbed
robot.h@0:caa8ee3bd882, 2017-11-12 (annotated)
- Committer:
- megrootens
- Date:
- Sun Nov 12 00:14:05 2017 +0000
- Revision:
- 0:caa8ee3bd882
- Child:
- 1:ff11ee1c6baa
setup code. too much play in gears and axles slip. But performance already better than bio robotics.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
megrootens | 0:caa8ee3bd882 | 1 | // GEAR RATIO AND ENCODER COUNTS |
megrootens | 0:caa8ee3bd882 | 2 | #define GEAR_RATIO 131 |
megrootens | 0:caa8ee3bd882 | 3 | #define COUNTS_PER_REV 64 |
megrootens | 0:caa8ee3bd882 | 4 | |
megrootens | 0:caa8ee3bd882 | 5 | // Motor 1 |
megrootens | 0:caa8ee3bd882 | 6 | #define M1_DIR D7 |
megrootens | 0:caa8ee3bd882 | 7 | #define M1_PWM D6 |
megrootens | 0:caa8ee3bd882 | 8 | #define M1_ENC_A D10 |
megrootens | 0:caa8ee3bd882 | 9 | #define M1_ENC_B D11 |
megrootens | 0:caa8ee3bd882 | 10 | |
megrootens | 0:caa8ee3bd882 | 11 | // Motor 2 |
megrootens | 0:caa8ee3bd882 | 12 | #define M2_DIR D4 |
megrootens | 0:caa8ee3bd882 | 13 | #define M2_PWM D5 |
megrootens | 0:caa8ee3bd882 | 14 | #define M2_ENC_A D12 |
megrootens | 0:caa8ee3bd882 | 15 | #define M2_ENC_B D13 |
megrootens | 0:caa8ee3bd882 | 16 | |
megrootens | 0:caa8ee3bd882 | 17 | #include "motor.h" |
megrootens | 0:caa8ee3bd882 | 18 | #include "controller.h" |
megrootens | 0:caa8ee3bd882 | 19 | |
megrootens | 0:caa8ee3bd882 | 20 | |
megrootens | 0:caa8ee3bd882 | 21 | |
megrootens | 0:caa8ee3bd882 | 22 | /** |
megrootens | 0:caa8ee3bd882 | 23 | * Robot |
megrootens | 0:caa8ee3bd882 | 24 | */ |
megrootens | 0:caa8ee3bd882 | 25 | namespace robot |
megrootens | 0:caa8ee3bd882 | 26 | { |
megrootens | 0:caa8ee3bd882 | 27 | |
megrootens | 0:caa8ee3bd882 | 28 | enum State { |
megrootens | 0:caa8ee3bd882 | 29 | OFF, |
megrootens | 0:caa8ee3bd882 | 30 | CALIBRATION, |
megrootens | 0:caa8ee3bd882 | 31 | HOMING, |
megrootens | 0:caa8ee3bd882 | 32 | READY, |
megrootens | 0:caa8ee3bd882 | 33 | DEMO, |
megrootens | 0:caa8ee3bd882 | 34 | MANUAL |
megrootens | 0:caa8ee3bd882 | 35 | }; |
megrootens | 0:caa8ee3bd882 | 36 | |
megrootens | 0:caa8ee3bd882 | 37 | const char *StateNames[] = { |
megrootens | 0:caa8ee3bd882 | 38 | "Off", |
megrootens | 0:caa8ee3bd882 | 39 | "Calibration", |
megrootens | 0:caa8ee3bd882 | 40 | "Homing", |
megrootens | 0:caa8ee3bd882 | 41 | "Ready", |
megrootens | 0:caa8ee3bd882 | 42 | "Demo", |
megrootens | 0:caa8ee3bd882 | 43 | "Manual" |
megrootens | 0:caa8ee3bd882 | 44 | }; |
megrootens | 0:caa8ee3bd882 | 45 | |
megrootens | 0:caa8ee3bd882 | 46 | void SwitchState(State new_state); |
megrootens | 0:caa8ee3bd882 | 47 | void GoToNextState(); |
megrootens | 0:caa8ee3bd882 | 48 | |
megrootens | 0:caa8ee3bd882 | 49 | State state = OFF; |
megrootens | 0:caa8ee3bd882 | 50 | |
megrootens | 0:caa8ee3bd882 | 51 | const double kSampleTime = 0.001; |
megrootens | 0:caa8ee3bd882 | 52 | |
megrootens | 0:caa8ee3bd882 | 53 | const double kL1 = 0.30; |
megrootens | 0:caa8ee3bd882 | 54 | const double kL2 = 0.38; |
megrootens | 0:caa8ee3bd882 | 55 | |
megrootens | 0:caa8ee3bd882 | 56 | const double kCalibAngleMotor1 = 140; |
megrootens | 0:caa8ee3bd882 | 57 | const double kCalibAngleMotor2 = 0; |
megrootens | 0:caa8ee3bd882 | 58 | |
megrootens | 0:caa8ee3bd882 | 59 | |
megrootens | 0:caa8ee3bd882 | 60 | Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV); |
megrootens | 0:caa8ee3bd882 | 61 | Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true); |
megrootens | 0:caa8ee3bd882 | 62 | |
megrootens | 0:caa8ee3bd882 | 63 | Controller c1(.8,.1,0,kSampleTime); |
megrootens | 0:caa8ee3bd882 | 64 | Controller c2(.8,.1,0,kSampleTime); |
megrootens | 0:caa8ee3bd882 | 65 | |
megrootens | 0:caa8ee3bd882 | 66 | bool is_calibrated = false; |
megrootens | 0:caa8ee3bd882 | 67 | |
megrootens | 0:caa8ee3bd882 | 68 | bool has_power(); |
megrootens | 0:caa8ee3bd882 | 69 | |
megrootens | 0:caa8ee3bd882 | 70 | double get_angle(int i); |
megrootens | 0:caa8ee3bd882 | 71 | double get_x(); |
megrootens | 0:caa8ee3bd882 | 72 | double get_y(); |
megrootens | 0:caa8ee3bd882 | 73 | |
megrootens | 0:caa8ee3bd882 | 74 | double ForwardKinematicsX(double theta_1, double theta_2); |
megrootens | 0:caa8ee3bd882 | 75 | double ForwardKinematicsY(double theta_1, double theta_2); |
megrootens | 0:caa8ee3bd882 | 76 | |
megrootens | 0:caa8ee3bd882 | 77 | double InverseKinematicsTheta(double x, double y, int i); |
megrootens | 0:caa8ee3bd882 | 78 | |
megrootens | 0:caa8ee3bd882 | 79 | void Start(); |
megrootens | 0:caa8ee3bd882 | 80 | void Stop(); |
megrootens | 0:caa8ee3bd882 | 81 | void ControlLoop(); |
megrootens | 0:caa8ee3bd882 | 82 | |
megrootens | 0:caa8ee3bd882 | 83 | void SetCalibrationAngles(); |
megrootens | 0:caa8ee3bd882 | 84 | } |