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.h@1:ff11ee1c6baa, 2017-11-12 (annotated)

Committer:
megrootens
Date:
Sun Nov 12 12:05:22 2017 +0000
Revision:
1:ff11ee1c6baa
Parent:
0:caa8ee3bd882
Child:
2:df0c6af898ac
tuned controller, added 2-data point moving average velocity estimate

Who changed what in which revision?

UserRevisionLine numberNew 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 1:ff11ee1c6baa 63 Controller c1(.200,.100,0.020,kSampleTime);
megrootens 1:ff11ee1c6baa 64 Controller c2(.200,.100,0.020,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 }