William Pedler / Mbed 2 deprecated FinalFinal_SeniorDesign

Code used for the 2013 LRM Senior Design Team at VCU 2013 Senior Design Linear Rotary Motor

Dependencies:   mbed

main.cpp@3:0a1b8c86a31e, 2013-04-24 (annotated)

Committer:
pedlerw
Date:
Wed Apr 24 18:25:18 2013 +0000
Revision:
3:0a1b8c86a31e
Parent:
2:675f8691f469 
Read Previous

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pedlerw 0:b1f00c299273 1 //FinalFinal_SeniorDesign//
pedlerw 0:b1f00c299273 2 //Written by William Pedler 4/23/2013//
pedlerw 0:b1f00c299273 3 //This is the program the 2013 LRM team is using as the Final Program Written
pedlerw 0:b1f00c299273 4 //for Senior Design. This program uses two xbee s1 for serial interface//
pedlerw 0:b1f00c299273 5
pedlerw 0:b1f00c299273 6 //Includes//
pedlerw 0:b1f00c299273 7 #include "mbed.h"
pedlerw 0:b1f00c299273 8 using namespace std;
pedlerw 0:b1f00c299273 9
pedlerw 0:b1f00c299273 10 //Define//
pedlerw 0:b1f00c299273 11 #define DEBUG
pedlerw 0:b1f00c299273 12 #define MAX 1.0
pedlerw 0:b1f00c299273 13 #define MIN 0.0
pedlerw 0:b1f00c299273 14 #define large_num 500 //This times 4 controls how long the sensors read for each position//
pedlerw 0:b1f00c299273 15 #define one 1
pedlerw 0:b1f00c299273 16 #define two 2
pedlerw 0:b1f00c299273 17 #define three 3.0
pedlerw 0:b1f00c299273 18 #define four 4
pedlerw 0:b1f00c299273 19 #define five 5
pedlerw 0:b1f00c299273 20 #define six 6
pedlerw 0:b1f00c299273 21 #define seven 7
pedlerw 0:b1f00c299273 22 #define eight 8
pedlerw 0:b1f00c299273 23 #define nine 9
pedlerw 0:b1f00c299273 24 #define track_length 42
pedlerw 0:b1f00c299273 25 #define fast 0.125
pedlerw 0:b1f00c299273 26 #define medium 1.0
pedlerw 0:b1f00c299273 27 #define slow 2.0
pedlerw 0:b1f00c299273 28 #define sensor_set 0.005
pedlerw 0:b1f00c299273 29 #define rot_fast 0.080
pedlerw 0:b1f00c299273 30 #define rot_med 0.100
pedlerw 0:b1f00c299273 31 #define rot_slow 0.120
pedlerw 0:b1f00c299273 32 #define rot_pos 24
pedlerw 0:b1f00c299273 33 #define rot_step 15
pedlerw 0:b1f00c299273 34
pedlerw 0:b1f00c299273 35 //Declar global variables//
pedlerw 0:b1f00c299273 36 float left_min[track_length] = {0};
pedlerw 0:b1f00c299273 37 float left_max[track_length] = {0};
pedlerw 0:b1f00c299273 38 float right_min[track_length] = {0};
pedlerw 0:b1f00c299273 39 float right_max[track_length] = {0};
pedlerw 0:b1f00c299273 40 float rotary_threshold[rot_pos] = {0};
pedlerw 0:b1f00c299273 41 float threash = 0;
pedlerw 0:b1f00c299273 42 int rot_position = 0;
pedlerw 2:675f8691f469 43 bool state = 0; //Zero is when laser is blocked...1 is when laser is hitting the photocell
pedlerw 0:b1f00c299273 44
pedlerw 0:b1f00c299273 45 //Labeling the Local file system//
pedlerw 0:b1f00c299273 46 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
pedlerw 0:b1f00c299273 47 //Labeling onboard LED's//
pedlerw 0:b1f00c299273 48 DigitalOut led1(LED1);
pedlerw 0:b1f00c299273 49 DigitalOut led2(LED2);
pedlerw 0:b1f00c299273 50 DigitalOut led3(LED3);
pedlerw 0:b1f00c299273 51 DigitalOut led4(LED4);
pedlerw 0:b1f00c299273 52 //Label linear coils//
pedlerw 0:b1f00c299273 53 DigitalOut linear0(p5); //coil 1
pedlerw 0:b1f00c299273 54 DigitalOut linear1(p6); //coil 2
pedlerw 0:b1f00c299273 55 DigitalOut linear2(p7); //coil 3
pedlerw 0:b1f00c299273 56 DigitalOut linear3(p8); //coil 4
pedlerw 0:b1f00c299273 57 DigitalOut linear4(p9); //coil 5
pedlerw 0:b1f00c299273 58 DigitalOut linear5(p10); //coil 6
pedlerw 0:b1f00c299273 59 DigitalOut linear6(p11); //coil 7
pedlerw 0:b1f00c299273 60 //Reduce noise by declaring all unused Analog pins as DigitalOut//
pedlerw 0:b1f00c299273 61 AnalogIn rot_indication(p15);
pedlerw 0:b1f00c299273 62 AnalogIn zero(p16);
pedlerw 0:b1f00c299273 63 DigitalOut left17(p17);
pedlerw 0:b1f00c299273 64 DigitalOut left18(p18);
pedlerw 0:b1f00c299273 65 AnalogIn distanceR(p19); //Vout yellow GND black Vss red
pedlerw 0:b1f00c299273 66 AnalogIn distanceL(p20); //Vout yellow GND black Vss red
pedlerw 0:b1f00c299273 67 //Declar rotary coils Digital I/O//
pedlerw 0:b1f00c299273 68 DigitalOut rotary0(p21);
pedlerw 0:b1f00c299273 69 DigitalOut rotary1(p22);
pedlerw 0:b1f00c299273 70 DigitalOut rotary2(p23);
pedlerw 0:b1f00c299273 71 DigitalOut rotary3(p24);
pedlerw 0:b1f00c299273 72 DigitalOut rotary4(p25);
pedlerw 0:b1f00c299273 73 DigitalOut rotary5(p26);
pedlerw 0:b1f00c299273 74 //Set serial com//
pedlerw 0:b1f00c299273 75 Serial xbee(p13,p14);
pedlerw 0:b1f00c299273 76
pedlerw 0:b1f00c299273 77 //Function Prototypes//
pedlerw 0:b1f00c299273 78 void move_all_left(int num); //num for # of coil sets left//
pedlerw 0:b1f00c299273 79 void move_all_right(int num); //num for # of coil sets right//
pedlerw 0:b1f00c299273 80 float get_Lsensor_min();
pedlerw 0:b1f00c299273 81 float get_Lsensor_max();
pedlerw 0:b1f00c299273 82 float get_Rsensor_min();
pedlerw 0:b1f00c299273 83 float get_Rsensor_max();
pedlerw 0:b1f00c299273 84 void move_one_right(int p); //p for position//
pedlerw 0:b1f00c299273 85 void move_one_left(int p); //p for position//
pedlerw 0:b1f00c299273 86 int get_position();
pedlerw 0:b1f00c299273 87 void calibrate_linear();
pedlerw 0:b1f00c299273 88 void read_file();
pedlerw 0:b1f00c299273 89 int move_linear(int l); //l for location//
pedlerw 0:b1f00c299273 90 int error_check();
pedlerw 0:b1f00c299273 91 void display_mode();
pedlerw 0:b1f00c299273 92 void move_all_leftBobby(int num);
pedlerw 0:b1f00c299273 93 void move_all_rightBobby(int num);
pedlerw 0:b1f00c299273 94 void scroll_up(int line); //line for number lines to scroll//
pedlerw 0:b1f00c299273 95 void cork_screw();
pedlerw 0:b1f00c299273 96 void rotate_all_cw(int set, float speed);
pedlerw 0:b1f00c299273 97 void rotate_all_ccw(int set, float speed);
pedlerw 0:b1f00c299273 98 void rotary_speed();
pedlerw 0:b1f00c299273 99 void fast_rotary_cw();
pedlerw 0:b1f00c299273 100 void small_rotary_speed();
pedlerw 0:b1f00c299273 101 void rot_one_cw();
pedlerw 0:b1f00c299273 102 void rot_one_ccw();
pedlerw 0:b1f00c299273 103 void calibrate_rot();
pedlerw 0:b1f00c299273 104 int get_user_angle(int desired_angle);
pedlerw 0:b1f00c299273 105 void go_to_angle(int angle);
pedlerw 0:b1f00c299273 106 void rot_pos_inc();
pedlerw 0:b1f00c299273 107 void rot_pos_dec();
pedlerw 2:675f8691f469 108 bool check_state(bool current_state);
pedlerw 0:b1f00c299273 109 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 110 //////////////////////////////////MAIN/////////////////////////////////////////
pedlerw 0:b1f00c299273 111 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 112 int main()
pedlerw 0:b1f00c299273 113 {
pedlerw 0:b1f00c299273 114 //Declarations & Initializations//
pedlerw 0:b1f00c299273 115 int desired_location = 0;
pedlerw 0:b1f00c299273 116 int desired_angle = 0;
pedlerw 0:b1f00c299273 117 int user_instruction = -1;
pedlerw 0:b1f00c299273 118
pedlerw 0:b1f00c299273 119 //Initialize Linear Coils//
pedlerw 0:b1f00c299273 120 linear0 = 1;
pedlerw 0:b1f00c299273 121 linear1 = 1;
pedlerw 0:b1f00c299273 122 linear2 = 1;
pedlerw 0:b1f00c299273 123 linear3 = 1;
pedlerw 0:b1f00c299273 124 linear4 = 1;
pedlerw 0:b1f00c299273 125 linear5 = 1;
pedlerw 0:b1f00c299273 126 linear6 = 1;
pedlerw 0:b1f00c299273 127 //Initialize Rotary Coils//
pedlerw 0:b1f00c299273 128 //Declar rotary coils Digital I/O//
pedlerw 0:b1f00c299273 129 rotary0 = 1;
pedlerw 0:b1f00c299273 130 rotary1 = 1;
pedlerw 0:b1f00c299273 131 rotary2 = 1;
pedlerw 0:b1f00c299273 132 rotary3 = 1;
pedlerw 0:b1f00c299273 133 rotary4 = 1;
pedlerw 0:b1f00c299273 134 rotary5 = 1;
pedlerw 0:b1f00c299273 135 //Initialize onbord led//
pedlerw 0:b1f00c299273 136 led1 = 0;
pedlerw 0:b1f00c299273 137 led2 = 0;
pedlerw 0:b1f00c299273 138 led3 = 0;
pedlerw 0:b1f00c299273 139 led4 = 0;
pedlerw 0:b1f00c299273 140 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 141 ///////////////////////////New User Interface//////////////////////////////////
pedlerw 0:b1f00c299273 142 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 143
pedlerw 0:b1f00c299273 144 while(1) {
pedlerw 0:b1f00c299273 145 //-Prompt user with main menu-
pedlerw 0:b1f00c299273 146 switch(user_instruction) {
pedlerw 0:b1f00c299273 147 case 0:
pedlerw 0:b1f00c299273 148 user_instruction = -1;
pedlerw 0:b1f00c299273 149 break;
pedlerw 0:b1f00c299273 150 case 1:
pedlerw 0:b1f00c299273 151 //Calibrate Motor//
pedlerw 0:b1f00c299273 152 calibrate_linear();
pedlerw 0:b1f00c299273 153 //Go to operate motor//
pedlerw 0:b1f00c299273 154 user_instruction = 4; //Go to operation menu//
pedlerw 0:b1f00c299273 155 break;
pedlerw 0:b1f00c299273 156 case 2:
pedlerw 0:b1f00c299273 157 //Use old calibration file and begin operation
pedlerw 0:b1f00c299273 158 //Read file to fill in max and min//
pedlerw 0:b1f00c299273 159 read_file();
pedlerw 0:b1f00c299273 160 user_instruction = 4; //Go to operation menu//
pedlerw 0:b1f00c299273 161 break;
pedlerw 0:b1f00c299273 162 case 3:
pedlerw 0:b1f00c299273 163 display_mode();
pedlerw 0:b1f00c299273 164 user_instruction = -1; //Go to main menu//
pedlerw 0:b1f00c299273 165 break;
pedlerw 0:b1f00c299273 166 case 4:
pedlerw 0:b1f00c299273 167 //Operation Menu//
pedlerw 0:b1f00c299273 168 for(int i=0; i<30; i++) {
pedlerw 0:b1f00c299273 169 xbee.printf("\r\n");
pedlerw 0:b1f00c299273 170 }//End of for
pedlerw 0:b1f00c299273 171 scroll_up(30);
pedlerw 0:b1f00c299273 172 xbee.printf("\r\n\n****Operation Menu****");
pedlerw 0:b1f00c299273 173 xbee.printf("\r\nEnter a position number from 2 to 38: ");
pedlerw 0:b1f00c299273 174 xbee.printf("\r\nTo get back to the Main Menu press 0 and <Enter>.");
pedlerw 0:b1f00c299273 175 scroll_up(25);
pedlerw 0:b1f00c299273 176 desired_location = error_check();
pedlerw 0:b1f00c299273 177 if(desired_location == 0) {
pedlerw 0:b1f00c299273 178 user_instruction = -1;
pedlerw 0:b1f00c299273 179 break;
pedlerw 0:b1f00c299273 180 }//End of if
pedlerw 0:b1f00c299273 181 else if(desired_location > 40) {
pedlerw 0:b1f00c299273 182 user_instruction = 4;
pedlerw 0:b1f00c299273 183 xbee.printf("\r\nUser input was out of bounds");
pedlerw 0:b1f00c299273 184 xbee.printf("\r\nPlease try again");
pedlerw 0:b1f00c299273 185 scroll_up(20);
pedlerw 0:b1f00c299273 186 wait(slow);
pedlerw 0:b1f00c299273 187 break;
pedlerw 0:b1f00c299273 188 }//End of else if
pedlerw 0:b1f00c299273 189 else if(desired_location < 0) {
pedlerw 0:b1f00c299273 190 user_instruction = 4;
pedlerw 0:b1f00c299273 191 xbee.printf("\r\nUser input was out of bounds");
pedlerw 0:b1f00c299273 192 xbee.printf("\r\nPlease try again");
pedlerw 0:b1f00c299273 193 scroll_up(20);
pedlerw 0:b1f00c299273 194 wait(slow);
pedlerw 0:b1f00c299273 195 break;
pedlerw 0:b1f00c299273 196 }//End of else if
pedlerw 0:b1f00c299273 197 xbee.printf("\r\nEnter a rotary angle from 0 to 360");
pedlerw 0:b1f00c299273 198 scroll_up(25);
pedlerw 0:b1f00c299273 199 desired_angle = error_check();
pedlerw 0:b1f00c299273 200 desired_angle = get_user_angle(desired_angle);
pedlerw 0:b1f00c299273 201 xbee.printf("\r\nGoing to position %d", desired_location);
pedlerw 0:b1f00c299273 202 //Pass desired_location to move_linear();
pedlerw 0:b1f00c299273 203 //On normal exit move_linear returns 4... causes repeat of operation menu//
pedlerw 0:b1f00c299273 204 user_instruction = move_linear(desired_location);
pedlerw 0:b1f00c299273 205 //Go to the desired angle//
pedlerw 0:b1f00c299273 206 xbee.printf("\r\nGoing to angle %d", desired_angle);
pedlerw 0:b1f00c299273 207 go_to_angle(desired_angle);
pedlerw 0:b1f00c299273 208 break;
pedlerw 0:b1f00c299273 209 default:
pedlerw 0:b1f00c299273 210 //MAIN MENU//
pedlerw 0:b1f00c299273 211 scroll_up(30);
pedlerw 0:b1f00c299273 212 xbee.printf("\r\n\n****Main Menu****");
pedlerw 0:b1f00c299273 213 xbee.printf("\r\nFor Calibration press 1 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 214 xbee.printf("\r\nTo enter a desired location press 2 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 215 xbee.printf("\r\nFor display mode press 3 and\r\n<Enter>.");
pedlerw 0:b1f00c299273 216 scroll_up(22);
pedlerw 0:b1f00c299273 217 user_instruction = error_check();
pedlerw 0:b1f00c299273 218 break;
pedlerw 0:b1f00c299273 219 }//End of switch
pedlerw 0:b1f00c299273 220 }//End of while(1)//
pedlerw 0:b1f00c299273 221 }//End of main
pedlerw 0:b1f00c299273 222 /*******************************************************************************
pedlerw 0:b1f00c299273 223 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 224 /////////////////////////////FUNCITONS/////////////////////////////////////////
pedlerw 0:b1f00c299273 225 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 226 *******************************************************************************/
pedlerw 0:b1f00c299273 227 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 228 ////////////////////////////move_all_left//////////////////////////////////////
pedlerw 0:b1f00c299273 229 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 230 //This function accepts an interger value and moves the forcer to the left//
pedlerw 0:b1f00c299273 231 //The interger number of coil sets//
pedlerw 0:b1f00c299273 232 void move_all_left(int set)
pedlerw 0:b1f00c299273 233 {
pedlerw 0:b1f00c299273 234 //Declartions and initialization//
pedlerw 0:b1f00c299273 235 linear0 = 1; //Coil1 Mod0
pedlerw 0:b1f00c299273 236 linear1 = 1; //Coil2 Mod1
pedlerw 0:b1f00c299273 237 linear2 = 1; //Coil3 Mod2
pedlerw 0:b1f00c299273 238 linear3 = 1; //Coil4 Mod3
pedlerw 0:b1f00c299273 239 linear4 = 1; //Coil5 Mod4
pedlerw 0:b1f00c299273 240 linear5 = 1; //Coil6 Mod5
pedlerw 0:b1f00c299273 241 linear6 = 1; //Coil7 Mod6
pedlerw 0:b1f00c299273 242
pedlerw 0:b1f00c299273 243 //LEFT//
pedlerw 0:b1f00c299273 244 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 245 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 246 wait(fast);
pedlerw 0:b1f00c299273 247 linear4 =! linear4;
pedlerw 0:b1f00c299273 248 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 249 wait(fast);
pedlerw 0:b1f00c299273 250 linear5 =! linear5;
pedlerw 0:b1f00c299273 251 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 252 wait(fast);
pedlerw 0:b1f00c299273 253 linear6 =! linear6;
pedlerw 0:b1f00c299273 254 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 255 wait(fast);
pedlerw 0:b1f00c299273 256 linear0 =! linear0;
pedlerw 0:b1f00c299273 257 linear1 =! linear1; //Coil 2
pedlerw 0:b1f00c299273 258 wait(fast);
pedlerw 0:b1f00c299273 259 linear1 =! linear1;
pedlerw 0:b1f00c299273 260 linear2 =! linear2; //Coil 3
pedlerw 0:b1f00c299273 261 wait(fast);
pedlerw 0:b1f00c299273 262 linear2 =! linear2;
pedlerw 0:b1f00c299273 263 linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 264 wait(fast);
pedlerw 0:b1f00c299273 265 linear3 =! linear3;
pedlerw 0:b1f00c299273 266 }//End of For loop
pedlerw 0:b1f00c299273 267 return;
pedlerw 0:b1f00c299273 268 }//End of Funciton
pedlerw 0:b1f00c299273 269
pedlerw 0:b1f00c299273 270 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 271 ////////////////////////////move_all_right/////////////////////////////////////
pedlerw 0:b1f00c299273 272 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 273 //This function accepts an interger value and moves the forcer to the right//
pedlerw 0:b1f00c299273 274 //The interger number of coil sets//
pedlerw 0:b1f00c299273 275 void move_all_right(int set)
pedlerw 0:b1f00c299273 276 {
pedlerw 0:b1f00c299273 277 //Declartions and initialization//
pedlerw 0:b1f00c299273 278 linear0 = 1; //Coil1 Mod0
pedlerw 0:b1f00c299273 279 linear1 = 1; //Coil2 Mod1
pedlerw 0:b1f00c299273 280 linear2 = 1; //Coil3 Mod2
pedlerw 0:b1f00c299273 281 linear3 = 1; //Coil4 Mod3
pedlerw 0:b1f00c299273 282 linear4 = 1; //Coil5 Mod4
pedlerw 0:b1f00c299273 283 linear5 = 1; //Coil6 Mod5
pedlerw 0:b1f00c299273 284 linear6 = 1; //Coil7 Mod6
pedlerw 0:b1f00c299273 285
pedlerw 0:b1f00c299273 286 //RIGHT//
pedlerw 0:b1f00c299273 287 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 288 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 289 wait(fast);
pedlerw 0:b1f00c299273 290 linear0 =! linear0;
pedlerw 0:b1f00c299273 291 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 292 wait(fast);
pedlerw 0:b1f00c299273 293 linear6 =! linear6;
pedlerw 0:b1f00c299273 294 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 295 wait(fast);
pedlerw 0:b1f00c299273 296 linear5 =! linear5;
pedlerw 0:b1f00c299273 297 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 298 wait(fast);
pedlerw 0:b1f00c299273 299 linear4 =! linear4;
pedlerw 0:b1f00c299273 300 linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 301 wait(fast);
pedlerw 0:b1f00c299273 302 linear3 =! linear3;
pedlerw 0:b1f00c299273 303 linear2 =! linear2; //Coil 3
pedlerw 0:b1f00c299273 304 wait(fast);
pedlerw 0:b1f00c299273 305 linear2 =! linear2;
pedlerw 0:b1f00c299273 306 linear1 =! linear1; //Coil 2
pedlerw 0:b1f00c299273 307 wait(fast);
pedlerw 0:b1f00c299273 308 linear1 =! linear1;
pedlerw 0:b1f00c299273 309 }//End of For loop
pedlerw 0:b1f00c299273 310 return;
pedlerw 0:b1f00c299273 311 }//End of Funciton
pedlerw 0:b1f00c299273 312
pedlerw 0:b1f00c299273 313 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 314 ////////////////////////////get_Lsensor_min////////////////////////////////////
pedlerw 0:b1f00c299273 315 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 316 //returns the min distance value of left sensor when called//
pedlerw 0:b1f00c299273 317 float get_Lsensor_min()
pedlerw 0:b1f00c299273 318 {
pedlerw 0:b1f00c299273 319 //Declarations and initializations//
pedlerw 0:b1f00c299273 320 float min = MAX;
pedlerw 0:b1f00c299273 321 float read;
pedlerw 0:b1f00c299273 322 for(int i=0; i<large_num; i++) {
pedlerw 0:b1f00c299273 323 wait(sensor_set);
pedlerw 0:b1f00c299273 324 read = distanceL.read();
pedlerw 0:b1f00c299273 325 if(read < min) {
pedlerw 0:b1f00c299273 326 min = read;
pedlerw 0:b1f00c299273 327 }//End of if
pedlerw 0:b1f00c299273 328 else {
pedlerw 0:b1f00c299273 329 wait(MIN);
pedlerw 0:b1f00c299273 330 }//End of else
pedlerw 0:b1f00c299273 331 }//End of for
pedlerw 0:b1f00c299273 332 return(min);
pedlerw 0:b1f00c299273 333 }//End of Function
pedlerw 0:b1f00c299273 334
pedlerw 0:b1f00c299273 335 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 336 //////////////////////////////get_Lsensor_max//////////////////////////////////
pedlerw 0:b1f00c299273 337 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 338 //returns the max distance value of the left sensor when called//
pedlerw 0:b1f00c299273 339 float get_Lsensor_max()
pedlerw 0:b1f00c299273 340 {
pedlerw 0:b1f00c299273 341 //Declarations and initializations//
pedlerw 0:b1f00c299273 342 float max = MIN;
pedlerw 0:b1f00c299273 343 float read;
pedlerw 0:b1f00c299273 344 for(int i=0; i<large_num; i++) {
pedlerw 0:b1f00c299273 345 wait(sensor_set);
pedlerw 0:b1f00c299273 346 read = distanceL.read();
pedlerw 0:b1f00c299273 347 if(read > max) {
pedlerw 0:b1f00c299273 348 max = read;
pedlerw 0:b1f00c299273 349 }//End of if
pedlerw 0:b1f00c299273 350 else {
pedlerw 0:b1f00c299273 351 wait(MIN);
pedlerw 0:b1f00c299273 352 }//End of else
pedlerw 0:b1f00c299273 353 }//End of for
pedlerw 0:b1f00c299273 354 return(max);
pedlerw 0:b1f00c299273 355 }//End of function
pedlerw 0:b1f00c299273 356
pedlerw 0:b1f00c299273 357 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 358 /////////////////////////////get_Rsensor_min///////////////////////////////////
pedlerw 0:b1f00c299273 359 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 360 //Function returns the min value of the right sensor when called//
pedlerw 0:b1f00c299273 361 float get_Rsensor_min()
pedlerw 0:b1f00c299273 362 {
pedlerw 0:b1f00c299273 363 //Declarations and initializations//
pedlerw 0:b1f00c299273 364 float min = MAX;
pedlerw 0:b1f00c299273 365 float read;
pedlerw 0:b1f00c299273 366 for(int i=0; i<large_num; i++) {
pedlerw 0:b1f00c299273 367 wait(sensor_set);
pedlerw 0:b1f00c299273 368 read = distanceR.read();
pedlerw 0:b1f00c299273 369 if(read < min) {
pedlerw 0:b1f00c299273 370 min = read;
pedlerw 0:b1f00c299273 371 }//End of if
pedlerw 0:b1f00c299273 372 else {
pedlerw 0:b1f00c299273 373 wait(MIN);
pedlerw 0:b1f00c299273 374 }//End of else
pedlerw 0:b1f00c299273 375 }//End of for
pedlerw 0:b1f00c299273 376 return(min);
pedlerw 0:b1f00c299273 377 }//End of function
pedlerw 0:b1f00c299273 378
pedlerw 0:b1f00c299273 379 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 380 ///////////////////////////////get_Rsensor_max/////////////////////////////////
pedlerw 0:b1f00c299273 381 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 382 float get_Rsensor_max()
pedlerw 0:b1f00c299273 383 {
pedlerw 0:b1f00c299273 384 //Declarations and initializations//
pedlerw 0:b1f00c299273 385 float max = MIN;
pedlerw 0:b1f00c299273 386 float read;
pedlerw 0:b1f00c299273 387 for(int i=0; i<large_num; i++) {
pedlerw 0:b1f00c299273 388 wait(sensor_set);
pedlerw 0:b1f00c299273 389 read = distanceR.read();
pedlerw 0:b1f00c299273 390 if(read > max) {
pedlerw 0:b1f00c299273 391 max = read;
pedlerw 0:b1f00c299273 392 }//End of if
pedlerw 0:b1f00c299273 393 else {
pedlerw 0:b1f00c299273 394 wait(MIN);
pedlerw 0:b1f00c299273 395 }//End of else
pedlerw 0:b1f00c299273 396 }//End of for
pedlerw 0:b1f00c299273 397 return(max);
pedlerw 0:b1f00c299273 398 }//End of function
pedlerw 0:b1f00c299273 399
pedlerw 0:b1f00c299273 400 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 401 ///////////////////////////////move_one_right//////////////////////////////////
pedlerw 0:b1f00c299273 402 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 403 //Function takes current position and moves one to the right
pedlerw 0:b1f00c299273 404 void move_one_right(int position)
pedlerw 0:b1f00c299273 405 {
pedlerw 0:b1f00c299273 406 //Declarations and initializations//
pedlerw 0:b1f00c299273 407 int current_coil = abs(position % seven);
pedlerw 0:b1f00c299273 408 //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 0:b1f00c299273 409 switch(current_coil) {
pedlerw 0:b1f00c299273 410 case 0:
pedlerw 0:b1f00c299273 411 linear6 = 1;
pedlerw 0:b1f00c299273 412 linear5 = 1;
pedlerw 0:b1f00c299273 413 linear4 = 1;
pedlerw 0:b1f00c299273 414 linear3 = 1;
pedlerw 0:b1f00c299273 415 linear2 = 1;
pedlerw 0:b1f00c299273 416 linear1 = 1;
pedlerw 0:b1f00c299273 417 linear0 = 0; //Fire Coil 1//
pedlerw 0:b1f00c299273 418 break;
pedlerw 0:b1f00c299273 419 case 1:
pedlerw 0:b1f00c299273 420 linear6 = 0; //Fire Coil 7//
pedlerw 0:b1f00c299273 421 linear5 = 1;
pedlerw 0:b1f00c299273 422 linear4 = 1;
pedlerw 0:b1f00c299273 423 linear3 = 1;
pedlerw 0:b1f00c299273 424 linear2 = 1;
pedlerw 0:b1f00c299273 425 linear1 = 1;
pedlerw 0:b1f00c299273 426 linear0 = 1;
pedlerw 0:b1f00c299273 427 break;
pedlerw 0:b1f00c299273 428 case 2:
pedlerw 0:b1f00c299273 429 linear6 = 1;
pedlerw 0:b1f00c299273 430 linear5 = 0; //Fire Coil 6//
pedlerw 0:b1f00c299273 431 linear4 = 1;
pedlerw 0:b1f00c299273 432 linear3 = 1;
pedlerw 0:b1f00c299273 433 linear2 = 1;
pedlerw 0:b1f00c299273 434 linear1 = 1;
pedlerw 0:b1f00c299273 435 linear0 = 1;
pedlerw 0:b1f00c299273 436 break;
pedlerw 0:b1f00c299273 437 case 3:
pedlerw 0:b1f00c299273 438 linear6 = 1;
pedlerw 0:b1f00c299273 439 linear5 = 1;
pedlerw 0:b1f00c299273 440 linear4 = 0; //Fire Coil 5//
pedlerw 0:b1f00c299273 441 linear3 = 1;
pedlerw 0:b1f00c299273 442 linear2 = 1;
pedlerw 0:b1f00c299273 443 linear1 = 1;
pedlerw 0:b1f00c299273 444 linear0 = 1;
pedlerw 0:b1f00c299273 445 break;
pedlerw 0:b1f00c299273 446 case 4:
pedlerw 0:b1f00c299273 447 linear6 = 1;
pedlerw 0:b1f00c299273 448 linear5 = 1;
pedlerw 0:b1f00c299273 449 linear4 = 1;
pedlerw 0:b1f00c299273 450 linear3 = 0; //Fire Coil 4//
pedlerw 0:b1f00c299273 451 linear2 = 1;
pedlerw 0:b1f00c299273 452 linear1 = 1;
pedlerw 0:b1f00c299273 453 linear0 = 1;
pedlerw 0:b1f00c299273 454 break;
pedlerw 0:b1f00c299273 455 case 5:
pedlerw 0:b1f00c299273 456 linear6 = 1;
pedlerw 0:b1f00c299273 457 linear5 = 1;
pedlerw 0:b1f00c299273 458 linear4 = 1;
pedlerw 0:b1f00c299273 459 linear3 = 1;
pedlerw 0:b1f00c299273 460 linear2 = 0; //Fire Coil 3//
pedlerw 0:b1f00c299273 461 linear1 = 1;
pedlerw 0:b1f00c299273 462 linear0 = 1;
pedlerw 0:b1f00c299273 463 break;
pedlerw 0:b1f00c299273 464 case 6:
pedlerw 0:b1f00c299273 465 linear6 = 1;
pedlerw 0:b1f00c299273 466 linear5 = 1;
pedlerw 0:b1f00c299273 467 linear4 = 1;
pedlerw 0:b1f00c299273 468 linear3 = 1;
pedlerw 0:b1f00c299273 469 linear2 = 1;
pedlerw 0:b1f00c299273 470 linear1 = 0; //Fire Coil 2//
pedlerw 0:b1f00c299273 471 linear0 = 1;
pedlerw 0:b1f00c299273 472 break;
pedlerw 0:b1f00c299273 473 default:
pedlerw 0:b1f00c299273 474 wait(MIN);
pedlerw 0:b1f00c299273 475 }//End of switch
pedlerw 0:b1f00c299273 476 }//End of function
pedlerw 0:b1f00c299273 477
pedlerw 0:b1f00c299273 478 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 479 ///////////////////////////////move_one_left//////////////////////////////////
pedlerw 0:b1f00c299273 480 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 481 //Function takes current position and moves one to the left
pedlerw 0:b1f00c299273 482 void move_one_left(int position)
pedlerw 0:b1f00c299273 483 {
pedlerw 0:b1f00c299273 484 //Declarations and initializations//
pedlerw 0:b1f00c299273 485 int current_coil = position % seven;
pedlerw 0:b1f00c299273 486 //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 0:b1f00c299273 487 switch(current_coil) {
pedlerw 0:b1f00c299273 488 case 0:
pedlerw 0:b1f00c299273 489 linear6 = 1;
pedlerw 0:b1f00c299273 490 linear5 = 0; //Fire Coil 6//
pedlerw 0:b1f00c299273 491 linear4 = 1;
pedlerw 0:b1f00c299273 492 linear3 = 1;
pedlerw 0:b1f00c299273 493 linear2 = 1;
pedlerw 0:b1f00c299273 494 linear1 = 1;
pedlerw 0:b1f00c299273 495 linear0 = 1;
pedlerw 0:b1f00c299273 496 break;
pedlerw 0:b1f00c299273 497 case 1:
pedlerw 0:b1f00c299273 498 linear6 = 1;
pedlerw 0:b1f00c299273 499 linear5 = 1;
pedlerw 0:b1f00c299273 500 linear4 = 0; //Fire Coil 5//
pedlerw 0:b1f00c299273 501 linear3 = 1;
pedlerw 0:b1f00c299273 502 linear2 = 1;
pedlerw 0:b1f00c299273 503 linear1 = 1;
pedlerw 0:b1f00c299273 504 linear0 = 1;
pedlerw 0:b1f00c299273 505 break;
pedlerw 0:b1f00c299273 506 case 2:
pedlerw 0:b1f00c299273 507 linear6 = 1;
pedlerw 0:b1f00c299273 508 linear5 = 1;
pedlerw 0:b1f00c299273 509 linear4 = 1;
pedlerw 0:b1f00c299273 510 linear3 = 0; //Fire Coil 4//
pedlerw 0:b1f00c299273 511 linear2 = 1;
pedlerw 0:b1f00c299273 512 linear1 = 1;
pedlerw 0:b1f00c299273 513 linear0 = 1;
pedlerw 0:b1f00c299273 514 break;
pedlerw 0:b1f00c299273 515 case 3:
pedlerw 0:b1f00c299273 516 linear6 = 1;
pedlerw 0:b1f00c299273 517 linear5 = 1;
pedlerw 0:b1f00c299273 518 linear4 = 1;
pedlerw 0:b1f00c299273 519 linear3 = 1;
pedlerw 0:b1f00c299273 520 linear2 = 0; //Fire Coil 3//
pedlerw 0:b1f00c299273 521 linear1 = 1;
pedlerw 0:b1f00c299273 522 linear0 = 1;
pedlerw 0:b1f00c299273 523 break;
pedlerw 0:b1f00c299273 524 case 4:
pedlerw 0:b1f00c299273 525 linear6 = 1;
pedlerw 0:b1f00c299273 526 linear5 = 1;
pedlerw 0:b1f00c299273 527 linear4 = 1;
pedlerw 0:b1f00c299273 528 linear3 = 1;
pedlerw 0:b1f00c299273 529 linear2 = 1;
pedlerw 0:b1f00c299273 530 linear1 = 0; //Fire Coil 2//
pedlerw 0:b1f00c299273 531 linear0 = 1;
pedlerw 0:b1f00c299273 532 break;
pedlerw 0:b1f00c299273 533 case 5:
pedlerw 0:b1f00c299273 534 linear6 = 1;
pedlerw 0:b1f00c299273 535 linear5 = 1;
pedlerw 0:b1f00c299273 536 linear4 = 1;
pedlerw 0:b1f00c299273 537 linear3 = 1;
pedlerw 0:b1f00c299273 538 linear2 = 1;
pedlerw 0:b1f00c299273 539 linear1 = 1;
pedlerw 0:b1f00c299273 540 linear0 = 0; //Fire Coil 1//
pedlerw 0:b1f00c299273 541 break;
pedlerw 0:b1f00c299273 542 case 6:
pedlerw 0:b1f00c299273 543 linear6 = 0; //Fire Coil 7//
pedlerw 0:b1f00c299273 544 linear5 = 1;
pedlerw 0:b1f00c299273 545 linear4 = 1;
pedlerw 0:b1f00c299273 546 linear3 = 1;
pedlerw 0:b1f00c299273 547 linear2 = 1;
pedlerw 0:b1f00c299273 548 linear1 = 1;
pedlerw 0:b1f00c299273 549 linear0 = 1;
pedlerw 0:b1f00c299273 550 break;
pedlerw 0:b1f00c299273 551 default:
pedlerw 0:b1f00c299273 552 wait(MIN);
pedlerw 0:b1f00c299273 553 }//End of switch
pedlerw 0:b1f00c299273 554 }//End of function
pedlerw 0:b1f00c299273 555
pedlerw 0:b1f00c299273 556 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 557 /////////////////////////////get_position//////////////////////////////////////
pedlerw 0:b1f00c299273 558 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 559
pedlerw 0:b1f00c299273 560 //Function reads current position on track and returns the mod 7//
pedlerw 0:b1f00c299273 561 //Function uses calibration data to determin current position//
pedlerw 0:b1f00c299273 562 int get_position()
pedlerw 0:b1f00c299273 563 {
pedlerw 0:b1f00c299273 564 //Declrations and Initializaitons//
pedlerw 0:b1f00c299273 565 int position = -1;
pedlerw 0:b1f00c299273 566 int count = 0;
pedlerw 0:b1f00c299273 567 while(position == -1) {
pedlerw 0:b1f00c299273 568 wait(slow);
pedlerw 0:b1f00c299273 569 float left_sensor = distanceL.read();
pedlerw 0:b1f00c299273 570 float right_sensor = distanceR.read();
pedlerw 0:b1f00c299273 571 //xbee.printf("\r\nleft_sensor:%f",left_sensor);
pedlerw 0:b1f00c299273 572 //xbee.printf("\r\nright_sensor:%f",right_sensor);
pedlerw 0:b1f00c299273 573 //Use closest sensor//
pedlerw 0:b1f00c299273 574 if(left_sensor > right_sensor) {
pedlerw 0:b1f00c299273 575 //xbee.printf("\r\nleft > right");
pedlerw 0:b1f00c299273 576 //Look at Lsensor data//
pedlerw 0:b1f00c299273 577 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 578 if(left_sensor <= left_max[i] && left_sensor >= left_min[i]) {
pedlerw 0:b1f00c299273 579 position = i;
pedlerw 0:b1f00c299273 580 xbee.printf("\r\nPosition L1 %d",position);
pedlerw 0:b1f00c299273 581 return position;
pedlerw 0:b1f00c299273 582 }//End of if//
pedlerw 0:b1f00c299273 583 else if(left_sensor < left_max[i] && left_sensor > left_max[i+one]) {
pedlerw 0:b1f00c299273 584 position = i+one;
pedlerw 0:b1f00c299273 585 xbee.printf("\r\nPosition L2 %d",position);
pedlerw 0:b1f00c299273 586 return position;
pedlerw 0:b1f00c299273 587 }//End of else if//
pedlerw 0:b1f00c299273 588 else if(left_sensor < left_max[i] && left_sensor > left_min[i+one]) {
pedlerw 0:b1f00c299273 589 position = i;
pedlerw 0:b1f00c299273 590 xbee.printf("\r\nPosition L3 %d",position);
pedlerw 0:b1f00c299273 591 return position;
pedlerw 0:b1f00c299273 592 }//End of else if//
pedlerw 0:b1f00c299273 593 }//End of for//
pedlerw 0:b1f00c299273 594 }//End of if//
pedlerw 0:b1f00c299273 595
pedlerw 0:b1f00c299273 596 else if(right_sensor >= left_sensor) {
pedlerw 0:b1f00c299273 597 xbee.printf("\r\nright > left");
pedlerw 0:b1f00c299273 598 //Look at Rsensor data//
pedlerw 0:b1f00c299273 599 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 600 if(right_sensor <= right_max[i] && right_sensor >= right_min[i]) {
pedlerw 0:b1f00c299273 601 position = i;
pedlerw 0:b1f00c299273 602 xbee.printf("\r\nPosition R1 %d",position);
pedlerw 0:b1f00c299273 603 return position;
pedlerw 0:b1f00c299273 604 }//End of if//
pedlerw 0:b1f00c299273 605 else if(right_sensor > right_max[i] && right_sensor < right_max[i+one]) {
pedlerw 0:b1f00c299273 606 position = i+one;
pedlerw 0:b1f00c299273 607 xbee.printf("\r\nPosition R2 %d",position);
pedlerw 0:b1f00c299273 608 return position;
pedlerw 0:b1f00c299273 609 }//End of else if//
pedlerw 0:b1f00c299273 610 else if(right_sensor > right_max[i] && right_sensor < right_min[i+one]) {
pedlerw 0:b1f00c299273 611 position = i;
pedlerw 0:b1f00c299273 612 xbee.printf("\r\nPosition R3 %d",position);
pedlerw 0:b1f00c299273 613 return position;
pedlerw 0:b1f00c299273 614 }//End of else if//
pedlerw 0:b1f00c299273 615 }//End of for//
pedlerw 0:b1f00c299273 616 }//End of else if//
pedlerw 0:b1f00c299273 617 count++;
pedlerw 0:b1f00c299273 618 if(count > five) {
pedlerw 0:b1f00c299273 619 return(-2);
pedlerw 0:b1f00c299273 620 }//End of if//
pedlerw 0:b1f00c299273 621 }//End of while(position....
pedlerw 0:b1f00c299273 622 return position;
pedlerw 0:b1f00c299273 623 }//End of function
pedlerw 0:b1f00c299273 624
pedlerw 0:b1f00c299273 625 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 626 ////////////////////////////////CALIBRATE//////////////////////////////////////
pedlerw 0:b1f00c299273 627 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 628 /*
pedlerw 0:b1f00c299273 629 -Calibrate Motor
pedlerw 0:b1f00c299273 630 1. Move all the way to the LEFT side of the track.
pedlerw 0:b1f00c299273 631 2. Get max and mins from both distance sensors.
pedlerw 0:b1f00c299273 632 3. Store the max and mins in various arrays.
pedlerw 0:b1f00c299273 633 4. Move one position to the RIGHT and repeat for entire track.
pedlerw 0:b1f00c299273 634 5. Once the entire track has been recorded export to a txt file on the mbed.
pedlerw 0:b1f00c299273 635 Format for txt file:
pedlerw 0:b1f00c299273 636 Position # :Left Low :Left High :Right Low :Right High
pedlerw 0:b1f00c299273 637 0 :#### :#### :#### :####
pedlerw 0:b1f00c299273 638 1 (3 \t) :#### (2 \t)...
pedlerw 0:b1f00c299273 639 .
pedlerw 0:b1f00c299273 640 .
pedlerw 0:b1f00c299273 641 #EOF
pedlerw 0:b1f00c299273 642 */
pedlerw 0:b1f00c299273 643 void calibrate_linear()
pedlerw 0:b1f00c299273 644 {
pedlerw 0:b1f00c299273 645 //1.Move all the way to the LEFT side of the track//
pedlerw 0:b1f00c299273 646 move_all_left(six);
pedlerw 0:b1f00c299273 647 //2.Get Sensor Data//
pedlerw 0:b1f00c299273 648 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 649 //3.Store in various arrays
pedlerw 0:b1f00c299273 650 wait(slow);
pedlerw 0:b1f00c299273 651 left_min[i] = get_Lsensor_min();
pedlerw 0:b1f00c299273 652 left_max[i] = get_Lsensor_max();
pedlerw 0:b1f00c299273 653 right_min[i] = get_Rsensor_min();
pedlerw 0:b1f00c299273 654 right_max[i] = get_Rsensor_max();
pedlerw 0:b1f00c299273 655 led4 = !led4;
pedlerw 0:b1f00c299273 656 //4.Move to the right one//
pedlerw 0:b1f00c299273 657 move_one_right(i);
pedlerw 0:b1f00c299273 658 }//End of for
pedlerw 0:b1f00c299273 659
pedlerw 0:b1f00c299273 660 //5. Export data to a txt file on the mbed//
pedlerw 0:b1f00c299273 661 FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing
pedlerw 0:b1f00c299273 662 fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n");
pedlerw 0:b1f00c299273 663 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 664 fprintf(fp,"%d\t %f\t %f\t %f\t %f\r\n",i,left_min[i],left_max[i],right_min[i],right_max[i]);
pedlerw 0:b1f00c299273 665 /*
pedlerw 0:b1f00c299273 666 //Example File Handeling Code//
pedlerw 0:b1f00c299273 667 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
pedlerw 0:b1f00c299273 668 fprintf(fp, "Hello World!");
pedlerw 0:b1f00c299273 669 fclose(fp);
pedlerw 0:b1f00c299273 670 */
pedlerw 0:b1f00c299273 671 }//End of for
pedlerw 0:b1f00c299273 672 fclose(fp);
pedlerw 0:b1f00c299273 673 //Export each variable to its own file//
pedlerw 0:b1f00c299273 674 //Write leftmin.txt file//
pedlerw 0:b1f00c299273 675 FILE *fp1 = fopen("/local/leftmin.txt", "w");
pedlerw 0:b1f00c299273 676 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 677 fprintf(fp1,"%f\r\n",left_min[i]);
pedlerw 0:b1f00c299273 678 }//End of for//
pedlerw 0:b1f00c299273 679 fclose(fp1);
pedlerw 0:b1f00c299273 680 //Write leftmax.txt file//
pedlerw 0:b1f00c299273 681 FILE *fp2 = fopen("/local/leftmax.txt", "w");
pedlerw 0:b1f00c299273 682 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 683 fprintf(fp2,"%f\r\n",left_max[i]);
pedlerw 0:b1f00c299273 684 }//End of for//
pedlerw 0:b1f00c299273 685 fclose(fp2);
pedlerw 0:b1f00c299273 686 //Write rightmin.txt file//
pedlerw 0:b1f00c299273 687 FILE *fp3 = fopen("/local/rightmin.txt", "w");
pedlerw 0:b1f00c299273 688 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 689 fprintf(fp3,"%f\r\n",right_min[i]);
pedlerw 0:b1f00c299273 690 }//End of for//
pedlerw 0:b1f00c299273 691 fclose(fp3);
pedlerw 0:b1f00c299273 692 //Write rightmax.txt file//
pedlerw 0:b1f00c299273 693 FILE *fp4 = fopen("/local/rightmax.txt", "w");
pedlerw 0:b1f00c299273 694 for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 695 fprintf(fp4,"%f\r\n",right_max[i]);
pedlerw 0:b1f00c299273 696 }//End of for//
pedlerw 0:b1f00c299273 697 fclose(fp4);
pedlerw 0:b1f00c299273 698
pedlerw 0:b1f00c299273 699 //Set an LED high so we know calibration is complete//
pedlerw 0:b1f00c299273 700 led1 = 1;
pedlerw 0:b1f00c299273 701 //move_all_left(six);
pedlerw 0:b1f00c299273 702 calibrate_rot();
pedlerw 0:b1f00c299273 703 move_all_left(six);
pedlerw 0:b1f00c299273 704 }//End of function
pedlerw 0:b1f00c299273 705
pedlerw 0:b1f00c299273 706 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 707 ////////////////////////////read_file//////////////////////////////////////////
pedlerw 0:b1f00c299273 708 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 709 //read_file reads the text files containing calibration data
pedlerw 0:b1f00c299273 710 void read_file()
pedlerw 0:b1f00c299273 711 {
pedlerw 0:b1f00c299273 712 char line[80];
pedlerw 0:b1f00c299273 713 int i = 0;
pedlerw 0:b1f00c299273 714 FILE *fr1;
pedlerw 0:b1f00c299273 715 fr1 = fopen ("/local/leftmin.txt", "rt");
pedlerw 0:b1f00c299273 716 while(fgets(line, 80, fr1) != NULL) {
pedlerw 0:b1f00c299273 717 sscanf (line, "%f", &left_min[i]);
pedlerw 0:b1f00c299273 718 i++;
pedlerw 0:b1f00c299273 719 }//End of while
pedlerw 0:b1f00c299273 720 fclose(fr1);
pedlerw 0:b1f00c299273 721
pedlerw 0:b1f00c299273 722 i = 0;
pedlerw 0:b1f00c299273 723 FILE *fr2;
pedlerw 0:b1f00c299273 724 fr2 = fopen ("/local/leftmax.txt", "rt");
pedlerw 0:b1f00c299273 725 while(fgets(line, 80, fr2) != NULL) {
pedlerw 0:b1f00c299273 726 sscanf (line, "%f", &left_max[i]);
pedlerw 0:b1f00c299273 727 i++;
pedlerw 0:b1f00c299273 728 }//End of while
pedlerw 0:b1f00c299273 729 fclose(fr2);
pedlerw 0:b1f00c299273 730
pedlerw 0:b1f00c299273 731 i = 0;
pedlerw 0:b1f00c299273 732 FILE *fr3;
pedlerw 0:b1f00c299273 733 fr3 = fopen ("/local/rightmin.txt", "rt");
pedlerw 0:b1f00c299273 734 while(fgets(line, 80, fr3) != NULL) {
pedlerw 0:b1f00c299273 735 sscanf (line, "%f", &right_min[i]);
pedlerw 0:b1f00c299273 736 i++;
pedlerw 0:b1f00c299273 737 }//End of while
pedlerw 0:b1f00c299273 738 fclose(fr3);
pedlerw 0:b1f00c299273 739
pedlerw 0:b1f00c299273 740 i = 0;
pedlerw 0:b1f00c299273 741 FILE *fr4;
pedlerw 0:b1f00c299273 742 fr4 = fopen ("/local/rightmax.txt", "rt");
pedlerw 0:b1f00c299273 743 while(fgets(line, 80, fr4) != NULL) {
pedlerw 0:b1f00c299273 744 sscanf (line, "%f", &right_max[i]);
pedlerw 0:b1f00c299273 745 i++;
pedlerw 0:b1f00c299273 746 }//End of while
pedlerw 0:b1f00c299273 747 fclose(fr4);
pedlerw 0:b1f00c299273 748
pedlerw 0:b1f00c299273 749 i = 0;
pedlerw 0:b1f00c299273 750 FILE *fr5;
pedlerw 0:b1f00c299273 751 fr5 = fopen ("/local/rotary.txt","rt");
pedlerw 0:b1f00c299273 752 while(fgets(line, 80,fr5) != NULL){
pedlerw 0:b1f00c299273 753 sscanf (line, "%f", &rotary_threshold[i]);
pedlerw 0:b1f00c299273 754 i++;
pedlerw 0:b1f00c299273 755 }//End of while
pedlerw 0:b1f00c299273 756
pedlerw 0:b1f00c299273 757 i = 0;
pedlerw 0:b1f00c299273 758 FILE *fr6;
pedlerw 0:b1f00c299273 759 fr6 = fopen ("/local/threash.txt","rt");
pedlerw 0:b1f00c299273 760 while(fgets(line, 80, fr6) != NULL){
pedlerw 0:b1f00c299273 761 sscanf (line, "%f", &threash);
pedlerw 0:b1f00c299273 762 i++;
pedlerw 0:b1f00c299273 763 }//End of while
pedlerw 0:b1f00c299273 764
pedlerw 0:b1f00c299273 765
pedlerw 0:b1f00c299273 766 }//End of function
pedlerw 0:b1f00c299273 767
pedlerw 0:b1f00c299273 768 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 769 //////////////////////////////move_linear//////////////////////////////////////
pedlerw 0:b1f00c299273 770 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 771 //move_linear accepts a desired locaiton and moves the forcer accordingly//
pedlerw 0:b1f00c299273 772 //Returns 4 on normal exit... prompting the user for a new position//
pedlerw 0:b1f00c299273 773 int move_linear(int desired_location)
pedlerw 0:b1f00c299273 774 {
pedlerw 0:b1f00c299273 775 int current_location = get_position();
pedlerw 0:b1f00c299273 776 int difference = current_location - desired_location;
pedlerw 0:b1f00c299273 777 int count = 0;
pedlerw 0:b1f00c299273 778 xbee.printf("\r\nInside move_linear function");
pedlerw 0:b1f00c299273 779 xbee.printf("\r\nCurrent_location %d", current_location);
pedlerw 0:b1f00c299273 780 if(desired_location == -1) {
pedlerw 0:b1f00c299273 781 return -1;
pedlerw 0:b1f00c299273 782 }//End of if
pedlerw 0:b1f00c299273 783 while(current_location != desired_location) {
pedlerw 0:b1f00c299273 784 difference = current_location - desired_location;
pedlerw 0:b1f00c299273 785 xbee.printf("\r\nDifference %d",difference);
pedlerw 0:b1f00c299273 786 while(difference > 0) {
pedlerw 0:b1f00c299273 787 if(count > four) {
pedlerw 0:b1f00c299273 788 for(int i=0; i<difference; i++) {
pedlerw 0:b1f00c299273 789 move_one_left(current_location-one);
pedlerw 0:b1f00c299273 790 current_location--;
pedlerw 0:b1f00c299273 791 }//End of for
pedlerw 0:b1f00c299273 792 }//End of if//
pedlerw 0:b1f00c299273 793 //Move LEFT//
pedlerw 0:b1f00c299273 794 xbee.printf("\r\nMoving Left");
pedlerw 0:b1f00c299273 795 led2 = 1;
pedlerw 0:b1f00c299273 796 wait(medium);
pedlerw 0:b1f00c299273 797 for(int i=0; i<difference; i++) {
pedlerw 0:b1f00c299273 798 move_one_left(current_location);
pedlerw 0:b1f00c299273 799 current_location--;
pedlerw 0:b1f00c299273 800 wait(fast);
pedlerw 0:b1f00c299273 801 }//End of for
pedlerw 0:b1f00c299273 802 current_location = get_position();
pedlerw 0:b1f00c299273 803 difference = current_location - desired_location;
pedlerw 0:b1f00c299273 804 led2 = 0;
pedlerw 0:b1f00c299273 805 count++;
pedlerw 0:b1f00c299273 806 if(current_location == -2) {
pedlerw 0:b1f00c299273 807 return(four);
pedlerw 0:b1f00c299273 808 }//End of if//
pedlerw 0:b1f00c299273 809 }//End of while(greater than zero)//
pedlerw 0:b1f00c299273 810
pedlerw 0:b1f00c299273 811 while(difference < 0) {
pedlerw 0:b1f00c299273 812 if(count > four) {
pedlerw 0:b1f00c299273 813 for(int i=0; i<(abs(difference)); i++) {
pedlerw 0:b1f00c299273 814 move_one_right(current_location+one);
pedlerw 0:b1f00c299273 815 current_location++;
pedlerw 0:b1f00c299273 816 }//End of for//
pedlerw 0:b1f00c299273 817 }//End of if//
pedlerw 0:b1f00c299273 818 //Move RIGHT//
pedlerw 0:b1f00c299273 819 xbee.printf("\r\nMoving Right");
pedlerw 0:b1f00c299273 820 led3 = 1;
pedlerw 0:b1f00c299273 821 wait(medium);
pedlerw 0:b1f00c299273 822 for(int i=0; i<(abs(difference)); i++) {
pedlerw 0:b1f00c299273 823 move_one_right(current_location);
pedlerw 0:b1f00c299273 824 current_location++;
pedlerw 0:b1f00c299273 825 wait(fast);
pedlerw 0:b1f00c299273 826 }//End of for
pedlerw 0:b1f00c299273 827 current_location = get_position();
pedlerw 0:b1f00c299273 828 difference = current_location - desired_location;
pedlerw 0:b1f00c299273 829 led3 = 0;
pedlerw 0:b1f00c299273 830 count++;
pedlerw 0:b1f00c299273 831 if(current_location == -2) {
pedlerw 0:b1f00c299273 832 return(four);
pedlerw 0:b1f00c299273 833 }//End of if//
pedlerw 0:b1f00c299273 834 }//End of while(difference less than 0)//
pedlerw 0:b1f00c299273 835 }//End of while(current not desired)//
pedlerw 0:b1f00c299273 836 return(four);
pedlerw 0:b1f00c299273 837 }//End of function//
pedlerw 0:b1f00c299273 838
pedlerw 0:b1f00c299273 839 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 840 ////////////////////////////error_check////////////////////////////////////////
pedlerw 0:b1f00c299273 841 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 842 //Function does scanf error checking//
pedlerw 0:b1f00c299273 843 //It clears the keyboard buffer incase anything other than numbers a pressed//
pedlerw 0:b1f00c299273 844 int error_check()
pedlerw 0:b1f00c299273 845 {
pedlerw 0:b1f00c299273 846 char not_number;
pedlerw 0:b1f00c299273 847 int number;
pedlerw 0:b1f00c299273 848 int flag;
pedlerw 0:b1f00c299273 849 while ((flag = xbee.scanf("%d", &number)) != EOF) {
pedlerw 0:b1f00c299273 850 if (flag != 1) {
pedlerw 0:b1f00c299273 851 not_number = xbee.getc();
pedlerw 0:b1f00c299273 852 #ifdef DEBUG
pedlerw 0:b1f00c299273 853 xbee.printf("\r\ndegub:%c in input stream, discarding", not_number);
pedlerw 0:b1f00c299273 854 #endif
pedlerw 0:b1f00c299273 855 }//End of if
pedlerw 0:b1f00c299273 856 else {
pedlerw 0:b1f00c299273 857 printf("\r\nRETURN = %d",number);
pedlerw 0:b1f00c299273 858 return(number);
pedlerw 0:b1f00c299273 859 }//End of else//
pedlerw 0:b1f00c299273 860 }//End of while((flag...
pedlerw 0:b1f00c299273 861 return(number);
pedlerw 0:b1f00c299273 862 }//End of function//
pedlerw 0:b1f00c299273 863
pedlerw 0:b1f00c299273 864 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 865 ////////////////////////////////display_mode///////////////////////////////////
pedlerw 0:b1f00c299273 866 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 867 //Function set up to run display mode options//
pedlerw 0:b1f00c299273 868 void display_mode()
pedlerw 0:b1f00c299273 869 {
pedlerw 0:b1f00c299273 870 int option = -1;
pedlerw 0:b1f00c299273 871 int exit = 0;
pedlerw 0:b1f00c299273 872 //char input = 1;
pedlerw 0:b1f00c299273 873 int linear_auto = 3;
pedlerw 0:b1f00c299273 874 int linear_oscill = 3;
pedlerw 0:b1f00c299273 875 int combined = 3;
pedlerw 0:b1f00c299273 876 int high_speed = 3;
pedlerw 0:b1f00c299273 877 int rotary_auto = 3;
pedlerw 0:b1f00c299273 878 int set_up = 0;
pedlerw 0:b1f00c299273 879
pedlerw 0:b1f00c299273 880 while(exit != one) {
pedlerw 0:b1f00c299273 881 switch (option) {
pedlerw 0:b1f00c299273 882
pedlerw 0:b1f00c299273 883 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 884 /////////////////////////////DISPLAY OPTIONS///////////////////////////////////
pedlerw 0:b1f00c299273 885 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 886
pedlerw 0:b1f00c299273 887 case 1:
pedlerw 0:b1f00c299273 888 //LINEAR AUTO PROGRAM//
pedlerw 0:b1f00c299273 889 for(int j=0; j<linear_auto; j++) {
pedlerw 0:b1f00c299273 890 for(int i=0; i<12; i++) {
pedlerw 0:b1f00c299273 891 move_all_left(abs(five-i));
pedlerw 0:b1f00c299273 892 move_all_right(abs(five-i));
pedlerw 0:b1f00c299273 893 }//End of for//
pedlerw 0:b1f00c299273 894 }//End of for//
pedlerw 0:b1f00c299273 895 option = -1;
pedlerw 0:b1f00c299273 896 break;
pedlerw 0:b1f00c299273 897
pedlerw 0:b1f00c299273 898 case 2:
pedlerw 0:b1f00c299273 899 //LINEAR OSCILLATION//
pedlerw 0:b1f00c299273 900 for(int j=0; j<linear_oscill; j++) {
pedlerw 0:b1f00c299273 901 for(int i=0; i<12; i++) {
pedlerw 0:b1f00c299273 902 move_all_leftBobby(abs(five-i));
pedlerw 0:b1f00c299273 903 move_all_rightBobby(abs(five-i));
pedlerw 0:b1f00c299273 904 }//End of for//
pedlerw 0:b1f00c299273 905 }//End of for//
pedlerw 0:b1f00c299273 906 option = -1;
pedlerw 0:b1f00c299273 907 break;
pedlerw 0:b1f00c299273 908
pedlerw 0:b1f00c299273 909 case 3:
pedlerw 0:b1f00c299273 910 //COMBINED//
pedlerw 0:b1f00c299273 911 for(int i=0; i<combined; i++) {
pedlerw 0:b1f00c299273 912 //1.Linear//
pedlerw 0:b1f00c299273 913 move_all_right(five);
pedlerw 0:b1f00c299273 914 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 915 wait(fast);
pedlerw 0:b1f00c299273 916 linear0 =! linear0;
pedlerw 0:b1f00c299273 917 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 918 wait(fast);
pedlerw 0:b1f00c299273 919 linear6 =! linear6;
pedlerw 0:b1f00c299273 920 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 921 wait(fast);
pedlerw 0:b1f00c299273 922 linear5 =! linear5;
pedlerw 0:b1f00c299273 923 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 924 wait(fast);
pedlerw 0:b1f00c299273 925 linear4 =! linear4;
pedlerw 0:b1f00c299273 926 linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 927 wait(fast);
pedlerw 0:b1f00c299273 928 linear3 =! linear3;
pedlerw 0:b1f00c299273 929 wait(0.10);
pedlerw 0:b1f00c299273 930 //2.Rotate//
pedlerw 0:b1f00c299273 931 rotate_all_cw(eight, rot_slow);
pedlerw 0:b1f00c299273 932 wait(0.10);
pedlerw 0:b1f00c299273 933 rotate_all_ccw(eight, rot_slow);
pedlerw 0:b1f00c299273 934 //3.Cork Screw//
pedlerw 0:b1f00c299273 935 cork_screw();
pedlerw 0:b1f00c299273 936 wait(1.00);
pedlerw 0:b1f00c299273 937 //move_all_right(six);
pedlerw 0:b1f00c299273 938 //wait(0.10);
pedlerw 0:b1f00c299273 939 //4.Repeat//
pedlerw 0:b1f00c299273 940 }//End of for loop
pedlerw 0:b1f00c299273 941 option = -1;
pedlerw 0:b1f00c299273 942 break;
pedlerw 0:b1f00c299273 943
pedlerw 0:b1f00c299273 944 case 4:
pedlerw 0:b1f00c299273 945 //HIGH SPEED ROTARY//
pedlerw 0:b1f00c299273 946 rotary0 = 1;
pedlerw 0:b1f00c299273 947 rotary1 = 1;
pedlerw 0:b1f00c299273 948 rotary2 = 1;
pedlerw 0:b1f00c299273 949 rotary3 = 1;
pedlerw 0:b1f00c299273 950 rotary4 = 1;
pedlerw 0:b1f00c299273 951 rotary5 = 1;
pedlerw 0:b1f00c299273 952 for(int i=0; i<high_speed; i++) {
pedlerw 0:b1f00c299273 953 rotary_speed();
pedlerw 0:b1f00c299273 954 }//End of for//
pedlerw 0:b1f00c299273 955 option = -1;
pedlerw 0:b1f00c299273 956 break;
pedlerw 0:b1f00c299273 957 case 5:
pedlerw 0:b1f00c299273 958 //ROTARY AUTO PROGRAM//
pedlerw 0:b1f00c299273 959 for(int i=0; i<rotary_auto; i++) {
pedlerw 0:b1f00c299273 960 rotate_all_ccw(six, rot_fast);
pedlerw 0:b1f00c299273 961 rotate_all_cw(six, rot_fast);
pedlerw 0:b1f00c299273 962 }//End of for//
pedlerw 0:b1f00c299273 963 option = -1;
pedlerw 0:b1f00c299273 964 break;
pedlerw 0:b1f00c299273 965 case 6:
pedlerw 0:b1f00c299273 966 fast_rotary_cw();
pedlerw 0:b1f00c299273 967 option = -1;
pedlerw 0:b1f00c299273 968 break;
pedlerw 0:b1f00c299273 969 case 7:
pedlerw 0:b1f00c299273 970 //SMALL FORCER ROTARY AUTO PROGRAM//
pedlerw 0:b1f00c299273 971 rotary0 = 1;
pedlerw 0:b1f00c299273 972 rotary1 = 1;
pedlerw 0:b1f00c299273 973 rotary2 = 1;
pedlerw 0:b1f00c299273 974 rotary3 = 1;
pedlerw 0:b1f00c299273 975 rotary4 = 1;
pedlerw 0:b1f00c299273 976 rotary5 = 1;
pedlerw 0:b1f00c299273 977 for(int i=0; i<high_speed; i++) {
pedlerw 0:b1f00c299273 978 small_rotary_speed();
pedlerw 0:b1f00c299273 979 }//End of for//
pedlerw 0:b1f00c299273 980 option = -1;
pedlerw 0:b1f00c299273 981 break;
pedlerw 0:b1f00c299273 982 case 9:
pedlerw 0:b1f00c299273 983 //SET UP CASE//
pedlerw 0:b1f00c299273 984 xbee.printf("\r\n****Display Mode Set UP****");
pedlerw 0:b1f00c299273 985 xbee.printf("\r\nChoose a program and press <Enter>.");
pedlerw 0:b1f00c299273 986 xbee.printf("\r\n1. Linear Auto Program");
pedlerw 0:b1f00c299273 987 xbee.printf("\r\n2. Linear Oscillation");
pedlerw 0:b1f00c299273 988 xbee.printf("\r\n3. Combined Operation");
pedlerw 0:b1f00c299273 989 xbee.printf("\r\n4. High Speed Rotary");
pedlerw 0:b1f00c299273 990 xbee.printf("\r\n5. Rotary Auto Program");
pedlerw 0:b1f00c299273 991 scroll_up(22);
pedlerw 0:b1f00c299273 992 set_up = error_check();
pedlerw 0:b1f00c299273 993 switch(set_up) {
pedlerw 0:b1f00c299273 994 case 1:
pedlerw 0:b1f00c299273 995 //Edit LINEAR AUTO PROGRAM//
pedlerw 0:b1f00c299273 996 xbee.printf("\r\nHow many times would you like to run Linear Auto Program?");
pedlerw 0:b1f00c299273 997 scroll_up(27);
pedlerw 0:b1f00c299273 998 linear_auto = error_check();
pedlerw 0:b1f00c299273 999 break;
pedlerw 0:b1f00c299273 1000 case 2:
pedlerw 0:b1f00c299273 1001 //Edit LINEAR OSCILLATION//
pedlerw 0:b1f00c299273 1002 xbee.printf("\r\nHow many times would you like to run Linear Oscillation?");
pedlerw 0:b1f00c299273 1003 scroll_up(27);
pedlerw 0:b1f00c299273 1004 linear_oscill = error_check();
pedlerw 0:b1f00c299273 1005 break;
pedlerw 0:b1f00c299273 1006 case 3:
pedlerw 0:b1f00c299273 1007 //Edit COMBINED OPERATION//
pedlerw 0:b1f00c299273 1008 xbee.printf("\r\nHow many times would you like to run Combined Operation?");
pedlerw 0:b1f00c299273 1009 scroll_up(27);
pedlerw 0:b1f00c299273 1010 combined = error_check();
pedlerw 0:b1f00c299273 1011 break;
pedlerw 0:b1f00c299273 1012 case 4:
pedlerw 0:b1f00c299273 1013 //Edit HIGH SPEED ROTARY//
pedlerw 0:b1f00c299273 1014 xbee.printf("\r\nHow many times would you like to run High Speed Rotary?");
pedlerw 0:b1f00c299273 1015 scroll_up(27);
pedlerw 0:b1f00c299273 1016 high_speed = error_check();
pedlerw 0:b1f00c299273 1017 break;
pedlerw 0:b1f00c299273 1018 case 5:
pedlerw 0:b1f00c299273 1019 //Edit ROTARY AUTO PROGRAM//
pedlerw 0:b1f00c299273 1020 xbee.printf("\r\nHow many times would you like to run Rotary Auto Program?");
pedlerw 0:b1f00c299273 1021 scroll_up(27);
pedlerw 0:b1f00c299273 1022 rotary_auto = error_check();
pedlerw 0:b1f00c299273 1023 break;
pedlerw 0:b1f00c299273 1024 }//End of switch(setup)
pedlerw 0:b1f00c299273 1025 option = -1;
pedlerw 0:b1f00c299273 1026 break;
pedlerw 0:b1f00c299273 1027
pedlerw 0:b1f00c299273 1028 case 0:
pedlerw 0:b1f00c299273 1029 //Return to the main menu//
pedlerw 0:b1f00c299273 1030 exit = one;
pedlerw 0:b1f00c299273 1031 break;
pedlerw 0:b1f00c299273 1032 default:
pedlerw 0:b1f00c299273 1033 option = -1;
pedlerw 0:b1f00c299273 1034 scroll_up(30);
pedlerw 0:b1f00c299273 1035 xbee.printf("\r\n\n****Display Mode****");
pedlerw 0:b1f00c299273 1036 xbee.printf("\r\nTo run LINEAR AUTO PROGRAM press 1 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1037 xbee.printf("\r\nTo run LINEAR OSCILLATION press 2 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1038 xbee.printf("\r\nTo run COMBINED OPERATION press 3 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1039 xbee.printf("\r\nTo run HIGH SPEED ROTARY press 4 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1040 xbee.printf("\r\nTo run ROTARY AUTO PROGRAM press 5 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1041 xbee.printf("\r\nTo run SMALL FORCER SPEED RUN press 6 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1042 //xbee.printf("\r\nTo run SMALL FORCER AUTO ROTARY press 7 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1043 xbee.printf("\r\nFor SET UP press 9 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1044 xbee.printf("\r\nTo return to the previous menu press 0 and \r\n<Enter>.");
pedlerw 0:b1f00c299273 1045 scroll_up(12);
pedlerw 0:b1f00c299273 1046 option = error_check();
pedlerw 0:b1f00c299273 1047 break;
pedlerw 0:b1f00c299273 1048 }//End of switch//
pedlerw 0:b1f00c299273 1049 }//End of while//
pedlerw 0:b1f00c299273 1050 return;
pedlerw 0:b1f00c299273 1051 }//End of function//
pedlerw 0:b1f00c299273 1052
pedlerw 0:b1f00c299273 1053 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1054 ////////////////////////////move_all_leftBobby/////////////////////////////////
pedlerw 0:b1f00c299273 1055 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1056 //This function accepts an interger value and moves the forcer to the left//
pedlerw 0:b1f00c299273 1057 //The interger number of coil sets//
pedlerw 0:b1f00c299273 1058 void move_all_leftBobby(int set)
pedlerw 0:b1f00c299273 1059 {
pedlerw 0:b1f00c299273 1060 //Declartions and initialization//
pedlerw 0:b1f00c299273 1061 linear0 = 1; //Coil1 Mod0
pedlerw 0:b1f00c299273 1062 linear1 = 1; //Coil2 Mod1
pedlerw 0:b1f00c299273 1063 linear2 = 1; //Coil3 Mod2
pedlerw 0:b1f00c299273 1064 linear3 = 1; //Coil4 Mod3
pedlerw 0:b1f00c299273 1065 linear4 = 1; //Coil5 Mod4
pedlerw 0:b1f00c299273 1066 linear5 = 1; //Coil6 Mod5
pedlerw 0:b1f00c299273 1067 linear6 = 1; //Coil7 Mod6
pedlerw 0:b1f00c299273 1068
pedlerw 0:b1f00c299273 1069 //LEFT//
pedlerw 0:b1f00c299273 1070 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 1071 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 1072 wait(fast);
pedlerw 0:b1f00c299273 1073 linear4 =! linear4;
pedlerw 0:b1f00c299273 1074 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 1075 wait(fast);
pedlerw 0:b1f00c299273 1076 linear5 =! linear5;
pedlerw 0:b1f00c299273 1077 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 1078 wait(fast);
pedlerw 0:b1f00c299273 1079 linear6 =! linear6;
pedlerw 0:b1f00c299273 1080 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 1081 wait(fast);
pedlerw 0:b1f00c299273 1082 linear0 =! linear0;
pedlerw 0:b1f00c299273 1083 linear1 =! linear1; //Coil 2
pedlerw 0:b1f00c299273 1084 wait(fast);
pedlerw 0:b1f00c299273 1085 linear1 =! linear1;
pedlerw 0:b1f00c299273 1086 linear2 =! linear2; //Coil 3
pedlerw 0:b1f00c299273 1087 wait(fast);
pedlerw 0:b1f00c299273 1088 linear2 =! linear2;
pedlerw 0:b1f00c299273 1089 linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 1090 wait(fast);
pedlerw 0:b1f00c299273 1091 linear3 =! linear3;
pedlerw 0:b1f00c299273 1092 }//End of For loop
pedlerw 0:b1f00c299273 1093
pedlerw 0:b1f00c299273 1094 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 1095 wait(fast);
pedlerw 0:b1f00c299273 1096 linear4 =! linear4;
pedlerw 0:b1f00c299273 1097 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 1098 wait(fast);
pedlerw 0:b1f00c299273 1099 linear5 =! linear5;
pedlerw 0:b1f00c299273 1100 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 1101 wait(fast);
pedlerw 0:b1f00c299273 1102 linear6 =! linear6;
pedlerw 0:b1f00c299273 1103 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 1104 wait(fast);
pedlerw 0:b1f00c299273 1105 linear0 =! linear0;
pedlerw 0:b1f00c299273 1106 //linear1 =! linear1; //Coil 2
pedlerw 0:b1f00c299273 1107 //wait(fast);
pedlerw 0:b1f00c299273 1108 //linear1 =! linear1;
pedlerw 0:b1f00c299273 1109
pedlerw 0:b1f00c299273 1110 return;
pedlerw 0:b1f00c299273 1111 }//End of Funciton
pedlerw 0:b1f00c299273 1112
pedlerw 0:b1f00c299273 1113 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1114 ////////////////////////////move_all_rightBobby////////////////////////////////
pedlerw 0:b1f00c299273 1115 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1116 //This function accepts an interger value and moves the forcer to the right//
pedlerw 0:b1f00c299273 1117 //The interger number of coil sets//
pedlerw 0:b1f00c299273 1118 void move_all_rightBobby(int set)
pedlerw 0:b1f00c299273 1119 {
pedlerw 0:b1f00c299273 1120 //Declartions and initialization//
pedlerw 0:b1f00c299273 1121 linear0 = 1; //Coil1 Mod0
pedlerw 0:b1f00c299273 1122 linear1 = 1; //Coil2 Mod1
pedlerw 0:b1f00c299273 1123 linear2 = 1; //Coil3 Mod2
pedlerw 0:b1f00c299273 1124 linear3 = 1; //Coil4 Mod3
pedlerw 0:b1f00c299273 1125 linear4 = 1; //Coil5 Mod4
pedlerw 0:b1f00c299273 1126 linear5 = 1; //Coil6 Mod5
pedlerw 0:b1f00c299273 1127 linear6 = 1; //Coil7 Mod6
pedlerw 0:b1f00c299273 1128
pedlerw 0:b1f00c299273 1129 //RIGHT//
pedlerw 0:b1f00c299273 1130 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 1131 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 1132 wait(fast);
pedlerw 0:b1f00c299273 1133 linear0 =! linear0;
pedlerw 0:b1f00c299273 1134 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 1135 wait(fast);
pedlerw 0:b1f00c299273 1136 linear6 =! linear6;
pedlerw 0:b1f00c299273 1137 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 1138 wait(fast);
pedlerw 0:b1f00c299273 1139 linear5 =! linear5;
pedlerw 0:b1f00c299273 1140 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 1141 wait(fast);
pedlerw 0:b1f00c299273 1142 linear4 =! linear4;
pedlerw 0:b1f00c299273 1143 linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 1144 wait(fast);
pedlerw 0:b1f00c299273 1145 linear3 =! linear3;
pedlerw 0:b1f00c299273 1146 linear2 =! linear2; //Coil 3
pedlerw 0:b1f00c299273 1147 wait(fast);
pedlerw 0:b1f00c299273 1148 linear2 =! linear2;
pedlerw 0:b1f00c299273 1149 linear1 =! linear1; //Coil 2
pedlerw 0:b1f00c299273 1150 wait(fast);
pedlerw 0:b1f00c299273 1151 linear1 =! linear1;
pedlerw 0:b1f00c299273 1152 }//End of For loop
pedlerw 0:b1f00c299273 1153
pedlerw 0:b1f00c299273 1154 linear0 =! linear0; //Coil 1
pedlerw 0:b1f00c299273 1155 wait(fast);
pedlerw 0:b1f00c299273 1156 linear0 =! linear0;
pedlerw 0:b1f00c299273 1157 linear6 =! linear6; //Coil 7
pedlerw 0:b1f00c299273 1158 wait(fast);
pedlerw 0:b1f00c299273 1159 linear6 =! linear6;
pedlerw 0:b1f00c299273 1160 linear5 =! linear5; //Coil 6
pedlerw 0:b1f00c299273 1161 wait(fast);
pedlerw 0:b1f00c299273 1162 linear5 =! linear5;
pedlerw 0:b1f00c299273 1163 linear4 =! linear4; //Coil 5
pedlerw 0:b1f00c299273 1164 wait(fast);
pedlerw 0:b1f00c299273 1165 linear4 =! linear4;
pedlerw 0:b1f00c299273 1166 //linear3 =! linear3; //Coil 4
pedlerw 0:b1f00c299273 1167 //wait(fast);
pedlerw 0:b1f00c299273 1168 //linear3 =! linear3;
pedlerw 0:b1f00c299273 1169
pedlerw 0:b1f00c299273 1170
pedlerw 0:b1f00c299273 1171 return;
pedlerw 0:b1f00c299273 1172 }//End of Funciton
pedlerw 0:b1f00c299273 1173
pedlerw 0:b1f00c299273 1174 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1175 //////////////////////////////scroll_up()//////////////////////////////////////
pedlerw 0:b1f00c299273 1176 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1177 //Function accepts an integer and scrolls the text on the serial terminal that
pedlerw 0:b1f00c299273 1178 //Number of spaces//
pedlerw 0:b1f00c299273 1179 void scroll_up(int space)
pedlerw 0:b1f00c299273 1180 {
pedlerw 0:b1f00c299273 1181 for(int i=0; i<space; i++) {
pedlerw 0:b1f00c299273 1182 xbee.printf("\r\n");
pedlerw 0:b1f00c299273 1183 }//End of for
pedlerw 0:b1f00c299273 1184 }//End of function
pedlerw 0:b1f00c299273 1185
pedlerw 0:b1f00c299273 1186 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1187 ///////////////////////////////cork_screw()////////////////////////////////////
pedlerw 0:b1f00c299273 1188 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1189 //Function moves linear and rotary at the same time... from right to left//////
pedlerw 0:b1f00c299273 1190 void cork_screw()
pedlerw 0:b1f00c299273 1191 {
pedlerw 0:b1f00c299273 1192 rotary0 = 1;
pedlerw 0:b1f00c299273 1193 rotary1 = 1;
pedlerw 0:b1f00c299273 1194 rotary2 = 1;
pedlerw 0:b1f00c299273 1195 rotary3 = 1;
pedlerw 0:b1f00c299273 1196 rotary4 = 1;
pedlerw 0:b1f00c299273 1197 rotary5 = 1;
pedlerw 0:b1f00c299273 1198 int length = track_length;
pedlerw 0:b1f00c299273 1199 //for(int i=0; i<track_length; i++) {
pedlerw 0:b1f00c299273 1200 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1201 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1202 wait(fast);
pedlerw 0:b1f00c299273 1203 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1204 wait(fast);
pedlerw 0:b1f00c299273 1205 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1206 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1207 wait(fast);
pedlerw 0:b1f00c299273 1208 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1209 wait(fast);
pedlerw 0:b1f00c299273 1210 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1211 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1212 wait(fast);
pedlerw 0:b1f00c299273 1213 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1214 wait(fast);
pedlerw 0:b1f00c299273 1215 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1216 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1217 wait(fast);
pedlerw 0:b1f00c299273 1218 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1219 wait(fast);
pedlerw 0:b1f00c299273 1220 rotary4 = !rotary4;
pedlerw 0:b1f00c299273 1221 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1222 wait(fast);
pedlerw 0:b1f00c299273 1223 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1224 wait(fast);
pedlerw 0:b1f00c299273 1225 rotary5 = !rotary5;
pedlerw 0:b1f00c299273 1226 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1227 wait(fast);
pedlerw 0:b1f00c299273 1228 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1229 wait(fast);
pedlerw 0:b1f00c299273 1230 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1231 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1232 wait(fast);
pedlerw 0:b1f00c299273 1233 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1234 wait(fast);
pedlerw 0:b1f00c299273 1235 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1236 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1237 wait(fast);
pedlerw 0:b1f00c299273 1238 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1239 wait(fast);
pedlerw 0:b1f00c299273 1240 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1241 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1242 wait(fast);
pedlerw 0:b1f00c299273 1243 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1244 wait(fast);
pedlerw 0:b1f00c299273 1245 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1246 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1247 wait(fast);
pedlerw 0:b1f00c299273 1248 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1249 wait(fast);
pedlerw 0:b1f00c299273 1250 rotary4 = !rotary4;
pedlerw 0:b1f00c299273 1251 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1252 wait(fast);
pedlerw 0:b1f00c299273 1253 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1254 wait(fast);
pedlerw 0:b1f00c299273 1255 rotary5 = !rotary5;
pedlerw 0:b1f00c299273 1256 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1257 wait(fast);
pedlerw 0:b1f00c299273 1258 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1259 wait(fast);
pedlerw 0:b1f00c299273 1260 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1261 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1262 wait(fast);
pedlerw 0:b1f00c299273 1263 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1264 wait(fast);
pedlerw 0:b1f00c299273 1265 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1266 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1267 wait(fast);
pedlerw 0:b1f00c299273 1268 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1269 wait(fast);
pedlerw 0:b1f00c299273 1270 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1271 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1272 wait(fast);
pedlerw 0:b1f00c299273 1273 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1274 wait(fast);
pedlerw 0:b1f00c299273 1275 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1276 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1277 wait(fast);
pedlerw 0:b1f00c299273 1278 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1279 wait(fast);
pedlerw 0:b1f00c299273 1280 rotary4 = !rotary4;
pedlerw 0:b1f00c299273 1281 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1282 wait(fast);
pedlerw 0:b1f00c299273 1283 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1284 wait(fast);
pedlerw 0:b1f00c299273 1285 rotary5 = !rotary5;
pedlerw 0:b1f00c299273 1286 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1287 wait(fast);
pedlerw 0:b1f00c299273 1288 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1289 wait(fast);
pedlerw 0:b1f00c299273 1290 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1291 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1292 wait(fast);
pedlerw 0:b1f00c299273 1293 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1294 wait(fast);
pedlerw 0:b1f00c299273 1295 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1296 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1297 wait(fast);
pedlerw 0:b1f00c299273 1298 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1299 wait(fast);
pedlerw 0:b1f00c299273 1300 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1301 move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1302 wait(fast);
pedlerw 0:b1f00c299273 1303 /*move_one_left(abs(length--));
pedlerw 0:b1f00c299273 1304 wait(fast);
pedlerw 0:b1f00c299273 1305 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1306 move_one_left(abs(length--)); // */
pedlerw 0:b1f00c299273 1307 //}//End of for//
pedlerw 0:b1f00c299273 1308 }//End of function//
pedlerw 0:b1f00c299273 1309
pedlerw 0:b1f00c299273 1310 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1311 /////////////////////////rotate_all_cw(int )///////////////////////////////////
pedlerw 0:b1f00c299273 1312 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1313 void rotate_all_cw(int set, float speed)
pedlerw 0:b1f00c299273 1314 {
pedlerw 0:b1f00c299273 1315 rotary0 = 1;
pedlerw 0:b1f00c299273 1316 rotary1 = 1;
pedlerw 0:b1f00c299273 1317 rotary2 = 1;
pedlerw 0:b1f00c299273 1318 rotary3 = 1;
pedlerw 0:b1f00c299273 1319 rotary4 = 1;
pedlerw 0:b1f00c299273 1320 rotary5 = 1;
pedlerw 0:b1f00c299273 1321 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 1322 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1323 wait(speed);
pedlerw 0:b1f00c299273 1324 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1325 wait(speed);
pedlerw 0:b1f00c299273 1326 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1327 wait(speed);
pedlerw 0:b1f00c299273 1328 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1329 wait(speed);
pedlerw 0:b1f00c299273 1330 rotary4 = !rotary4;
pedlerw 0:b1f00c299273 1331 wait(speed);
pedlerw 0:b1f00c299273 1332 rotary5 = !rotary5;
pedlerw 0:b1f00c299273 1333 wait(speed);
pedlerw 0:b1f00c299273 1334 }//End of for
pedlerw 0:b1f00c299273 1335 }//End of function//
pedlerw 0:b1f00c299273 1336
pedlerw 0:b1f00c299273 1337 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1338 ///////////////////////////rotate_all_ccw(int s)///////////////////////////////
pedlerw 0:b1f00c299273 1339 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1340 //Function rotate the forcer
pedlerw 0:b1f00c299273 1341 void rotate_all_ccw(int set, float speed)
pedlerw 0:b1f00c299273 1342 {
pedlerw 0:b1f00c299273 1343 rotary0 = 1;
pedlerw 0:b1f00c299273 1344 rotary1 = 1;
pedlerw 0:b1f00c299273 1345 rotary2 = 1;
pedlerw 0:b1f00c299273 1346 rotary3 = 1;
pedlerw 0:b1f00c299273 1347 rotary4 = 1;
pedlerw 0:b1f00c299273 1348 rotary5 = 1;
pedlerw 0:b1f00c299273 1349 for(int i=0; i<set; i++) {
pedlerw 0:b1f00c299273 1350 rotary5 = !rotary5;
pedlerw 0:b1f00c299273 1351 wait(speed);
pedlerw 0:b1f00c299273 1352 rotary4 = !rotary4;
pedlerw 0:b1f00c299273 1353 wait(speed);
pedlerw 0:b1f00c299273 1354 rotary3 = !rotary3;
pedlerw 0:b1f00c299273 1355 wait(speed);
pedlerw 0:b1f00c299273 1356 rotary2 = !rotary2;
pedlerw 0:b1f00c299273 1357 wait(speed);
pedlerw 0:b1f00c299273 1358 rotary1 = !rotary1;
pedlerw 0:b1f00c299273 1359 wait(speed);
pedlerw 0:b1f00c299273 1360 rotary0 = !rotary0;
pedlerw 0:b1f00c299273 1361 wait(speed);
pedlerw 0:b1f00c299273 1362 }//End of for
pedlerw 0:b1f00c299273 1363 }//End of function//
pedlerw 0:b1f00c299273 1364
pedlerw 0:b1f00c299273 1365 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1366 //////////////////////////////////////rotary_speed();//////////////////////////
pedlerw 0:b1f00c299273 1367 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1368 //rotary_speed() rotates the forcer faster and faster//
pedlerw 0:b1f00c299273 1369 void rotary_speed()
pedlerw 0:b1f00c299273 1370 {
pedlerw 0:b1f00c299273 1371 float time = 0.160;
pedlerw 0:b1f00c299273 1372 float step = 0.010;
pedlerw 0:b1f00c299273 1373 int a = 0;
pedlerw 0:b1f00c299273 1374 int cycle = 16;
pedlerw 0:b1f00c299273 1375 int change = 0;
pedlerw 0:b1f00c299273 1376 int turns = 4;
pedlerw 0:b1f00c299273 1377 int maxint = .08;
pedlerw 0:b1f00c299273 1378
pedlerw 0:b1f00c299273 1379 while(a<2) {
pedlerw 0:b1f00c299273 1380 for(int i=cycle; i>0; i--) {
pedlerw 0:b1f00c299273 1381 if(time == maxint) {
pedlerw 0:b1f00c299273 1382 turns = 80;
pedlerw 0:b1f00c299273 1383 } else if(time != maxint) {
pedlerw 0:b1f00c299273 1384 turns = 4;
pedlerw 0:b1f00c299273 1385 }
pedlerw 0:b1f00c299273 1386 if(change == 0) {
pedlerw 0:b1f00c299273 1387 if(i > cycle/2) {
pedlerw 0:b1f00c299273 1388 rotate_all_cw(turns,time);
pedlerw 0:b1f00c299273 1389 time = time - step;
pedlerw 0:b1f00c299273 1390 }//End of if
pedlerw 0:b1f00c299273 1391 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 0:b1f00c299273 1392 rotate_all_cw(turns,time);
pedlerw 0:b1f00c299273 1393 time = time + step;
pedlerw 0:b1f00c299273 1394 }//End of if
pedlerw 0:b1f00c299273 1395 } else if(change==1) {
pedlerw 0:b1f00c299273 1396 if(i > cycle/2) {
pedlerw 0:b1f00c299273 1397
pedlerw 0:b1f00c299273 1398 rotate_all_ccw(turns,time);
pedlerw 0:b1f00c299273 1399 time = time - step;
pedlerw 0:b1f00c299273 1400 }//End of if
pedlerw 0:b1f00c299273 1401 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 0:b1f00c299273 1402 rotate_all_ccw(turns,time);
pedlerw 0:b1f00c299273 1403 time = time + step;
pedlerw 0:b1f00c299273 1404 }//End of if
pedlerw 0:b1f00c299273 1405 }//End of else if//
pedlerw 0:b1f00c299273 1406 }//End of for//
pedlerw 0:b1f00c299273 1407 change = !change;
pedlerw 0:b1f00c299273 1408 a++;
pedlerw 0:b1f00c299273 1409 }//End of while
pedlerw 0:b1f00c299273 1410 }//End of function
pedlerw 0:b1f00c299273 1411
pedlerw 0:b1f00c299273 1412 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1413 ///////////////////////////fast_rotary_cw()////////////////////////////////////
pedlerw 0:b1f00c299273 1414 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1415 //Special function for little forcer
pedlerw 0:b1f00c299273 1416 void fast_rotary_cw(){
pedlerw 0:b1f00c299273 1417 rotate_all_cw(400, 0.01);
pedlerw 0:b1f00c299273 1418 }//End of Function//
pedlerw 0:b1f00c299273 1419
pedlerw 0:b1f00c299273 1420 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1421 ////////////////////////////////small_rotary_speed();//////////////////////////
pedlerw 0:b1f00c299273 1422 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1423 //small_rotary_speed() rotates the small forcer faster and faster//
pedlerw 0:b1f00c299273 1424 void small_rotary_speed()
pedlerw 0:b1f00c299273 1425 {
pedlerw 0:b1f00c299273 1426 float time = 0.080;
pedlerw 0:b1f00c299273 1427 float step = 0.005;
pedlerw 0:b1f00c299273 1428 int a = 0;
pedlerw 0:b1f00c299273 1429 int cycle = 32;
pedlerw 0:b1f00c299273 1430 int change = 0;
pedlerw 0:b1f00c299273 1431 int turns = 8;
pedlerw 0:b1f00c299273 1432 int maxint = 0.010;
pedlerw 0:b1f00c299273 1433
pedlerw 0:b1f00c299273 1434 while(a<2) {
pedlerw 0:b1f00c299273 1435 for(int i=cycle; i>0; i--) {
pedlerw 0:b1f00c299273 1436 if(time == maxint) {
pedlerw 0:b1f00c299273 1437 turns = 400;
pedlerw 0:b1f00c299273 1438 } else if(time != maxint) {
pedlerw 0:b1f00c299273 1439 turns = 8;
pedlerw 0:b1f00c299273 1440 }
pedlerw 0:b1f00c299273 1441 if(change == 0) {
pedlerw 0:b1f00c299273 1442 if(i > cycle/2) {
pedlerw 0:b1f00c299273 1443 rotate_all_cw(turns,time);
pedlerw 0:b1f00c299273 1444 time = time - step;
pedlerw 0:b1f00c299273 1445 }//End of if
pedlerw 0:b1f00c299273 1446 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 0:b1f00c299273 1447 rotate_all_cw(turns,time);
pedlerw 0:b1f00c299273 1448 time = time + step;
pedlerw 0:b1f00c299273 1449 }//End of if
pedlerw 0:b1f00c299273 1450 } else if(change==1) {
pedlerw 0:b1f00c299273 1451 if(i > cycle/2) {
pedlerw 0:b1f00c299273 1452 rotate_all_ccw(turns,time);
pedlerw 0:b1f00c299273 1453 time = time - step;
pedlerw 0:b1f00c299273 1454 }//End of if
pedlerw 0:b1f00c299273 1455 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 0:b1f00c299273 1456 rotate_all_ccw(turns,time);
pedlerw 0:b1f00c299273 1457 time = time + step;
pedlerw 0:b1f00c299273 1458 }//End of if
pedlerw 0:b1f00c299273 1459 }//End of else if//
pedlerw 0:b1f00c299273 1460 }//End of for//
pedlerw 0:b1f00c299273 1461 change = !change;
pedlerw 0:b1f00c299273 1462 a++;
pedlerw 0:b1f00c299273 1463 }//End of while
pedlerw 0:b1f00c299273 1464 }//End of function
pedlerw 0:b1f00c299273 1465 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1466 /////////////////////////////rot_one_cw()//////////////////////////////////////
pedlerw 0:b1f00c299273 1467 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1468 //Function analyzes current state of digital I/O for rotary and increments cw//
pedlerw 0:b1f00c299273 1469 void rot_one_cw(){
pedlerw 0:b1f00c299273 1470 if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1471 rotary0 = 0;
pedlerw 0:b1f00c299273 1472 rotary1 = 0;
pedlerw 0:b1f00c299273 1473 rotary2 = 0;
pedlerw 0:b1f00c299273 1474 rotary3 = 0;
pedlerw 0:b1f00c299273 1475 rotary4 = 0;
pedlerw 0:b1f00c299273 1476 rotary5 = 1;
pedlerw 0:b1f00c299273 1477 }
pedlerw 0:b1f00c299273 1478 else if(rotary0 == 1 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1479 rotary0 = 1;
pedlerw 0:b1f00c299273 1480 rotary1 = 1;
pedlerw 0:b1f00c299273 1481 rotary2 = 0;
pedlerw 0:b1f00c299273 1482 rotary3 = 0;
pedlerw 0:b1f00c299273 1483 rotary4 = 0;
pedlerw 0:b1f00c299273 1484 rotary5 = 0;
pedlerw 0:b1f00c299273 1485 }
pedlerw 0:b1f00c299273 1486 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1487 rotary0 = 1;
pedlerw 0:b1f00c299273 1488 rotary1 = 1;
pedlerw 0:b1f00c299273 1489 rotary2 = 1;
pedlerw 0:b1f00c299273 1490 rotary3 = 0;
pedlerw 0:b1f00c299273 1491 rotary4 = 0;
pedlerw 0:b1f00c299273 1492 rotary5 = 0;
pedlerw 0:b1f00c299273 1493 }
pedlerw 0:b1f00c299273 1494 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1495 rotary0 = 1;
pedlerw 0:b1f00c299273 1496 rotary1 = 1;
pedlerw 0:b1f00c299273 1497 rotary2 = 1;
pedlerw 0:b1f00c299273 1498 rotary3 = 1;
pedlerw 0:b1f00c299273 1499 rotary4 = 0;
pedlerw 0:b1f00c299273 1500 rotary5 = 0;
pedlerw 0:b1f00c299273 1501 }
pedlerw 0:b1f00c299273 1502 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1503 rotary0 = 1;
pedlerw 0:b1f00c299273 1504 rotary1 = 1;
pedlerw 0:b1f00c299273 1505 rotary2 = 1;
pedlerw 0:b1f00c299273 1506 rotary3 = 1;
pedlerw 0:b1f00c299273 1507 rotary4 = 1;
pedlerw 0:b1f00c299273 1508 rotary5 = 0;
pedlerw 0:b1f00c299273 1509 }
pedlerw 0:b1f00c299273 1510 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 0){
pedlerw 0:b1f00c299273 1511 rotary0 = 1;
pedlerw 0:b1f00c299273 1512 rotary1 = 1;
pedlerw 0:b1f00c299273 1513 rotary2 = 1;
pedlerw 0:b1f00c299273 1514 rotary3 = 1;
pedlerw 0:b1f00c299273 1515 rotary4 = 1;
pedlerw 0:b1f00c299273 1516 rotary5 = 1;
pedlerw 0:b1f00c299273 1517 }
pedlerw 0:b1f00c299273 1518 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1519 rotary0 = 0;
pedlerw 0:b1f00c299273 1520 rotary1 = 1;
pedlerw 0:b1f00c299273 1521 rotary2 = 1;
pedlerw 0:b1f00c299273 1522 rotary3 = 1;
pedlerw 0:b1f00c299273 1523 rotary4 = 1;
pedlerw 0:b1f00c299273 1524 rotary5 = 1;
pedlerw 0:b1f00c299273 1525 }
pedlerw 0:b1f00c299273 1526 else if(rotary0 == 0 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1527 rotary0 = 0;
pedlerw 0:b1f00c299273 1528 rotary1 = 0;
pedlerw 0:b1f00c299273 1529 rotary2 = 1;
pedlerw 0:b1f00c299273 1530 rotary3 = 1;
pedlerw 0:b1f00c299273 1531 rotary4 = 1;
pedlerw 0:b1f00c299273 1532 rotary5 = 1;
pedlerw 0:b1f00c299273 1533 }
pedlerw 0:b1f00c299273 1534 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1535 rotary0 = 0;
pedlerw 0:b1f00c299273 1536 rotary1 = 0;
pedlerw 0:b1f00c299273 1537 rotary2 = 0;
pedlerw 0:b1f00c299273 1538 rotary3 = 1;
pedlerw 0:b1f00c299273 1539 rotary4 = 1;
pedlerw 0:b1f00c299273 1540 rotary5 = 1;
pedlerw 0:b1f00c299273 1541 }
pedlerw 0:b1f00c299273 1542 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1543 rotary0 = 0;
pedlerw 0:b1f00c299273 1544 rotary1 = 0;
pedlerw 0:b1f00c299273 1545 rotary2 = 0;
pedlerw 0:b1f00c299273 1546 rotary3 = 0;
pedlerw 0:b1f00c299273 1547 rotary4 = 1;
pedlerw 0:b1f00c299273 1548 rotary5 = 1;
pedlerw 0:b1f00c299273 1549 }
pedlerw 0:b1f00c299273 1550 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1551 rotary0 = 0;
pedlerw 0:b1f00c299273 1552 rotary1 = 0;
pedlerw 0:b1f00c299273 1553 rotary2 = 0;
pedlerw 0:b1f00c299273 1554 rotary3 = 0;
pedlerw 0:b1f00c299273 1555 rotary4 = 0;
pedlerw 0:b1f00c299273 1556 rotary5 = 1;
pedlerw 0:b1f00c299273 1557 }
pedlerw 0:b1f00c299273 1558 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 1){
pedlerw 0:b1f00c299273 1559 rotary0 = 0;
pedlerw 0:b1f00c299273 1560 rotary1 = 0;
pedlerw 0:b1f00c299273 1561 rotary2 = 0;
pedlerw 0:b1f00c299273 1562 rotary3 = 0;
pedlerw 0:b1f00c299273 1563 rotary4 = 0;
pedlerw 0:b1f00c299273 1564 rotary5 = 0;
pedlerw 0:b1f00c299273 1565 }
pedlerw 0:b1f00c299273 1566 rot_pos_dec();
pedlerw 0:b1f00c299273 1567 }//End of function
pedlerw 0:b1f00c299273 1568
pedlerw 0:b1f00c299273 1569 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1570 ////////////////////////////rot_one_ccw()//////////////////////////////////////
pedlerw 0:b1f00c299273 1571 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1572 //Function analyzes current state of rotary digital I/O and increments ccw//
pedlerw 0:b1f00c299273 1573 void rot_one_ccw(){
pedlerw 0:b1f00c299273 1574 if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1575 rotary0 = 0;
pedlerw 0:b1f00c299273 1576 rotary1 = 0;
pedlerw 0:b1f00c299273 1577 rotary2 = 0;
pedlerw 0:b1f00c299273 1578 rotary3 = 0;
pedlerw 0:b1f00c299273 1579 rotary4 = 0;
pedlerw 0:b1f00c299273 1580 rotary5 = 1;
pedlerw 0:b1f00c299273 1581 }
pedlerw 0:b1f00c299273 1582 else if(rotary0 == 1 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1583 rotary0 = 0;
pedlerw 0:b1f00c299273 1584 rotary1 = 0;
pedlerw 0:b1f00c299273 1585 rotary2 = 0;
pedlerw 0:b1f00c299273 1586 rotary3 = 0;
pedlerw 0:b1f00c299273 1587 rotary4 = 0;
pedlerw 0:b1f00c299273 1588 rotary5 = 0;
pedlerw 0:b1f00c299273 1589 }
pedlerw 0:b1f00c299273 1590 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1591 rotary0 = 1;
pedlerw 0:b1f00c299273 1592 rotary1 = 0;
pedlerw 0:b1f00c299273 1593 rotary2 = 0;
pedlerw 0:b1f00c299273 1594 rotary3 = 0;
pedlerw 0:b1f00c299273 1595 rotary4 = 0;
pedlerw 0:b1f00c299273 1596 rotary5 = 0;
pedlerw 0:b1f00c299273 1597 }
pedlerw 0:b1f00c299273 1598 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1599 rotary0 = 1;
pedlerw 0:b1f00c299273 1600 rotary1 = 1;
pedlerw 0:b1f00c299273 1601 rotary2 = 0;
pedlerw 0:b1f00c299273 1602 rotary3 = 0;
pedlerw 0:b1f00c299273 1603 rotary4 = 0;
pedlerw 0:b1f00c299273 1604 rotary5 = 0;
pedlerw 0:b1f00c299273 1605 }
pedlerw 0:b1f00c299273 1606 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 0 && rotary5 == 0){
pedlerw 0:b1f00c299273 1607 rotary0 = 1;
pedlerw 0:b1f00c299273 1608 rotary1 = 1;
pedlerw 0:b1f00c299273 1609 rotary2 = 1;
pedlerw 0:b1f00c299273 1610 rotary3 = 0;
pedlerw 0:b1f00c299273 1611 rotary4 = 0;
pedlerw 0:b1f00c299273 1612 rotary5 = 0;
pedlerw 0:b1f00c299273 1613 }
pedlerw 0:b1f00c299273 1614 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 0){
pedlerw 0:b1f00c299273 1615 rotary0 = 1;
pedlerw 0:b1f00c299273 1616 rotary1 = 1;
pedlerw 0:b1f00c299273 1617 rotary2 = 1;
pedlerw 0:b1f00c299273 1618 rotary3 = 1;
pedlerw 0:b1f00c299273 1619 rotary4 = 0;
pedlerw 0:b1f00c299273 1620 rotary5 = 0;
pedlerw 0:b1f00c299273 1621 }
pedlerw 0:b1f00c299273 1622 else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1623 rotary0 = 1;
pedlerw 0:b1f00c299273 1624 rotary1 = 1;
pedlerw 0:b1f00c299273 1625 rotary2 = 1;
pedlerw 0:b1f00c299273 1626 rotary3 = 1;
pedlerw 0:b1f00c299273 1627 rotary4 = 1;
pedlerw 0:b1f00c299273 1628 rotary5 = 0;
pedlerw 0:b1f00c299273 1629 }
pedlerw 0:b1f00c299273 1630 else if(rotary0 == 0 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1631 rotary0 = 1;
pedlerw 0:b1f00c299273 1632 rotary1 = 1;
pedlerw 0:b1f00c299273 1633 rotary2 = 1;
pedlerw 0:b1f00c299273 1634 rotary3 = 1;
pedlerw 0:b1f00c299273 1635 rotary4 = 1;
pedlerw 0:b1f00c299273 1636 rotary5 = 1;
pedlerw 0:b1f00c299273 1637 }
pedlerw 0:b1f00c299273 1638 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1639 rotary0 = 0;
pedlerw 0:b1f00c299273 1640 rotary1 = 1;
pedlerw 0:b1f00c299273 1641 rotary2 = 1;
pedlerw 0:b1f00c299273 1642 rotary3 = 1;
pedlerw 0:b1f00c299273 1643 rotary4 = 1;
pedlerw 0:b1f00c299273 1644 rotary5 = 1;
pedlerw 0:b1f00c299273 1645 }
pedlerw 0:b1f00c299273 1646 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1647 rotary0 = 0;
pedlerw 0:b1f00c299273 1648 rotary1 = 0;
pedlerw 0:b1f00c299273 1649 rotary2 = 1;
pedlerw 0:b1f00c299273 1650 rotary3 = 1;
pedlerw 0:b1f00c299273 1651 rotary4 = 1;
pedlerw 0:b1f00c299273 1652 rotary5 = 1;
pedlerw 0:b1f00c299273 1653 }
pedlerw 0:b1f00c299273 1654 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 1 && rotary5 == 1){
pedlerw 0:b1f00c299273 1655 rotary0 = 0;
pedlerw 0:b1f00c299273 1656 rotary1 = 0;
pedlerw 0:b1f00c299273 1657 rotary2 = 0;
pedlerw 0:b1f00c299273 1658 rotary3 = 1;
pedlerw 0:b1f00c299273 1659 rotary4 = 1;
pedlerw 0:b1f00c299273 1660 rotary5 = 1;
pedlerw 0:b1f00c299273 1661 }
pedlerw 0:b1f00c299273 1662 else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 1){
pedlerw 0:b1f00c299273 1663 rotary0 = 0;
pedlerw 0:b1f00c299273 1664 rotary1 = 0;
pedlerw 0:b1f00c299273 1665 rotary2 = 0;
pedlerw 0:b1f00c299273 1666 rotary3 = 0;
pedlerw 0:b1f00c299273 1667 rotary4 = 1;
pedlerw 0:b1f00c299273 1668 rotary5 = 1;
pedlerw 0:b1f00c299273 1669 }
pedlerw 0:b1f00c299273 1670 rot_pos_inc();
pedlerw 0:b1f00c299273 1671 }//End of function
pedlerw 0:b1f00c299273 1672
pedlerw 0:b1f00c299273 1673 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1674 ///////////////////////////calibrate_rot()/////////////////////////////////////
pedlerw 0:b1f00c299273 1675 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1676 //Function rotates cw 24 times gathering return data from photo resistors//
pedlerw 0:b1f00c299273 1677 //Then it sets the threashold (global variable)//
pedlerw 0:b1f00c299273 1678 void calibrate_rot(){
pedlerw 0:b1f00c299273 1679 float sum = 0;
pedlerw 0:b1f00c299273 1680 rotary0 = 1;
pedlerw 0:b1f00c299273 1681 rotary1 = 1;
pedlerw 0:b1f00c299273 1682 rotary2 = 1;
pedlerw 0:b1f00c299273 1683 rotary3 = 1;
pedlerw 0:b1f00c299273 1684 rotary4 = 1;
pedlerw 0:b1f00c299273 1685 rotary5 = 1;
pedlerw 0:b1f00c299273 1686 for(int i=0;i<rot_pos;i++){
pedlerw 0:b1f00c299273 1687 rotary_threshold[i] = rot_indication.read();
pedlerw 1:27dcf7ff5b96 1688 rot_one_cw();
pedlerw 1:27dcf7ff5b96 1689 wait(rot_slow);
pedlerw 0:b1f00c299273 1690 }//End of for
pedlerw 0:b1f00c299273 1691 //Print rotary_threshold[i] to file//
pedlerw 0:b1f00c299273 1692 FILE *fp5= fopen("/local/rotary.txt", "w");
pedlerw 0:b1f00c299273 1693 for(int i=0;i<rot_pos;i++) {
pedlerw 0:b1f00c299273 1694 fprintf(fp5,"%f\r\n",rotary_threshold[i]);
pedlerw 0:b1f00c299273 1695 }//End of for//
pedlerw 0:b1f00c299273 1696 fclose(fp5);
pedlerw 0:b1f00c299273 1697 //Calculate Threash//
pedlerw 0:b1f00c299273 1698 for(int i=0;i<rot_pos;i++){
pedlerw 0:b1f00c299273 1699 sum = sum + rotary_threshold[i];
pedlerw 0:b1f00c299273 1700 }//End of for
pedlerw 0:b1f00c299273 1701 //Print Threash to file//
pedlerw 0:b1f00c299273 1702 threash = sum/rot_pos;
pedlerw 0:b1f00c299273 1703 FILE *fp6= fopen("/local/threash.txt", "w");
pedlerw 0:b1f00c299273 1704 fprintf(fp6,"%f\r\n",threash);
pedlerw 0:b1f00c299273 1705 fclose(fp6);
pedlerw 0:b1f00c299273 1706 }//End of function
pedlerw 0:b1f00c299273 1707
pedlerw 0:b1f00c299273 1708 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1709 /////////////////////////get_user_angle()//////////////////////////////////////
pedlerw 0:b1f00c299273 1710 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1711 //This function accepts an int and returns an int... the int value represents//
pedlerw 0:b1f00c299273 1712 //a desired angle...this returns the nearest angle within 15 degrees//
pedlerw 0:b1f00c299273 1713 int get_user_angle(int desired_angle){
pedlerw 0:b1f00c299273 1714 int angle = -1;
pedlerw 0:b1f00c299273 1715 if(desired_angle <= 0){
pedlerw 0:b1f00c299273 1716 angle = 0;
pedlerw 0:b1f00c299273 1717 }
pedlerw 0:b1f00c299273 1718 else if(desired_angle > 0 && desired_angle < (0+7.5)){
pedlerw 0:b1f00c299273 1719 angle = 0;
pedlerw 0:b1f00c299273 1720 }
pedlerw 0:b1f00c299273 1721 else if(desired_angle >= (0+7.5) && desired_angle <= 15){
pedlerw 0:b1f00c299273 1722 angle = 15;
pedlerw 0:b1f00c299273 1723 }
pedlerw 0:b1f00c299273 1724 else if(desired_angle > 15 && desired_angle < (15+7.5)){
pedlerw 0:b1f00c299273 1725 angle = 15;
pedlerw 0:b1f00c299273 1726 }
pedlerw 0:b1f00c299273 1727 else if(desired_angle >= (15+7.5) && desired_angle <= 30){
pedlerw 0:b1f00c299273 1728 angle = 30;
pedlerw 0:b1f00c299273 1729 }
pedlerw 0:b1f00c299273 1730 else if(desired_angle > 30 && desired_angle < (30+7.5)){
pedlerw 0:b1f00c299273 1731 angle = 30;
pedlerw 0:b1f00c299273 1732 }
pedlerw 0:b1f00c299273 1733 else if(desired_angle >= (30+7.5) && desired_angle <= 45){
pedlerw 0:b1f00c299273 1734 angle = 45;
pedlerw 0:b1f00c299273 1735 }
pedlerw 0:b1f00c299273 1736 else if(desired_angle > 45 && desired_angle < (45+7.5)){
pedlerw 0:b1f00c299273 1737 angle = 45;
pedlerw 0:b1f00c299273 1738 }
pedlerw 0:b1f00c299273 1739 else if(desired_angle >= (45+7.5) && desired_angle <= 60){
pedlerw 0:b1f00c299273 1740 angle = 60;
pedlerw 0:b1f00c299273 1741 }
pedlerw 0:b1f00c299273 1742 else if(desired_angle > 60 && desired_angle < (60+7.5)){
pedlerw 0:b1f00c299273 1743 angle = 60;
pedlerw 0:b1f00c299273 1744 }
pedlerw 0:b1f00c299273 1745 else if(desired_angle >= (60+7.5) && desired_angle <= 75){
pedlerw 0:b1f00c299273 1746 angle = 75;
pedlerw 0:b1f00c299273 1747 }
pedlerw 0:b1f00c299273 1748 else if(desired_angle > 75 && desired_angle < (75+7.5)){
pedlerw 0:b1f00c299273 1749 angle = 75;
pedlerw 0:b1f00c299273 1750 }
pedlerw 0:b1f00c299273 1751 else if(desired_angle >= (75+7.5) && desired_angle <= 90){
pedlerw 0:b1f00c299273 1752 angle = 90;
pedlerw 0:b1f00c299273 1753 }
pedlerw 0:b1f00c299273 1754 else if(desired_angle > 90 && desired_angle < (90+7.5)){
pedlerw 0:b1f00c299273 1755 angle = 90;
pedlerw 0:b1f00c299273 1756 }
pedlerw 0:b1f00c299273 1757 else if(desired_angle >= (90+7.5) && desired_angle <= 105){
pedlerw 0:b1f00c299273 1758 angle = 105;
pedlerw 0:b1f00c299273 1759 }
pedlerw 0:b1f00c299273 1760 else if(desired_angle > 105 && desired_angle < (105+7.5)){
pedlerw 0:b1f00c299273 1761 angle = 105;
pedlerw 0:b1f00c299273 1762 }
pedlerw 0:b1f00c299273 1763 else if(desired_angle >= (105+7.5) && desired_angle <= 120){
pedlerw 0:b1f00c299273 1764 angle = 120;
pedlerw 0:b1f00c299273 1765 }
pedlerw 0:b1f00c299273 1766 else if(desired_angle > 120 && desired_angle < (120+7.5)){
pedlerw 0:b1f00c299273 1767 angle = 120;
pedlerw 0:b1f00c299273 1768 }
pedlerw 0:b1f00c299273 1769 else if(desired_angle >= (120+7.5) && desired_angle <= 135){
pedlerw 0:b1f00c299273 1770 angle = 135;
pedlerw 0:b1f00c299273 1771 }
pedlerw 0:b1f00c299273 1772 else if(desired_angle > 135 && desired_angle < (135+7.5)){
pedlerw 0:b1f00c299273 1773 angle = 135;
pedlerw 0:b1f00c299273 1774 }
pedlerw 0:b1f00c299273 1775 else if(desired_angle >= (135+7.5) && desired_angle <= 150){
pedlerw 0:b1f00c299273 1776 angle = 150;
pedlerw 0:b1f00c299273 1777 }
pedlerw 0:b1f00c299273 1778 else if(desired_angle > 150 && desired_angle < (150+7.5)){
pedlerw 0:b1f00c299273 1779 angle = 150;
pedlerw 0:b1f00c299273 1780 }
pedlerw 0:b1f00c299273 1781 else if(desired_angle >= (150+7.5) && desired_angle <= 165){
pedlerw 0:b1f00c299273 1782 angle = 165;
pedlerw 0:b1f00c299273 1783 }
pedlerw 0:b1f00c299273 1784 else if(desired_angle > 165 && desired_angle < (165+7.5)){
pedlerw 0:b1f00c299273 1785 angle = 165;
pedlerw 0:b1f00c299273 1786 }
pedlerw 0:b1f00c299273 1787 else if(desired_angle >= (165+7.5) && desired_angle <= 180){
pedlerw 0:b1f00c299273 1788 angle = 180;
pedlerw 0:b1f00c299273 1789 }
pedlerw 0:b1f00c299273 1790 else if(desired_angle > 180 && desired_angle < (180+7.5)){
pedlerw 0:b1f00c299273 1791 angle = 180;
pedlerw 0:b1f00c299273 1792 }
pedlerw 0:b1f00c299273 1793 else if(desired_angle >= (180+7.5) && desired_angle <= 195){
pedlerw 0:b1f00c299273 1794 angle = 195;
pedlerw 0:b1f00c299273 1795 }
pedlerw 0:b1f00c299273 1796 else if(desired_angle > 195 && desired_angle < (195+7.5)){
pedlerw 0:b1f00c299273 1797 angle = 195;
pedlerw 0:b1f00c299273 1798 }
pedlerw 0:b1f00c299273 1799 else if(desired_angle >= (195+7.5) && desired_angle <= 210){
pedlerw 0:b1f00c299273 1800 angle = 210;
pedlerw 0:b1f00c299273 1801 }
pedlerw 0:b1f00c299273 1802 else if(desired_angle > 210 && desired_angle < (210+7.5)){
pedlerw 0:b1f00c299273 1803 angle = 210;
pedlerw 0:b1f00c299273 1804 }
pedlerw 0:b1f00c299273 1805 else if(desired_angle >= (210+7.5) && desired_angle <= 225){
pedlerw 0:b1f00c299273 1806 angle = 225;
pedlerw 0:b1f00c299273 1807 }
pedlerw 0:b1f00c299273 1808 else if(desired_angle > 225 && desired_angle < (225+7.5)){
pedlerw 0:b1f00c299273 1809 angle = 225;
pedlerw 0:b1f00c299273 1810 }
pedlerw 0:b1f00c299273 1811 else if(desired_angle >= (225+7.5) && desired_angle <= 240){
pedlerw 0:b1f00c299273 1812 angle = 240;
pedlerw 0:b1f00c299273 1813 }
pedlerw 0:b1f00c299273 1814 else if(desired_angle > 240 && desired_angle < (240+7.5)){
pedlerw 0:b1f00c299273 1815 angle = 240;
pedlerw 0:b1f00c299273 1816 }
pedlerw 0:b1f00c299273 1817 else if(desired_angle >= (240+7.5) && desired_angle <= 255){
pedlerw 0:b1f00c299273 1818 angle = 255;
pedlerw 0:b1f00c299273 1819 }
pedlerw 0:b1f00c299273 1820 else if(desired_angle > 255 && desired_angle < (255+7.5)){
pedlerw 0:b1f00c299273 1821 angle = 255;
pedlerw 0:b1f00c299273 1822 }
pedlerw 0:b1f00c299273 1823 else if(desired_angle >= (255+7.5) && desired_angle <= 270){
pedlerw 0:b1f00c299273 1824 angle = 270;
pedlerw 0:b1f00c299273 1825 }
pedlerw 0:b1f00c299273 1826 else if(desired_angle > 270 && desired_angle < (270+7.5)){
pedlerw 0:b1f00c299273 1827 angle = 270;
pedlerw 0:b1f00c299273 1828 }
pedlerw 0:b1f00c299273 1829 else if(desired_angle >= (270+7.5) && desired_angle <= 285){
pedlerw 0:b1f00c299273 1830 angle = 285;
pedlerw 0:b1f00c299273 1831 }
pedlerw 0:b1f00c299273 1832 else if(desired_angle > 285 && desired_angle < (285+7.5)){
pedlerw 0:b1f00c299273 1833 angle = 285;
pedlerw 0:b1f00c299273 1834 }
pedlerw 0:b1f00c299273 1835 else if(desired_angle >= (285+7.5) && desired_angle <= 300){
pedlerw 0:b1f00c299273 1836 angle = 300;
pedlerw 0:b1f00c299273 1837 }
pedlerw 0:b1f00c299273 1838 else if(desired_angle > 300 && desired_angle < (300+7.5)){
pedlerw 0:b1f00c299273 1839 angle = 300;
pedlerw 0:b1f00c299273 1840 }
pedlerw 0:b1f00c299273 1841 else if(desired_angle >= (300+7.5) && desired_angle <= 315){
pedlerw 0:b1f00c299273 1842 angle = 315;
pedlerw 0:b1f00c299273 1843 }
pedlerw 0:b1f00c299273 1844 else if(desired_angle > 315 && desired_angle < (315+7.5)){
pedlerw 0:b1f00c299273 1845 angle = 315;
pedlerw 0:b1f00c299273 1846 }
pedlerw 0:b1f00c299273 1847 else if(desired_angle >= (315+7.5) && desired_angle <= 330){
pedlerw 0:b1f00c299273 1848 angle = 330;
pedlerw 0:b1f00c299273 1849 }
pedlerw 0:b1f00c299273 1850 else if(desired_angle > 330 && desired_angle < (330+7.5)){
pedlerw 0:b1f00c299273 1851 angle = 330;
pedlerw 0:b1f00c299273 1852 }
pedlerw 0:b1f00c299273 1853 else if(desired_angle >= (330+7.5) && desired_angle <= 345){
pedlerw 0:b1f00c299273 1854 angle = 345;
pedlerw 0:b1f00c299273 1855 }
pedlerw 0:b1f00c299273 1856 else if(desired_angle > 345 && desired_angle < (345+7.5)){
pedlerw 0:b1f00c299273 1857 angle = 345;
pedlerw 0:b1f00c299273 1858 }
pedlerw 0:b1f00c299273 1859 else if(desired_angle >= (345+7.5) && desired_angle <= 360){
pedlerw 0:b1f00c299273 1860 angle = 0;
pedlerw 0:b1f00c299273 1861 }
pedlerw 0:b1f00c299273 1862 else if(desired_angle > 360){
pedlerw 0:b1f00c299273 1863 angle = 0;
pedlerw 0:b1f00c299273 1864 }
pedlerw 0:b1f00c299273 1865 else{
pedlerw 0:b1f00c299273 1866 xbee.printf("\r\nAngle input was not understandable");
pedlerw 0:b1f00c299273 1867 }
pedlerw 0:b1f00c299273 1868 return(angle);
pedlerw 0:b1f00c299273 1869 }//End of function
pedlerw 0:b1f00c299273 1870
pedlerw 0:b1f00c299273 1871 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1872 ///////////////////////////go_to_angle()///////////////////////////////////////
pedlerw 0:b1f00c299273 1873 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1874 //Function accepts an angle then rotates the forcer to said angle//
pedlerw 0:b1f00c299273 1875 void go_to_angle(int angle){
pedlerw 0:b1f00c299273 1876 while(rot_position != angle){
pedlerw 0:b1f00c299273 1877 if(rot_position - angle == 0){
pedlerw 0:b1f00c299273 1878 return;
pedlerw 0:b1f00c299273 1879 }//End of if
pedlerw 0:b1f00c299273 1880 else if(rot_position - angle > 0){
pedlerw 1:27dcf7ff5b96 1881 rot_one_cw();
pedlerw 1:27dcf7ff5b96 1882 wait(rot_slow);
pedlerw 0:b1f00c299273 1883 }//End of else if
pedlerw 0:b1f00c299273 1884 else if(rot_position - angle < 0){
pedlerw 0:b1f00c299273 1885 rot_one_ccw();
pedlerw 1:27dcf7ff5b96 1886 wait(rot_slow);
pedlerw 0:b1f00c299273 1887 }//End of else if
pedlerw 0:b1f00c299273 1888 }
pedlerw 0:b1f00c299273 1889 return;
pedlerw 0:b1f00c299273 1890 }//End of function
pedlerw 0:b1f00c299273 1891
pedlerw 0:b1f00c299273 1892 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1893 ///////////////////////////rot_pos_inc()///////////////////////////////////////
pedlerw 0:b1f00c299273 1894 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1895 void rot_pos_inc(){
pedlerw 0:b1f00c299273 1896 if(zero.read() > threash){
pedlerw 0:b1f00c299273 1897 rot_position = 0;
pedlerw 0:b1f00c299273 1898 return;
pedlerw 0:b1f00c299273 1899 }//End of if//
pedlerw 2:675f8691f469 1900 else if(state != check_state(state)){
pedlerw 0:b1f00c299273 1901 rot_position = rot_position + rot_step;
pedlerw 3:0a1b8c86a31e 1902 }//End of else if//
pedlerw 3:0a1b8c86a31e 1903 else{
pedlerw 3:0a1b8c86a31e 1904 xbee.printf("\r\nIncrement Error");
pedlerw 3:0a1b8c86a31e 1905 }
pedlerw 0:b1f00c299273 1906 }//End of fucntion
pedlerw 0:b1f00c299273 1907
pedlerw 0:b1f00c299273 1908 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1909 ////////////////////////////rot_pos_dec()//////////////////////////////////////
pedlerw 0:b1f00c299273 1910 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:b1f00c299273 1911 void rot_pos_dec(){
pedlerw 0:b1f00c299273 1912 if(zero.read() > threash){
pedlerw 0:b1f00c299273 1913 rot_position = 0;
pedlerw 0:b1f00c299273 1914 return;
pedlerw 0:b1f00c299273 1915 }//End of if//
pedlerw 2:675f8691f469 1916 else if(state != check_state(state)){
pedlerw 0:b1f00c299273 1917 rot_position = rot_position - rot_step;
pedlerw 0:b1f00c299273 1918 }//End of else if//
pedlerw 3:0a1b8c86a31e 1919 else{
pedlerw 3:0a1b8c86a31e 1920 xbee.printf("\r\nDecrement Error");
pedlerw 3:0a1b8c86a31e 1921 }
pedlerw 2:675f8691f469 1922 }//End of function
pedlerw 2:675f8691f469 1923
pedlerw 2:675f8691f469 1924 ///////////////////////////////////////////////////////////////////////////////
pedlerw 2:675f8691f469 1925 ///////////////////////////check_state()///////////////////////////////////////
pedlerw 2:675f8691f469 1926 ///////////////////////////////////////////////////////////////////////////////
pedlerw 2:675f8691f469 1927 bool check_state(bool current_state){
pedlerw 2:675f8691f469 1928 if(current_state == 0){
pedlerw 2:675f8691f469 1929 //Light was blocked//
pedlerw 2:675f8691f469 1930 if(rot_indication.read() > threash){
pedlerw 2:675f8691f469 1931 //in this case light is applied//
pedlerw 2:675f8691f469 1932 state = 1;
pedlerw 2:675f8691f469 1933 }
pedlerw 2:675f8691f469 1934 else if(rot_indication.read() < threash){
pedlerw 2:675f8691f469 1935 //in this case light has been blocked//
pedlerw 2:675f8691f469 1936 state = 0;
pedlerw 3:0a1b8c86a31e 1937 xbee.printf("\r\nState Change Error");
pedlerw 2:675f8691f469 1938 }
pedlerw 2:675f8691f469 1939 }//End of if
pedlerw 2:675f8691f469 1940 else if(current_state == 1){
pedlerw 2:675f8691f469 1941 //Light was applied//
pedlerw 2:675f8691f469 1942 if(rot_indication.read() > threash){
pedlerw 2:675f8691f469 1943 //in this case light is applied//
pedlerw 2:675f8691f469 1944 state = 1;
pedlerw 3:0a1b8c86a31e 1945 xbee.printf("\r\nState Change Error");
pedlerw 2:675f8691f469 1946 }
pedlerw 2:675f8691f469 1947 else if(rot_indication.read() < threash){
pedlerw 2:675f8691f469 1948 //in this case light has been blocked//
pedlerw 2:675f8691f469 1949 state = 0;
pedlerw 2:675f8691f469 1950 }
pedlerw 2:675f8691f469 1951 }//End of else if
pedlerw 2:675f8691f469 1952 return(state);
pedlerw 2:675f8691f469 1953 }//End of function//