Martijn Grootens / Mbed 2 deprecated mbr_2017_robot_gr20

Code to let Gr20's BioRobotics2017 robot come to live.

Dependencies:   FastPWM MODSERIAL QEI mbed

ref.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 "ref.h"
megrootens 7:b9a209f889f5 2 #include "robot.h"
megrootens 7:b9a209f889f5 3 #include "ui.h"
megrootens 7:b9a209f889f5 4
megrootens 7:b9a209f889f5 5 /**
megrootens 7:b9a209f889f5 6 * Reference implementation
megrootens 7:b9a209f889f5 7 */
megrootens 7:b9a209f889f5 8 namespace ref
megrootens 7:b9a209f889f5 9 {
megrootens 7:b9a209f889f5 10
megrootens 7:b9a209f889f5 11 // current reference angles
megrootens 7:b9a209f889f5 12 double theta_1 = 0;
megrootens 7:b9a209f889f5 13 double theta_2 = 0;
megrootens 7:b9a209f889f5 14
megrootens 7:b9a209f889f5 15 // current demo coord
megrootens 7:b9a209f889f5 16 int i_demo_coord = 0;
megrootens 7:b9a209f889f5 17
megrootens 7:b9a209f889f5 18
megrootens 7:b9a209f889f5 19 /**
megrootens 7:b9a209f889f5 20 * Get reference x-coordinate using reference angles and forward kinematics
megrootens 7:b9a209f889f5 21 */
megrootens 7:b9a209f889f5 22 double get_ref_x()
megrootens 7:b9a209f889f5 23 {
megrootens 7:b9a209f889f5 24 return robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 7:b9a209f889f5 25 }
megrootens 7:b9a209f889f5 26
megrootens 7:b9a209f889f5 27 /**
megrootens 7:b9a209f889f5 28 * Get reference x-coordinate using reference angles and forward kinematics
megrootens 7:b9a209f889f5 29 */
megrootens 7:b9a209f889f5 30 double get_ref_y()
megrootens 7:b9a209f889f5 31 {
megrootens 7:b9a209f889f5 32 return robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 7:b9a209f889f5 33 }
megrootens 7:b9a209f889f5 34
megrootens 7:b9a209f889f5 35 /**
megrootens 7:b9a209f889f5 36 * Update reference signal depending on robot state
megrootens 7:b9a209f889f5 37 */
megrootens 7:b9a209f889f5 38 void UpdateReference()
megrootens 7:b9a209f889f5 39 {
megrootens 7:b9a209f889f5 40 switch(robot::state) {
megrootens 7:b9a209f889f5 41 case robot::CALIBRATION:
megrootens 7:b9a209f889f5 42 CalibrationReference();
megrootens 7:b9a209f889f5 43 break;
megrootens 7:b9a209f889f5 44 case robot::HOMING:
megrootens 7:b9a209f889f5 45 HomingReference();
megrootens 7:b9a209f889f5 46 break;
megrootens 7:b9a209f889f5 47 case robot::DEMO:
megrootens 7:b9a209f889f5 48 DemoReference();
megrootens 7:b9a209f889f5 49 break;
megrootens 7:b9a209f889f5 50 }
megrootens 7:b9a209f889f5 51 }
megrootens 7:b9a209f889f5 52
megrootens 7:b9a209f889f5 53 /**
megrootens 7:b9a209f889f5 54 * Generate reference profile to bring robotto calibration position;
megrootens 7:b9a209f889f5 55 * at start-up the motor angles are '0' (relative encoders)
megrootens 7:b9a209f889f5 56 */
megrootens 7:b9a209f889f5 57 void CalibrationReference()
megrootens 7:b9a209f889f5 58 {
megrootens 7:b9a209f889f5 59 // Check when motors can no longer move farther
megrootens 7:b9a209f889f5 60 bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
megrootens 7:b9a209f889f5 61 bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);
megrootens 7:b9a209f889f5 62
megrootens 7:b9a209f889f5 63 // Turn off motor if end-stop reached
megrootens 7:b9a209f889f5 64 // Motor 2 _should_ arrive first
megrootens 7:b9a209f889f5 65 if (is_ready_1) {
megrootens 7:b9a209f889f5 66 robot::m1.Stop();
megrootens 7:b9a209f889f5 67 } else {
megrootens 7:b9a209f889f5 68 theta_1 += kCalibrationOmegaStep;
megrootens 7:b9a209f889f5 69 }
megrootens 7:b9a209f889f5 70
megrootens 7:b9a209f889f5 71 // Turn off motor if end-stop reached
megrootens 7:b9a209f889f5 72 if (is_ready_2) {
megrootens 7:b9a209f889f5 73 robot::m2.Stop();
megrootens 7:b9a209f889f5 74 } else {
megrootens 7:b9a209f889f5 75 theta_2 += kCalibrationOmegaStep;
megrootens 7:b9a209f889f5 76 }
megrootens 7:b9a209f889f5 77
megrootens 7:b9a209f889f5 78 if (is_ready_1 & is_ready_2) {
megrootens 7:b9a209f889f5 79 ui::serial.printf(" - Both motors at end-stop.\r\n");
megrootens 7:b9a209f889f5 80 robot::Stop();
megrootens 7:b9a209f889f5 81 // Set the calibration angles as current position
megrootens 7:b9a209f889f5 82 robot::SetCalibrationAngles();
megrootens 7:b9a209f889f5 83 robot::GoToNextState();
megrootens 7:b9a209f889f5 84 }
megrootens 7:b9a209f889f5 85 }
megrootens 7:b9a209f889f5 86
megrootens 7:b9a209f889f5 87 /**
megrootens 7:b9a209f889f5 88 * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
megrootens 7:b9a209f889f5 89 */
megrootens 7:b9a209f889f5 90 void HomingReference()
megrootens 7:b9a209f889f5 91 {
megrootens 7:b9a209f889f5 92 // First move motor 1
megrootens 7:b9a209f889f5 93 double dist_1 = kOriginTheta1 - theta_1;
megrootens 7:b9a209f889f5 94
megrootens 7:b9a209f889f5 95 if (abs(dist_1)<=kCalibrationOmegaStep) {
megrootens 7:b9a209f889f5 96 // Motor 1 ready
megrootens 7:b9a209f889f5 97 theta_1 = kOriginTheta1;
megrootens 7:b9a209f889f5 98
megrootens 7:b9a209f889f5 99 // Only move motor 2 after motor 1 at home
megrootens 7:b9a209f889f5 100 double dist_2 = kOriginTheta2 - theta_2;
megrootens 7:b9a209f889f5 101
megrootens 7:b9a209f889f5 102 if (abs(dist_2)<=kCalibrationOmegaStep) {
megrootens 7:b9a209f889f5 103 // motor 2 also ready
megrootens 7:b9a209f889f5 104 theta_2 = kOriginTheta2;
megrootens 7:b9a209f889f5 105
megrootens 7:b9a209f889f5 106 ui::serial.printf(" - Robot at home position.\r\n");
megrootens 7:b9a209f889f5 107 robot::GoToNextState();
megrootens 7:b9a209f889f5 108 } else {
megrootens 7:b9a209f889f5 109 // Motor 2 still to move
megrootens 7:b9a209f889f5 110 theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
megrootens 7:b9a209f889f5 111 }
megrootens 7:b9a209f889f5 112
megrootens 7:b9a209f889f5 113
megrootens 7:b9a209f889f5 114 } else {
megrootens 7:b9a209f889f5 115 // Motor 1 still to move
megrootens 7:b9a209f889f5 116 theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
megrootens 7:b9a209f889f5 117 }
megrootens 7:b9a209f889f5 118 }
megrootens 7:b9a209f889f5 119
megrootens 7:b9a209f889f5 120 /**
megrootens 7:b9a209f889f5 121 * Cycle through demo coordinates to display straight lines
megrootens 7:b9a209f889f5 122 */
megrootens 7:b9a209f889f5 123 void DemoReference()
megrootens 7:b9a209f889f5 124 {
megrootens 7:b9a209f889f5 125 // Compare current setpoint and goal coord and slowly move linearly in the
megrootens 7:b9a209f889f5 126 // right direction (straight line 2D)
megrootens 7:b9a209f889f5 127
megrootens 7:b9a209f889f5 128 double x = robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 7:b9a209f889f5 129 double y = robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 7:b9a209f889f5 130
megrootens 7:b9a209f889f5 131 double x_goal = kDemoCoords[0][i_demo_coord];
megrootens 7:b9a209f889f5 132 double y_goal = kDemoCoords[1][i_demo_coord];
megrootens 7:b9a209f889f5 133
megrootens 7:b9a209f889f5 134 double delta_x = x_goal - x;
megrootens 7:b9a209f889f5 135 double delta_y = y_goal - y;
megrootens 7:b9a209f889f5 136
megrootens 7:b9a209f889f5 137 double dist = sqrt(pow(delta_x,2) + pow(delta_y,2));
megrootens 7:b9a209f889f5 138
megrootens 7:b9a209f889f5 139 // Compute next setpoint (x,y)
megrootens 7:b9a209f889f5 140 double x_new = x;
megrootens 7:b9a209f889f5 141 double y_new = y;
megrootens 7:b9a209f889f5 142
megrootens 7:b9a209f889f5 143 if (dist<kMaxStep) {
megrootens 7:b9a209f889f5 144 x_new = x_goal;
megrootens 7:b9a209f889f5 145 y_new = y_goal;
megrootens 7:b9a209f889f5 146
megrootens 7:b9a209f889f5 147 ui::serial.printf(" - Reached Demo Coord %d\r\n",i_demo_coord);
megrootens 7:b9a209f889f5 148 i_demo_coord = ++i_demo_coord % kNumDemoCoords;
megrootens 7:b9a209f889f5 149
megrootens 7:b9a209f889f5 150 if (i_demo_coord == 0) {
megrootens 7:b9a209f889f5 151 robot::is_calibrated = false;
megrootens 7:b9a209f889f5 152 robot::SwitchState(robot::CALIBRATION);
megrootens 7:b9a209f889f5 153 }
megrootens 7:b9a209f889f5 154 } else {
megrootens 7:b9a209f889f5 155 x_new += delta_x / dist * kMaxStep;
megrootens 7:b9a209f889f5 156 y_new += delta_y / dist * kMaxStep;
megrootens 7:b9a209f889f5 157 }
megrootens 7:b9a209f889f5 158
megrootens 7:b9a209f889f5 159 // Compute motor angles
megrootens 7:b9a209f889f5 160 theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
megrootens 7:b9a209f889f5 161 theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
megrootens 7:b9a209f889f5 162 }
megrootens 7:b9a209f889f5 163
megrootens 7:b9a209f889f5 164 /**
megrootens 7:b9a209f889f5 165 * Set current robot position as reference position.
megrootens 7:b9a209f889f5 166 */
megrootens 7:b9a209f889f5 167 void SetPositionAsReference()
megrootens 7:b9a209f889f5 168 {
megrootens 7:b9a209f889f5 169 theta_1 = robot::get_angle(1);
megrootens 7:b9a209f889f5 170 theta_2 = robot::get_angle(2);
megrootens 7:b9a209f889f5 171 }
megrootens 7:b9a209f889f5 172
megrootens 7:b9a209f889f5 173 } // end namespace ref