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@5:088917beb5e4, 2017-11-13 (annotated)

Committer:
megrootens
Date:
Mon Nov 13 09:11:42 2017 +0000
Revision:
5:088917beb5e4
Parent:
2:df0c6af898ac
Child:
7:b9a209f889f5
added include guards in header files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 5:088917beb5e4 1 #ifndef _ROBOT_H_
megrootens 5:088917beb5e4 2 #define _ROBOT_H_
megrootens 5:088917beb5e4 3
megrootens 0:caa8ee3bd882 4 // GEAR RATIO AND ENCODER COUNTS
megrootens 0:caa8ee3bd882 5 #define GEAR_RATIO 131
megrootens 0:caa8ee3bd882 6 #define COUNTS_PER_REV 64
megrootens 0:caa8ee3bd882 7
megrootens 0:caa8ee3bd882 8 // Motor 1
megrootens 0:caa8ee3bd882 9 #define M1_DIR D7
megrootens 0:caa8ee3bd882 10 #define M1_PWM D6
megrootens 0:caa8ee3bd882 11 #define M1_ENC_A D10
megrootens 0:caa8ee3bd882 12 #define M1_ENC_B D11
megrootens 0:caa8ee3bd882 13
megrootens 0:caa8ee3bd882 14 // Motor 2
megrootens 0:caa8ee3bd882 15 #define M2_DIR D4
megrootens 0:caa8ee3bd882 16 #define M2_PWM D5
megrootens 0:caa8ee3bd882 17 #define M2_ENC_A D12
megrootens 0:caa8ee3bd882 18 #define M2_ENC_B D13
megrootens 0:caa8ee3bd882 19
megrootens 0:caa8ee3bd882 20 #include "motor.h"
megrootens 0:caa8ee3bd882 21 #include "controller.h"
megrootens 0:caa8ee3bd882 22
megrootens 0:caa8ee3bd882 23
megrootens 0:caa8ee3bd882 24
megrootens 0:caa8ee3bd882 25 /**
megrootens 0:caa8ee3bd882 26 * Robot
megrootens 2:df0c6af898ac 27 *
megrootens 2:df0c6af898ac 28 * Implementation can be found in main.cpp
megrootens 0:caa8ee3bd882 29 */
megrootens 0:caa8ee3bd882 30 namespace robot
megrootens 0:caa8ee3bd882 31 {
megrootens 2:df0c6af898ac 32 // sample time
megrootens 2:df0c6af898ac 33 const double kSampleTime = 0.001;
megrootens 0:caa8ee3bd882 34
megrootens 2:df0c6af898ac 35 // Robot states
megrootens 0:caa8ee3bd882 36 enum State {
megrootens 0:caa8ee3bd882 37 OFF,
megrootens 0:caa8ee3bd882 38 CALIBRATION,
megrootens 0:caa8ee3bd882 39 HOMING,
megrootens 0:caa8ee3bd882 40 READY,
megrootens 0:caa8ee3bd882 41 DEMO,
megrootens 0:caa8ee3bd882 42 MANUAL
megrootens 0:caa8ee3bd882 43 };
megrootens 0:caa8ee3bd882 44
megrootens 2:df0c6af898ac 45 // Robot state description
megrootens 0:caa8ee3bd882 46 const char *StateNames[] = {
megrootens 0:caa8ee3bd882 47 "Off",
megrootens 0:caa8ee3bd882 48 "Calibration",
megrootens 0:caa8ee3bd882 49 "Homing",
megrootens 0:caa8ee3bd882 50 "Ready",
megrootens 0:caa8ee3bd882 51 "Demo",
megrootens 0:caa8ee3bd882 52 "Manual"
megrootens 0:caa8ee3bd882 53 };
megrootens 0:caa8ee3bd882 54
megrootens 2:df0c6af898ac 55 // State changing
megrootens 0:caa8ee3bd882 56 void SwitchState(State new_state);
megrootens 0:caa8ee3bd882 57 void GoToNextState();
megrootens 0:caa8ee3bd882 58
megrootens 0:caa8ee3bd882 59
megrootens 2:df0c6af898ac 60 /**
megrootens 2:df0c6af898ac 61 * Robot properties
megrootens 2:df0c6af898ac 62 */
megrootens 0:caa8ee3bd882 63 const double kL1 = 0.30;
megrootens 0:caa8ee3bd882 64 const double kL2 = 0.38;
megrootens 0:caa8ee3bd882 65
megrootens 0:caa8ee3bd882 66 const double kCalibAngleMotor1 = 140;
megrootens 2:df0c6af898ac 67 const double kCalibAngleMotor2 = -3;
megrootens 0:caa8ee3bd882 68
megrootens 2:df0c6af898ac 69 /**
megrootens 2:df0c6af898ac 70 * Motors and controllers
megrootens 2:df0c6af898ac 71 */
megrootens 0:caa8ee3bd882 72 Motor m1(M1_PWM, M1_DIR, M1_ENC_A, M1_ENC_B, GEAR_RATIO*COUNTS_PER_REV);
megrootens 0:caa8ee3bd882 73 Motor m2(M2_PWM, M2_DIR, M2_ENC_A, M2_ENC_B, GEAR_RATIO*COUNTS_PER_REV, true);
megrootens 0:caa8ee3bd882 74
megrootens 2:df0c6af898ac 75 Controller c1(.500,.200,0.070,kSampleTime);
megrootens 2:df0c6af898ac 76 Controller c2(.400,.200,0.050,kSampleTime);
megrootens 0:caa8ee3bd882 77
megrootens 2:df0c6af898ac 78 /**
megrootens 2:df0c6af898ac 79 * Robot state
megrootens 2:df0c6af898ac 80 */
megrootens 2:df0c6af898ac 81 State state = OFF;
megrootens 0:caa8ee3bd882 82 bool is_calibrated = false;
megrootens 0:caa8ee3bd882 83
megrootens 0:caa8ee3bd882 84 bool has_power();
megrootens 0:caa8ee3bd882 85
megrootens 0:caa8ee3bd882 86 double get_angle(int i);
megrootens 0:caa8ee3bd882 87 double get_x();
megrootens 0:caa8ee3bd882 88 double get_y();
megrootens 0:caa8ee3bd882 89
megrootens 2:df0c6af898ac 90 /**
megrootens 2:df0c6af898ac 91 * Forward and inverse kinematics
megrootens 2:df0c6af898ac 92 */
megrootens 0:caa8ee3bd882 93 double ForwardKinematicsX(double theta_1, double theta_2);
megrootens 0:caa8ee3bd882 94 double ForwardKinematicsY(double theta_1, double theta_2);
megrootens 0:caa8ee3bd882 95 double InverseKinematicsTheta(double x, double y, int i);
megrootens 0:caa8ee3bd882 96
megrootens 2:df0c6af898ac 97 /**
megrootens 2:df0c6af898ac 98 * Robot actions
megrootens 2:df0c6af898ac 99 */
megrootens 0:caa8ee3bd882 100 void Start();
megrootens 0:caa8ee3bd882 101 void Stop();
megrootens 0:caa8ee3bd882 102 void ControlLoop();
megrootens 0:caa8ee3bd882 103
megrootens 0:caa8ee3bd882 104 void SetCalibrationAngles();
megrootens 2:df0c6af898ac 105 }
megrootens 5:088917beb5e4 106
megrootens 5:088917beb5e4 107 #endif