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@1:27dcf7ff5b96, 2013-04-24 (annotated)

Committer:
pedlerw
Date:
Wed Apr 24 00:46:06 2013 +0000
Revision:
1:27dcf7ff5b96
Parent:
0:b1f00c299273
Child:
2:675f8691f469
04/23/2013; William Pedler; Latest update

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