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@2:7facfc9e53ff, 2020-04-29 (annotated)

Committer:
mikermarza
Date:
Wed Apr 29 03:04:02 2020 +0000
Revision:
2:7facfc9e53ff
Parent:
1:8134c43fdb08
Child:
3:e1fd0b211fcd
Adjusted comments

Who changed what in which revision?

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