ECE4180 Final Project Team / Mbed 2 deprecated Sliding_Camera_Rail

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos Linear_Stepper_Motor_Nema17

Fork of Sliding_Camera_Rail by Michael Marzano

main.cpp@0:59fcbfc00e50, 2020-04-28 (annotated)

Committer:
mikermarza
Date:
Tue Apr 28 21:39:27 2020 +0000
Revision:
0:59fcbfc00e50
Child:
1:8134c43fdb08
Final Working Version, Submission Ready

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikermarza 0:59fcbfc00e50 1 #include "mbed.h"
mikermarza 0:59fcbfc00e50 2 #include "rtos.h"
mikermarza 0:59fcbfc00e50 3 #include "lin_step_mtr.h"
mikermarza 0:59fcbfc00e50 4
mikermarza 0:59fcbfc00e50 5 /**********
mikermarza 0:59fcbfc00e50 6 * Defines *
mikermarza 0:59fcbfc00e50 7 **********/
mikermarza 0:59fcbfc00e50 8 #define ABSOLUTE_MIN 0 // minimum rev for software stop
mikermarza 0:59fcbfc00e50 9 #define ABSOLUTE_MAX 49.5 // maximum rev for software stop
mikermarza 0:59fcbfc00e50 10 #define MAX_SPEED 400 // maximum speed in RPM
mikermarza 0:59fcbfc00e50 11 #define MIN_SPEED 200 // minimum speed in RPM
mikermarza 0:59fcbfc00e50 12
mikermarza 0:59fcbfc00e50 13 // range over all possible speeds, used in calculating the rotation input to
mikermarza 0:59fcbfc00e50 14 // LinStepMtr::rotate() for smooth operation
mikermarza 0:59fcbfc00e50 15 #define SPEED_RNG MAX_SPEED - MIN_SPEED
mikermarza 0:59fcbfc00e50 16
mikermarza 0:59fcbfc00e50 17
mikermarza 0:59fcbfc00e50 18 /***********
mikermarza 0:59fcbfc00e50 19 * Hardware *
mikermarza 0:59fcbfc00e50 20 ***********/
mikermarza 0:59fcbfc00e50 21 LinStepMtr mtr(p19, p20, p17, p18); // for linear motor, coresponds t0 A,A',B,B'
mikermarza 0:59fcbfc00e50 22 BusOut lights (LED4,LED3,LED2,LED1);
mikermarza 0:59fcbfc00e50 23 //Serial xbee(p28,27);
mikermarza 0:59fcbfc00e50 24 Serial xbee(USBTX,USBRX);
mikermarza 0:59fcbfc00e50 25
mikermarza 0:59fcbfc00e50 26 /********
mikermarza 0:59fcbfc00e50 27 * Enums *
mikermarza 0:59fcbfc00e50 28 ********/
mikermarza 0:59fcbfc00e50 29 typedef enum {
mikermarza 0:59fcbfc00e50 30 OFF = 0b0000,
mikermarza 0:59fcbfc00e50 31 ALL = 0b1111,
mikermarza 0:59fcbfc00e50 32 L2 = 0b1100,
mikermarza 0:59fcbfc00e50 33 R2 = 0b0011,
mikermarza 0:59fcbfc00e50 34 AH1 = 0b1010,
mikermarza 0:59fcbfc00e50 35 AH2 = 0b0101
mikermarza 0:59fcbfc00e50 36 } LightPattern;
mikermarza 0:59fcbfc00e50 37
mikermarza 0:59fcbfc00e50 38 typedef enum {
mikermarza 0:59fcbfc00e50 39 STOP = 0,
mikermarza 0:59fcbfc00e50 40 MV_L,
mikermarza 0:59fcbfc00e50 41 MV_R,
mikermarza 0:59fcbfc00e50 42 } RailState;
mikermarza 0:59fcbfc00e50 43
mikermarza 0:59fcbfc00e50 44 typedef enum {
mikermarza 0:59fcbfc00e50 45 NORMAL = 0,
mikermarza 0:59fcbfc00e50 46 LOCK_N, // LOCK for normal mode
mikermarza 0:59fcbfc00e50 47 CALIBRATE,
mikermarza 0:59fcbfc00e50 48 GO_HOME,
mikermarza 0:59fcbfc00e50 49 RESET,
mikermarza 0:59fcbfc00e50 50 OVERRIDE
mikermarza 0:59fcbfc00e50 51 } RailMode;
mikermarza 0:59fcbfc00e50 52
mikermarza 0:59fcbfc00e50 53 typedef enum {
mikermarza 0:59fcbfc00e50 54 GOOD = 0,
mikermarza 0:59fcbfc00e50 55 START,
mikermarza 0:59fcbfc00e50 56 HOME,
mikermarza 0:59fcbfc00e50 57 AWAY
mikermarza 0:59fcbfc00e50 58 } CalibrateStatus;
mikermarza 0:59fcbfc00e50 59
mikermarza 0:59fcbfc00e50 60 /***********************
mikermarza 0:59fcbfc00e50 61 * Flag/State Variables *
mikermarza 0:59fcbfc00e50 62 ***********************/
mikermarza 0:59fcbfc00e50 63 volatile RailState rail_state; // the state of rail movement
mikermarza 0:59fcbfc00e50 64 volatile RailMode rail_mode; // rail mode, described in RailMode enum
mikermarza 0:59fcbfc00e50 65 volatile CalibrateStatus cal_stat; // status of calibration
mikermarza 0:59fcbfc00e50 66 volatile double home; // rev count for rail's "home", initialized to ABSOLUTE_MIN
mikermarza 0:59fcbfc00e50 67 volatile double away; // rev count for rail's farthest movement point initialized to ABSOLUTE_MAX;
mikermarza 0:59fcbfc00e50 68 volatile char speed; // current speed setting from inputs
mikermarza 0:59fcbfc00e50 69 volatile float rot; // number of revolutions to rotate based on speed
mikermarza 0:59fcbfc00e50 70
mikermarza 0:59fcbfc00e50 71 volatile bool going_home; // flag for if rail is headed back to "home"
mikermarza 0:59fcbfc00e50 72 volatile bool home_set; // flag for if home has been set
mikermarza 0:59fcbfc00e50 73 volatile bool away_set; // flag for if away has been set
mikermarza 0:59fcbfc00e50 74 volatile bool cal_done; // flag for if calibration is complete
mikermarza 0:59fcbfc00e50 75 volatile bool toggle;
mikermarza 0:59fcbfc00e50 76
mikermarza 0:59fcbfc00e50 77 /***************************
mikermarza 0:59fcbfc00e50 78 * General Static Functions *
mikermarza 0:59fcbfc00e50 79 ***************************/
mikermarza 0:59fcbfc00e50 80
mikermarza 0:59fcbfc00e50 81 static inline void send_ack()
mikermarza 0:59fcbfc00e50 82 {
mikermarza 0:59fcbfc00e50 83 xbee.printf("!A");
mikermarza 0:59fcbfc00e50 84 }
mikermarza 0:59fcbfc00e50 85
mikermarza 0:59fcbfc00e50 86 /*******************
mikermarza 0:59fcbfc00e50 87 * Thread Functions *
mikermarza 0:59fcbfc00e50 88 *******************/
mikermarza 0:59fcbfc00e50 89 // reads the input from the serial com port
mikermarza 0:59fcbfc00e50 90 void read_input(void const *arg)
mikermarza 0:59fcbfc00e50 91 {
mikermarza 0:59fcbfc00e50 92 char input;
mikermarza 0:59fcbfc00e50 93
mikermarza 0:59fcbfc00e50 94 while(1) {
mikermarza 0:59fcbfc00e50 95 if(!xbee.readable()){
mikermarza 0:59fcbfc00e50 96 Thread::yield();
mikermarza 0:59fcbfc00e50 97 } else {
mikermarza 0:59fcbfc00e50 98 input = xbee.getc();
mikermarza 0:59fcbfc00e50 99 if (input != '!'){
mikermarza 0:59fcbfc00e50 100 Thread::yield();
mikermarza 0:59fcbfc00e50 101 }else {
mikermarza 0:59fcbfc00e50 102 while(!xbee.readable()) Thread::yield();
mikermarza 0:59fcbfc00e50 103 input = xbee.getc();
mikermarza 0:59fcbfc00e50 104
mikermarza 0:59fcbfc00e50 105 switch(input){
mikermarza 0:59fcbfc00e50 106 case '0': // no movement
mikermarza 0:59fcbfc00e50 107 rail_state = STOP;
mikermarza 0:59fcbfc00e50 108 while(!xbee.readable()) Thread::yield();
mikermarza 0:59fcbfc00e50 109 input = xbee.getc();
mikermarza 0:59fcbfc00e50 110 break;
mikermarza 0:59fcbfc00e50 111 case '1':
mikermarza 0:59fcbfc00e50 112 case '2':
mikermarza 0:59fcbfc00e50 113 case '3':
mikermarza 0:59fcbfc00e50 114 case '4':
mikermarza 0:59fcbfc00e50 115 case '5':
mikermarza 0:59fcbfc00e50 116 // handle speed
mikermarza 0:59fcbfc00e50 117 if(speed != input) {
mikermarza 0:59fcbfc00e50 118 speed = input;
mikermarza 0:59fcbfc00e50 119 rot = ((float)input - 49) * .0125 + .2;
mikermarza 0:59fcbfc00e50 120 mtr.set_speed(((float) input - 49)*50 + 200);
mikermarza 0:59fcbfc00e50 121 }
mikermarza 0:59fcbfc00e50 122 while(!xbee.readable()) Thread::yield();
mikermarza 0:59fcbfc00e50 123 input = xbee.getc();
mikermarza 0:59fcbfc00e50 124 xbee.printf("DIR: %c\n",input);
mikermarza 0:59fcbfc00e50 125
mikermarza 0:59fcbfc00e50 126 switch(input){
mikermarza 0:59fcbfc00e50 127 case 'L': // move left
mikermarza 0:59fcbfc00e50 128 rail_state = MV_L;
mikermarza 0:59fcbfc00e50 129 break;
mikermarza 0:59fcbfc00e50 130 case 'R': // move right
mikermarza 0:59fcbfc00e50 131 rail_state = MV_R;
mikermarza 0:59fcbfc00e50 132 break;
mikermarza 0:59fcbfc00e50 133 default:
mikermarza 0:59fcbfc00e50 134 break;
mikermarza 0:59fcbfc00e50 135 }
mikermarza 0:59fcbfc00e50 136 break;
mikermarza 0:59fcbfc00e50 137 case 'S': // stop movement
mikermarza 0:59fcbfc00e50 138 rail_state = STOP;
mikermarza 0:59fcbfc00e50 139 break;
mikermarza 0:59fcbfc00e50 140 case 'F': // Lock or Unlock rail
mikermarza 0:59fcbfc00e50 141 switch(rail_mode) {
mikermarza 0:59fcbfc00e50 142 case NORMAL:
mikermarza 0:59fcbfc00e50 143 rail_mode = LOCK_N;
mikermarza 0:59fcbfc00e50 144 break;
mikermarza 0:59fcbfc00e50 145 case LOCK_N:
mikermarza 0:59fcbfc00e50 146 rail_mode = NORMAL;
mikermarza 0:59fcbfc00e50 147 rail_state = STOP;
mikermarza 0:59fcbfc00e50 148 break;
mikermarza 0:59fcbfc00e50 149 default:
mikermarza 0:59fcbfc00e50 150 break; // do nothing
mikermarza 0:59fcbfc00e50 151 }
mikermarza 0:59fcbfc00e50 152 break;
mikermarza 0:59fcbfc00e50 153 case 'C':
mikermarza 0:59fcbfc00e50 154 switch(rail_mode){
mikermarza 0:59fcbfc00e50 155 case NORMAL:
mikermarza 0:59fcbfc00e50 156 case LOCK_N:
mikermarza 0:59fcbfc00e50 157 case CALIBRATE:
mikermarza 0:59fcbfc00e50 158 switch(cal_stat){
mikermarza 0:59fcbfc00e50 159 case GOOD:
mikermarza 0:59fcbfc00e50 160 rail_state = STOP;
mikermarza 0:59fcbfc00e50 161 home_set = false;
mikermarza 0:59fcbfc00e50 162 away_set = false;
mikermarza 0:59fcbfc00e50 163 cal_done = false;
mikermarza 0:59fcbfc00e50 164 cal_stat = START;
mikermarza 0:59fcbfc00e50 165 rail_mode = CALIBRATE;
mikermarza 0:59fcbfc00e50 166 Thread::wait(600);
mikermarza 0:59fcbfc00e50 167 break;
mikermarza 0:59fcbfc00e50 168 case START:
mikermarza 0:59fcbfc00e50 169 case HOME:
mikermarza 0:59fcbfc00e50 170 home = mtr.get_rev();
mikermarza 0:59fcbfc00e50 171 rail_state = STOP;
mikermarza 0:59fcbfc00e50 172 home_set = true;
mikermarza 0:59fcbfc00e50 173 send_ack();
mikermarza 0:59fcbfc00e50 174 cal_stat = AWAY;
mikermarza 0:59fcbfc00e50 175 Thread::wait(600);
mikermarza 0:59fcbfc00e50 176 break;
mikermarza 0:59fcbfc00e50 177 case AWAY:
mikermarza 0:59fcbfc00e50 178 away = mtr.get_rev();
mikermarza 0:59fcbfc00e50 179 rail_state = STOP;
mikermarza 0:59fcbfc00e50 180 away_set = true;
mikermarza 0:59fcbfc00e50 181 send_ack();
mikermarza 0:59fcbfc00e50 182 cal_stat = GOOD;
mikermarza 0:59fcbfc00e50 183 Thread::wait(600);
mikermarza 0:59fcbfc00e50 184 break;
mikermarza 0:59fcbfc00e50 185 }
mikermarza 0:59fcbfc00e50 186 break;
mikermarza 0:59fcbfc00e50 187 default:
mikermarza 0:59fcbfc00e50 188 break;
mikermarza 0:59fcbfc00e50 189 }
mikermarza 0:59fcbfc00e50 190 break;
mikermarza 0:59fcbfc00e50 191 case 'H': // send rail to home
mikermarza 0:59fcbfc00e50 192 switch(rail_mode){
mikermarza 0:59fcbfc00e50 193 case NORMAL:
mikermarza 0:59fcbfc00e50 194 case LOCK_N:
mikermarza 0:59fcbfc00e50 195 rail_state = STOP;
mikermarza 0:59fcbfc00e50 196 going_home = true;
mikermarza 0:59fcbfc00e50 197 rail_mode = GO_HOME;
mikermarza 0:59fcbfc00e50 198 break;
mikermarza 0:59fcbfc00e50 199 }
mikermarza 0:59fcbfc00e50 200 break;
mikermarza 0:59fcbfc00e50 201 default:
mikermarza 0:59fcbfc00e50 202 break; // do nothing because invalid input
mikermarza 0:59fcbfc00e50 203 }
mikermarza 0:59fcbfc00e50 204 Thread::yield(); // hopefully makes this thread run a little faster
mikermarza 0:59fcbfc00e50 205 }
mikermarza 0:59fcbfc00e50 206 }
mikermarza 0:59fcbfc00e50 207 }
mikermarza 0:59fcbfc00e50 208
mikermarza 0:59fcbfc00e50 209 }
mikermarza 0:59fcbfc00e50 210
mikermarza 0:59fcbfc00e50 211 // blinks leds a set number of times with a wait time of w
mikermarza 0:59fcbfc00e50 212 static inline void blink_leds(int t, int w, LightPattern p)
mikermarza 0:59fcbfc00e50 213 {
mikermarza 0:59fcbfc00e50 214 for(int i=0; i < t; ++i){
mikermarza 0:59fcbfc00e50 215 lights = OFF;
mikermarza 0:59fcbfc00e50 216 Thread::wait(w);
mikermarza 0:59fcbfc00e50 217 lights = p;
mikermarza 0:59fcbfc00e50 218 Thread::wait(w);
mikermarza 0:59fcbfc00e50 219 }
mikermarza 0:59fcbfc00e50 220 lights = OFF;
mikermarza 0:59fcbfc00e50 221 Thread::wait(w);
mikermarza 0:59fcbfc00e50 222 }
mikermarza 0:59fcbfc00e50 223
mikermarza 0:59fcbfc00e50 224 // reads flag variables to control LED status lights
mikermarza 0:59fcbfc00e50 225 void status_lights(void const *arg)
mikermarza 0:59fcbfc00e50 226 {
mikermarza 0:59fcbfc00e50 227 while(1) {
mikermarza 0:59fcbfc00e50 228 switch(rail_mode){
mikermarza 0:59fcbfc00e50 229 case LOCK_N:
mikermarza 0:59fcbfc00e50 230 lights = ALL;
mikermarza 0:59fcbfc00e50 231 Thread::yield();
mikermarza 0:59fcbfc00e50 232 break;
mikermarza 0:59fcbfc00e50 233 case CALIBRATE:
mikermarza 0:59fcbfc00e50 234 //blink twice
mikermarza 0:59fcbfc00e50 235 blink_leds(2,150,ALL);
mikermarza 0:59fcbfc00e50 236
mikermarza 0:59fcbfc00e50 237 toggle = true;
mikermarza 0:59fcbfc00e50 238 while(!home_set){
mikermarza 0:59fcbfc00e50 239 if(toggle)
mikermarza 0:59fcbfc00e50 240 lights = L2;
mikermarza 0:59fcbfc00e50 241 else
mikermarza 0:59fcbfc00e50 242 lights = OFF;
mikermarza 0:59fcbfc00e50 243
mikermarza 0:59fcbfc00e50 244 toggle = !toggle;
mikermarza 0:59fcbfc00e50 245 Thread::wait(500);
mikermarza 0:59fcbfc00e50 246 }
mikermarza 0:59fcbfc00e50 247 blink_leds(2,150,L2);
mikermarza 0:59fcbfc00e50 248
mikermarza 0:59fcbfc00e50 249 toggle = true;
mikermarza 0:59fcbfc00e50 250 while(!away_set){
mikermarza 0:59fcbfc00e50 251 if(toggle)
mikermarza 0:59fcbfc00e50 252 lights = R2;
mikermarza 0:59fcbfc00e50 253 else
mikermarza 0:59fcbfc00e50 254 lights = OFF;
mikermarza 0:59fcbfc00e50 255
mikermarza 0:59fcbfc00e50 256 toggle = !toggle;
mikermarza 0:59fcbfc00e50 257 Thread::wait(500);
mikermarza 0:59fcbfc00e50 258 }
mikermarza 0:59fcbfc00e50 259 blink_leds(2,150,R2);
mikermarza 0:59fcbfc00e50 260 Thread::wait(500);
mikermarza 0:59fcbfc00e50 261
mikermarza 0:59fcbfc00e50 262 while(!cal_done);
mikermarza 0:59fcbfc00e50 263
mikermarza 0:59fcbfc00e50 264 blink_leds(2,150,ALL);
mikermarza 0:59fcbfc00e50 265 break;
mikermarza 0:59fcbfc00e50 266 case GO_HOME:
mikermarza 0:59fcbfc00e50 267 // blink thrice
mikermarza 0:59fcbfc00e50 268 blink_leds(3,150,ALL);
mikermarza 0:59fcbfc00e50 269
mikermarza 0:59fcbfc00e50 270 toggle = true;
mikermarza 0:59fcbfc00e50 271 while(going_home){
mikermarza 0:59fcbfc00e50 272 if(toggle)
mikermarza 0:59fcbfc00e50 273 lights = ALL;
mikermarza 0:59fcbfc00e50 274 else
mikermarza 0:59fcbfc00e50 275 lights = OFF;
mikermarza 0:59fcbfc00e50 276
mikermarza 0:59fcbfc00e50 277 toggle = !toggle;
mikermarza 0:59fcbfc00e50 278 Thread::wait(600);
mikermarza 0:59fcbfc00e50 279 }
mikermarza 0:59fcbfc00e50 280
mikermarza 0:59fcbfc00e50 281 // blink thrice for complete
mikermarza 0:59fcbfc00e50 282 blink_leds(3,150,ALL);
mikermarza 0:59fcbfc00e50 283
mikermarza 0:59fcbfc00e50 284 break;
mikermarza 0:59fcbfc00e50 285 case NORMAL: // no lights for normal opperation
mikermarza 0:59fcbfc00e50 286 default:
mikermarza 0:59fcbfc00e50 287 lights = OFF;
mikermarza 0:59fcbfc00e50 288 Thread::yield();
mikermarza 0:59fcbfc00e50 289 break;
mikermarza 0:59fcbfc00e50 290 }
mikermarza 0:59fcbfc00e50 291 }
mikermarza 0:59fcbfc00e50 292 }
mikermarza 0:59fcbfc00e50 293
mikermarza 0:59fcbfc00e50 294
mikermarza 0:59fcbfc00e50 295 /****************
mikermarza 0:59fcbfc00e50 296 * Main Function *
mikermarza 0:59fcbfc00e50 297 ****************/
mikermarza 0:59fcbfc00e50 298
mikermarza 0:59fcbfc00e50 299 int main() {
mikermarza 0:59fcbfc00e50 300
mikermarza 0:59fcbfc00e50 301 // Variables
mikermarza 0:59fcbfc00e50 302 double cur_loc; // current location in revolutions. used for GO_HOME
mikermarza 0:59fcbfc00e50 303
mikermarza 0:59fcbfc00e50 304 //set defaults for global variables
mikermarza 0:59fcbfc00e50 305 rail_state = STOP;
mikermarza 0:59fcbfc00e50 306 rail_mode = NORMAL;
mikermarza 0:59fcbfc00e50 307 cal_stat = GOOD;
mikermarza 0:59fcbfc00e50 308 home = ABSOLUTE_MIN;
mikermarza 0:59fcbfc00e50 309 away = ABSOLUTE_MAX;
mikermarza 0:59fcbfc00e50 310 speed = '5';
mikermarza 0:59fcbfc00e50 311 rot = .25;
mikermarza 0:59fcbfc00e50 312
mikermarza 0:59fcbfc00e50 313 going_home = false;
mikermarza 0:59fcbfc00e50 314 home_set = true;
mikermarza 0:59fcbfc00e50 315 away_set = true;
mikermarza 0:59fcbfc00e50 316 cal_done = true;
mikermarza 0:59fcbfc00e50 317 toggle = true;
mikermarza 0:59fcbfc00e50 318
mikermarza 0:59fcbfc00e50 319 // Start Threads
mikermarza 0:59fcbfc00e50 320 Thread input(read_input);
mikermarza 0:59fcbfc00e50 321 Thread lights(status_lights);
mikermarza 0:59fcbfc00e50 322
mikermarza 0:59fcbfc00e50 323 wait(.001);;
mikermarza 0:59fcbfc00e50 324
mikermarza 0:59fcbfc00e50 325 // set up motor
mikermarza 0:59fcbfc00e50 326 mtr.set_min_rev_cnt(home);
mikermarza 0:59fcbfc00e50 327 mtr.set_max_rev_cnt(away);
mikermarza 0:59fcbfc00e50 328
mikermarza 0:59fcbfc00e50 329 while(1) {
mikermarza 0:59fcbfc00e50 330 switch(rail_mode) {
mikermarza 0:59fcbfc00e50 331 case NORMAL:
mikermarza 0:59fcbfc00e50 332 switch(rail_state) {
mikermarza 0:59fcbfc00e50 333 case MV_L:
mikermarza 0:59fcbfc00e50 334 mtr.rotate(LinStepMtr::CW,rot);
mikermarza 0:59fcbfc00e50 335 break;
mikermarza 0:59fcbfc00e50 336 case MV_R:
mikermarza 0:59fcbfc00e50 337 mtr.rotate(LinStepMtr::CCW,rot);
mikermarza 0:59fcbfc00e50 338 break;
mikermarza 0:59fcbfc00e50 339 case STOP:
mikermarza 0:59fcbfc00e50 340 default:
mikermarza 0:59fcbfc00e50 341 break;
mikermarza 0:59fcbfc00e50 342 }
mikermarza 0:59fcbfc00e50 343 break;
mikermarza 0:59fcbfc00e50 344 case LOCK_N:
mikermarza 0:59fcbfc00e50 345 // do nothing
mikermarza 0:59fcbfc00e50 346 break;
mikermarza 0:59fcbfc00e50 347 case CALIBRATE:
mikermarza 0:59fcbfc00e50 348 switch(cal_stat) {
mikermarza 0:59fcbfc00e50 349 case START:
mikermarza 0:59fcbfc00e50 350 // reset stats
mikermarza 0:59fcbfc00e50 351 mtr.RESET_rev_cnts();
mikermarza 0:59fcbfc00e50 352 cal_stat = HOME;
mikermarza 0:59fcbfc00e50 353 break;
mikermarza 0:59fcbfc00e50 354 case GOOD:
mikermarza 0:59fcbfc00e50 355 // save software bounds to rotation library
mikermarza 0:59fcbfc00e50 356 if(away < home) { //away is right of home
mikermarza 0:59fcbfc00e50 357 mtr.set_min_rev_cnt(away);
mikermarza 0:59fcbfc00e50 358 mtr.set_max_rev_cnt(home);
mikermarza 0:59fcbfc00e50 359 } else { // away is left of home
mikermarza 0:59fcbfc00e50 360 mtr.set_min_rev_cnt(home);
mikermarza 0:59fcbfc00e50 361 mtr.set_max_rev_cnt(away);
mikermarza 0:59fcbfc00e50 362 }
mikermarza 0:59fcbfc00e50 363
mikermarza 0:59fcbfc00e50 364 cal_done = true;
mikermarza 0:59fcbfc00e50 365 send_ack();
mikermarza 0:59fcbfc00e50 366 rail_mode = NORMAL;
mikermarza 0:59fcbfc00e50 367 break;
mikermarza 0:59fcbfc00e50 368 default:
mikermarza 0:59fcbfc00e50 369 break;
mikermarza 0:59fcbfc00e50 370 }
mikermarza 0:59fcbfc00e50 371 switch(rail_state) {
mikermarza 0:59fcbfc00e50 372 case MV_L:
mikermarza 0:59fcbfc00e50 373 mtr.rotate(LinStepMtr::CW,rot);
mikermarza 0:59fcbfc00e50 374 break;
mikermarza 0:59fcbfc00e50 375 case MV_R:
mikermarza 0:59fcbfc00e50 376 mtr.rotate(LinStepMtr::CCW,rot);
mikermarza 0:59fcbfc00e50 377 break;
mikermarza 0:59fcbfc00e50 378 case STOP:
mikermarza 0:59fcbfc00e50 379 default:
mikermarza 0:59fcbfc00e50 380 break;
mikermarza 0:59fcbfc00e50 381 }
mikermarza 0:59fcbfc00e50 382 break;
mikermarza 0:59fcbfc00e50 383 case GO_HOME:
mikermarza 0:59fcbfc00e50 384 Thread::wait(1000);
mikermarza 0:59fcbfc00e50 385
mikermarza 0:59fcbfc00e50 386 // get currecnt location
mikermarza 0:59fcbfc00e50 387 cur_loc = mtr.get_rev();
mikermarza 0:59fcbfc00e50 388
mikermarza 0:59fcbfc00e50 389 mtr.set_speed(MAX_SPEED);
mikermarza 0:59fcbfc00e50 390 speed = '0';
mikermarza 0:59fcbfc00e50 391
mikermarza 0:59fcbfc00e50 392 if(home < cur_loc) { // home is right of current location
mikermarza 0:59fcbfc00e50 393 mtr.rotate(LinStepMtr::CCW,cur_loc - home);
mikermarza 0:59fcbfc00e50 394 } else { // home is to the left of current location
mikermarza 0:59fcbfc00e50 395 mtr.rotate(LinStepMtr::CW, home - cur_loc);
mikermarza 0:59fcbfc00e50 396 }
mikermarza 0:59fcbfc00e50 397 going_home = false;
mikermarza 0:59fcbfc00e50 398 rail_state = STOP;
mikermarza 0:59fcbfc00e50 399 rail_mode = NORMAL;
mikermarza 0:59fcbfc00e50 400 send_ack();
mikermarza 0:59fcbfc00e50 401 Thread::wait(500);
mikermarza 0:59fcbfc00e50 402 break;
mikermarza 0:59fcbfc00e50 403
mikermarza 0:59fcbfc00e50 404 default:
mikermarza 0:59fcbfc00e50 405 break;
mikermarza 0:59fcbfc00e50 406 }
mikermarza 0:59fcbfc00e50 407
mikermarza 0:59fcbfc00e50 408 Thread::yield();
mikermarza 0:59fcbfc00e50 409 }
mikermarza 0:59fcbfc00e50 410 }
mikermarza 0:59fcbfc00e50 411