William Pedler / Mbed 2 deprecated SeniorDesign_ExpoPro

2013 VCU Senior Design Expo Linear Rotary Motor Control Program. This is the actual program used by the team at the Expo.

Dependencies:   mbed

main.cpp@4:29aafde2dcbc, 2013-04-25 (annotated)

Committer:
pedlerw
Date:
Thu Apr 25 13:13:54 2013 +0000
Revision:
4:29aafde2dcbc
Parent:
3:1d681d86f1dd 
This is the program used by the LRM Team at the 2013 Senior Design Expo.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pedlerw 0:2d8d828ff276 1 //SeniorDesign_ExpoPro//
pedlerw 0:2d8d828ff276 2 //Written by William Pedler 4/15/2013//
pedlerw 0:2d8d828ff276 3 //This is the program the 2013 LRM team is using at the Senior Design Show Case//
pedlerw 0:2d8d828ff276 4 //This program uses two xbee s1 for serial interface//
pedlerw 0:2d8d828ff276 5
pedlerw 0:2d8d828ff276 6 //Includes//
pedlerw 0:2d8d828ff276 7 #include "mbed.h"
pedlerw 0:2d8d828ff276 8 using namespace std;
pedlerw 0:2d8d828ff276 9
pedlerw 0:2d8d828ff276 10 //Define//
pedlerw 0:2d8d828ff276 11 #define DEBUG
pedlerw 0:2d8d828ff276 12 #define MAX 1.0
pedlerw 0:2d8d828ff276 13 #define MIN 0.0
pedlerw 0:2d8d828ff276 14 #define large_num 500 //This times 4 controls how long the sensors read for each position//
pedlerw 0:2d8d828ff276 15 #define one 1
pedlerw 0:2d8d828ff276 16 #define two 2
pedlerw 0:2d8d828ff276 17 #define three 3.0
pedlerw 0:2d8d828ff276 18 #define four 4
pedlerw 0:2d8d828ff276 19 #define five 5
pedlerw 0:2d8d828ff276 20 #define six 6
pedlerw 0:2d8d828ff276 21 #define seven 7
pedlerw 0:2d8d828ff276 22 #define eight 8
pedlerw 0:2d8d828ff276 23 #define nine 9
pedlerw 0:2d8d828ff276 24 #define track_length 42
pedlerw 0:2d8d828ff276 25 #define fast 0.125
pedlerw 0:2d8d828ff276 26 #define medium 1.0
pedlerw 0:2d8d828ff276 27 #define slow 2.0
pedlerw 0:2d8d828ff276 28 #define sensor_set 0.005
pedlerw 3:1d681d86f1dd 29 #define rot_fast 0.080
pedlerw 3:1d681d86f1dd 30 #define rot_med 0.100
pedlerw 3:1d681d86f1dd 31 #define rot_slow 0.120
pedlerw 0:2d8d828ff276 32
pedlerw 0:2d8d828ff276 33 //Declar global variables//
pedlerw 0:2d8d828ff276 34 float left_min[track_length] = {0};
pedlerw 0:2d8d828ff276 35 float left_max[track_length] = {0};
pedlerw 0:2d8d828ff276 36 float right_min[track_length] = {0};
pedlerw 0:2d8d828ff276 37 float right_max[track_length] = {0};
pedlerw 0:2d8d828ff276 38
pedlerw 0:2d8d828ff276 39 //Labeling the Local file system//
pedlerw 0:2d8d828ff276 40 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
pedlerw 0:2d8d828ff276 41 //Labeling onboard LED's//
pedlerw 0:2d8d828ff276 42 DigitalOut led1(LED1);
pedlerw 0:2d8d828ff276 43 DigitalOut led2(LED2);
pedlerw 0:2d8d828ff276 44 DigitalOut led3(LED3);
pedlerw 0:2d8d828ff276 45 DigitalOut led4(LED4);
pedlerw 0:2d8d828ff276 46 //Label linear coils//
pedlerw 0:2d8d828ff276 47 DigitalOut linear0(p5); //coil 1
pedlerw 0:2d8d828ff276 48 DigitalOut linear1(p6); //coil 2
pedlerw 0:2d8d828ff276 49 DigitalOut linear2(p7); //coil 3
pedlerw 0:2d8d828ff276 50 DigitalOut linear3(p8); //coil 4
pedlerw 0:2d8d828ff276 51 DigitalOut linear4(p9); //coil 5
pedlerw 0:2d8d828ff276 52 DigitalOut linear5(p10); //coil 6
pedlerw 0:2d8d828ff276 53 DigitalOut linear6(p11); //coil 7
pedlerw 0:2d8d828ff276 54 //Reduce noise by declaring all unused Analog pins as DigitalOut//
pedlerw 3:1d681d86f1dd 55 DigitalOut left15(p15);
pedlerw 3:1d681d86f1dd 56 DigitalOut left16(p16);
pedlerw 0:2d8d828ff276 57 DigitalOut left17(p17);
pedlerw 0:2d8d828ff276 58 DigitalOut left18(p18);
pedlerw 0:2d8d828ff276 59 AnalogIn distanceR(p19); //Vout yellow GND black Vss red
pedlerw 0:2d8d828ff276 60 AnalogIn distanceL(p20); //Vout yellow GND black Vss red
pedlerw 3:1d681d86f1dd 61 //Declar rotary coils Digital I/O//
pedlerw 3:1d681d86f1dd 62 DigitalOut rotary0(p21);
pedlerw 3:1d681d86f1dd 63 DigitalOut rotary1(p22);
pedlerw 3:1d681d86f1dd 64 DigitalOut rotary2(p23);
pedlerw 3:1d681d86f1dd 65 DigitalOut rotary3(p24);
pedlerw 3:1d681d86f1dd 66 DigitalOut rotary4(p25);
pedlerw 3:1d681d86f1dd 67 DigitalOut rotary5(p26);
pedlerw 0:2d8d828ff276 68 //Set serial com//
pedlerw 0:2d8d828ff276 69 Serial xbee(p13,p14);
pedlerw 0:2d8d828ff276 70
pedlerw 0:2d8d828ff276 71 //Function Prototypes//
pedlerw 0:2d8d828ff276 72 void move_all_left(int num); //num for # of coil sets left//
pedlerw 0:2d8d828ff276 73 void move_all_right(int num); //num for # of coil sets right//
pedlerw 0:2d8d828ff276 74 float get_Lsensor_min();
pedlerw 0:2d8d828ff276 75 float get_Lsensor_max();
pedlerw 0:2d8d828ff276 76 float get_Rsensor_min();
pedlerw 0:2d8d828ff276 77 float get_Rsensor_max();
pedlerw 0:2d8d828ff276 78 void move_one_right(int p); //p for position//
pedlerw 0:2d8d828ff276 79 void move_one_left(int p); //p for position//
pedlerw 0:2d8d828ff276 80 int get_position();
pedlerw 0:2d8d828ff276 81 void calibrate_linear();
pedlerw 0:2d8d828ff276 82 void read_file();
pedlerw 0:2d8d828ff276 83 int move_linear(int l); //l for location//
pedlerw 0:2d8d828ff276 84 int error_check();
pedlerw 0:2d8d828ff276 85 void display_mode();
pedlerw 3:1d681d86f1dd 86 void move_all_leftBobby(int num);
pedlerw 0:2d8d828ff276 87 void move_all_rightBobby(int num);
pedlerw 1:99f06e7e6906 88 void scroll_up(int line); //line for number lines to scroll//
pedlerw 3:1d681d86f1dd 89 void cork_screw();
pedlerw 3:1d681d86f1dd 90 void rotate_all_cw(int set, float speed);
pedlerw 3:1d681d86f1dd 91 void rotate_all_ccw(int set, float speed);
pedlerw 3:1d681d86f1dd 92 void rotary_speed();
pedlerw 3:1d681d86f1dd 93 void fast_rotary_cw();
pedlerw 4:29aafde2dcbc 94 void small_rotary_speed();
pedlerw 0:2d8d828ff276 95 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 96 //////////////////////////////////MAIN/////////////////////////////////////////
pedlerw 0:2d8d828ff276 97 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 98 int main()
pedlerw 3:1d681d86f1dd 99 {
pedlerw 0:2d8d828ff276 100 //Declarations & Initializations//
pedlerw 0:2d8d828ff276 101 int desired_location = 0;
pedlerw 0:2d8d828ff276 102 int user_instruction = -1;
pedlerw 3:1d681d86f1dd 103
pedlerw 0:2d8d828ff276 104 //Initialize Linear Coils//
pedlerw 0:2d8d828ff276 105 linear0 = 1;
pedlerw 0:2d8d828ff276 106 linear1 = 1;
pedlerw 0:2d8d828ff276 107 linear2 = 1;
pedlerw 0:2d8d828ff276 108 linear3 = 1;
pedlerw 0:2d8d828ff276 109 linear4 = 1;
pedlerw 0:2d8d828ff276 110 linear5 = 1;
pedlerw 3:1d681d86f1dd 111 linear6 = 1;
pedlerw 3:1d681d86f1dd 112 //Initialize Rotary Coils//
pedlerw 3:1d681d86f1dd 113 //Declar rotary coils Digital I/O//
pedlerw 3:1d681d86f1dd 114 rotary0 = 1;
pedlerw 3:1d681d86f1dd 115 rotary1 = 1;
pedlerw 3:1d681d86f1dd 116 rotary2 = 1;
pedlerw 3:1d681d86f1dd 117 rotary3 = 1;
pedlerw 3:1d681d86f1dd 118 rotary4 = 1;
pedlerw 3:1d681d86f1dd 119 rotary5 = 1;
pedlerw 0:2d8d828ff276 120 //Initialize onbord led//
pedlerw 0:2d8d828ff276 121 led1 = 0;
pedlerw 0:2d8d828ff276 122 led2 = 0;
pedlerw 0:2d8d828ff276 123 led3 = 0;
pedlerw 0:2d8d828ff276 124 led4 = 0;
pedlerw 0:2d8d828ff276 125 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 126 ///////////////////////////New User Interface//////////////////////////////////
pedlerw 0:2d8d828ff276 127 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 128
pedlerw 3:1d681d86f1dd 129 while(1) {
pedlerw 3:1d681d86f1dd 130 //-Prompt user with 2 options-
pedlerw 3:1d681d86f1dd 131 switch(user_instruction) {
pedlerw 3:1d681d86f1dd 132 case 0:
pedlerw 3:1d681d86f1dd 133 user_instruction = -1;
pedlerw 3:1d681d86f1dd 134 break;
pedlerw 3:1d681d86f1dd 135 case 1:
pedlerw 0:2d8d828ff276 136 //Calibrate Motor//
pedlerw 3:1d681d86f1dd 137 calibrate_linear();
pedlerw 0:2d8d828ff276 138 //Go to operate motor//
pedlerw 0:2d8d828ff276 139 user_instruction = 4; //Go to operation menu//
pedlerw 3:1d681d86f1dd 140 break;
pedlerw 0:2d8d828ff276 141 case 2:
pedlerw 0:2d8d828ff276 142 //Use old calibration file and begin operation
pedlerw 0:2d8d828ff276 143 //Read file to fill in max and min//
pedlerw 3:1d681d86f1dd 144 read_file();
pedlerw 0:2d8d828ff276 145 user_instruction = 4; //Go to operation menu//
pedlerw 3:1d681d86f1dd 146 break;
pedlerw 0:2d8d828ff276 147 case 3:
pedlerw 3:1d681d86f1dd 148 display_mode();
pedlerw 0:2d8d828ff276 149 user_instruction = -1; //Go to main menu//
pedlerw 3:1d681d86f1dd 150 break;
pedlerw 0:2d8d828ff276 151 case 4:
pedlerw 0:2d8d828ff276 152 //Operate Linear Part//
pedlerw 3:1d681d86f1dd 153 for(int i=0; i<30; i++) {
pedlerw 0:2d8d828ff276 154 xbee.printf("\r\n");
pedlerw 3:1d681d86f1dd 155 }//End of for
pedlerw 0:2d8d828ff276 156 scroll_up(30);
pedlerw 0:2d8d828ff276 157 xbee.printf("\r\n\n****Operation Menu****");
pedlerw 3:1d681d86f1dd 158 xbee.printf("\r\nEnter a position number from 2 to 38: ");
pedlerw 3:1d681d86f1dd 159 xbee.printf("\r\nTo get back to the Main Menu press 0 and <Enter>.");
pedlerw 3:1d681d86f1dd 160 scroll_up(25);
pedlerw 3:1d681d86f1dd 161 desired_location = error_check();
pedlerw 3:1d681d86f1dd 162 if(desired_location == 0) {
pedlerw 3:1d681d86f1dd 163 user_instruction = -1;
pedlerw 3:1d681d86f1dd 164 break;
pedlerw 3:1d681d86f1dd 165 }//End of if
pedlerw 3:1d681d86f1dd 166 else if(desired_location > 40) {
pedlerw 3:1d681d86f1dd 167 user_instruction = 4;
pedlerw 3:1d681d86f1dd 168 xbee.printf("\r\nUser input was out of bounds");
pedlerw 3:1d681d86f1dd 169 xbee.printf("\r\nPlease try again");
pedlerw 3:1d681d86f1dd 170 scroll_up(20);
pedlerw 3:1d681d86f1dd 171 wait(slow);
pedlerw 3:1d681d86f1dd 172 break;
pedlerw 3:1d681d86f1dd 173 }//End of else if
pedlerw 3:1d681d86f1dd 174 else if(desired_location < 0) {
pedlerw 3:1d681d86f1dd 175 user_instruction = 4;
pedlerw 3:1d681d86f1dd 176 xbee.printf("\r\nUser input was out of bounds");
pedlerw 3:1d681d86f1dd 177 xbee.printf("\r\nPlease try again");
pedlerw 3:1d681d86f1dd 178 scroll_up(20);
pedlerw 3:1d681d86f1dd 179 wait(slow);
pedlerw 3:1d681d86f1dd 180 break;
pedlerw 3:1d681d86f1dd 181 }//End of else if
pedlerw 3:1d681d86f1dd 182 xbee.printf("\r\nGoing to position %d", desired_location);
pedlerw 0:2d8d828ff276 183 //Pass desired_location to move_linear();
pedlerw 0:2d8d828ff276 184 //On normal exit move_linear returns 4... causes repeat of operation menu//
pedlerw 0:2d8d828ff276 185 user_instruction = move_linear(desired_location);
pedlerw 3:1d681d86f1dd 186 break;
pedlerw 0:2d8d828ff276 187 default:
pedlerw 0:2d8d828ff276 188 //Get valid user_input//
pedlerw 1:99f06e7e6906 189 scroll_up(30);
pedlerw 1:99f06e7e6906 190 xbee.printf("\r\n\n****Main Menu****");
pedlerw 4:29aafde2dcbc 191 xbee.printf("\r\nFor Calibration press 1 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 192 xbee.printf("\r\nTo enter a desired location press 2 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 193 xbee.printf("\r\nFor display mode press 3 and\r\n<Enter>.");
pedlerw 4:29aafde2dcbc 194 scroll_up(22);
pedlerw 3:1d681d86f1dd 195 user_instruction = error_check();
pedlerw 3:1d681d86f1dd 196 break;
pedlerw 0:2d8d828ff276 197 }//End of switch
pedlerw 0:2d8d828ff276 198 }//End of while(1)//
pedlerw 0:2d8d828ff276 199 }//End of main
pedlerw 0:2d8d828ff276 200 /*******************************************************************************
pedlerw 0:2d8d828ff276 201 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 202 /////////////////////////////FUNCITONS/////////////////////////////////////////
pedlerw 0:2d8d828ff276 203 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 204 *******************************************************************************/
pedlerw 0:2d8d828ff276 205 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 206 ////////////////////////////move_all_left//////////////////////////////////////
pedlerw 0:2d8d828ff276 207 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 208 //This function accepts an interger value and moves the forcer to the left//
pedlerw 0:2d8d828ff276 209 //The interger number of coil sets//
pedlerw 3:1d681d86f1dd 210 void move_all_left(int set)
pedlerw 3:1d681d86f1dd 211 {
pedlerw 0:2d8d828ff276 212 //Declartions and initialization//
pedlerw 3:1d681d86f1dd 213 linear0 = 1; //Coil1 Mod0
pedlerw 3:1d681d86f1dd 214 linear1 = 1; //Coil2 Mod1
pedlerw 3:1d681d86f1dd 215 linear2 = 1; //Coil3 Mod2
pedlerw 3:1d681d86f1dd 216 linear3 = 1; //Coil4 Mod3
pedlerw 3:1d681d86f1dd 217 linear4 = 1; //Coil5 Mod4
pedlerw 3:1d681d86f1dd 218 linear5 = 1; //Coil6 Mod5
pedlerw 3:1d681d86f1dd 219 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 220
pedlerw 3:1d681d86f1dd 221 //LEFT//
pedlerw 3:1d681d86f1dd 222 for(int i=0; i<set; i++) {
pedlerw 3:1d681d86f1dd 223 linear4 =! linear4; //Coil 5
pedlerw 3:1d681d86f1dd 224 wait(fast);
pedlerw 3:1d681d86f1dd 225 linear4 =! linear4;
pedlerw 3:1d681d86f1dd 226 linear5 =! linear5; //Coil 6
pedlerw 3:1d681d86f1dd 227 wait(fast);
pedlerw 3:1d681d86f1dd 228 linear5 =! linear5;
pedlerw 3:1d681d86f1dd 229 linear6 =! linear6; //Coil 7
pedlerw 3:1d681d86f1dd 230 wait(fast);
pedlerw 3:1d681d86f1dd 231 linear6 =! linear6;
pedlerw 3:1d681d86f1dd 232 linear0 =! linear0; //Coil 1
pedlerw 3:1d681d86f1dd 233 wait(fast);
pedlerw 3:1d681d86f1dd 234 linear0 =! linear0;
pedlerw 3:1d681d86f1dd 235 linear1 =! linear1; //Coil 2
pedlerw 3:1d681d86f1dd 236 wait(fast);
pedlerw 3:1d681d86f1dd 237 linear1 =! linear1;
pedlerw 3:1d681d86f1dd 238 linear2 =! linear2; //Coil 3
pedlerw 3:1d681d86f1dd 239 wait(fast);
pedlerw 3:1d681d86f1dd 240 linear2 =! linear2;
pedlerw 3:1d681d86f1dd 241 linear3 =! linear3; //Coil 4
pedlerw 3:1d681d86f1dd 242 wait(fast);
pedlerw 3:1d681d86f1dd 243 linear3 =! linear3;
pedlerw 3:1d681d86f1dd 244 }//End of For loop
pedlerw 0:2d8d828ff276 245 return;
pedlerw 0:2d8d828ff276 246 }//End of Funciton
pedlerw 0:2d8d828ff276 247
pedlerw 0:2d8d828ff276 248 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 249 ////////////////////////////move_all_right/////////////////////////////////////
pedlerw 0:2d8d828ff276 250 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 251 //This function accepts an interger value and moves the forcer to the right//
pedlerw 0:2d8d828ff276 252 //The interger number of coil sets//
pedlerw 3:1d681d86f1dd 253 void move_all_right(int set)
pedlerw 3:1d681d86f1dd 254 {
pedlerw 0:2d8d828ff276 255 //Declartions and initialization//
pedlerw 3:1d681d86f1dd 256 linear0 = 1; //Coil1 Mod0
pedlerw 3:1d681d86f1dd 257 linear1 = 1; //Coil2 Mod1
pedlerw 3:1d681d86f1dd 258 linear2 = 1; //Coil3 Mod2
pedlerw 3:1d681d86f1dd 259 linear3 = 1; //Coil4 Mod3
pedlerw 3:1d681d86f1dd 260 linear4 = 1; //Coil5 Mod4
pedlerw 3:1d681d86f1dd 261 linear5 = 1; //Coil6 Mod5
pedlerw 3:1d681d86f1dd 262 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 263
pedlerw 3:1d681d86f1dd 264 //RIGHT//
pedlerw 3:1d681d86f1dd 265 for(int i=0; i<set; i++) {
pedlerw 3:1d681d86f1dd 266 linear0 =! linear0; //Coil 1
pedlerw 3:1d681d86f1dd 267 wait(fast);
pedlerw 3:1d681d86f1dd 268 linear0 =! linear0;
pedlerw 3:1d681d86f1dd 269 linear6 =! linear6; //Coil 7
pedlerw 3:1d681d86f1dd 270 wait(fast);
pedlerw 3:1d681d86f1dd 271 linear6 =! linear6;
pedlerw 3:1d681d86f1dd 272 linear5 =! linear5; //Coil 6
pedlerw 3:1d681d86f1dd 273 wait(fast);
pedlerw 3:1d681d86f1dd 274 linear5 =! linear5;
pedlerw 3:1d681d86f1dd 275 linear4 =! linear4; //Coil 5
pedlerw 3:1d681d86f1dd 276 wait(fast);
pedlerw 3:1d681d86f1dd 277 linear4 =! linear4;
pedlerw 3:1d681d86f1dd 278 linear3 =! linear3; //Coil 4
pedlerw 3:1d681d86f1dd 279 wait(fast);
pedlerw 3:1d681d86f1dd 280 linear3 =! linear3;
pedlerw 3:1d681d86f1dd 281 linear2 =! linear2; //Coil 3
pedlerw 3:1d681d86f1dd 282 wait(fast);
pedlerw 3:1d681d86f1dd 283 linear2 =! linear2;
pedlerw 3:1d681d86f1dd 284 linear1 =! linear1; //Coil 2
pedlerw 3:1d681d86f1dd 285 wait(fast);
pedlerw 3:1d681d86f1dd 286 linear1 =! linear1;
pedlerw 3:1d681d86f1dd 287 }//End of For loop
pedlerw 0:2d8d828ff276 288 return;
pedlerw 0:2d8d828ff276 289 }//End of Funciton
pedlerw 0:2d8d828ff276 290
pedlerw 0:2d8d828ff276 291 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 292 ////////////////////////////get_Lsensor_min////////////////////////////////////
pedlerw 0:2d8d828ff276 293 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 294 //returns the min distance value of left sensor when called//
pedlerw 3:1d681d86f1dd 295 float get_Lsensor_min()
pedlerw 3:1d681d86f1dd 296 {
pedlerw 0:2d8d828ff276 297 //Declarations and initializations//
pedlerw 0:2d8d828ff276 298 float min = MAX;
pedlerw 0:2d8d828ff276 299 float read;
pedlerw 3:1d681d86f1dd 300 for(int i=0; i<large_num; i++) {
pedlerw 3:1d681d86f1dd 301 wait(sensor_set);
pedlerw 3:1d681d86f1dd 302 read = distanceL.read();
pedlerw 3:1d681d86f1dd 303 if(read < min) {
pedlerw 3:1d681d86f1dd 304 min = read;
pedlerw 3:1d681d86f1dd 305 }//End of if
pedlerw 3:1d681d86f1dd 306 else {
pedlerw 3:1d681d86f1dd 307 wait(MIN);
pedlerw 3:1d681d86f1dd 308 }//End of else
pedlerw 3:1d681d86f1dd 309 }//End of for
pedlerw 0:2d8d828ff276 310 return(min);
pedlerw 0:2d8d828ff276 311 }//End of Function
pedlerw 0:2d8d828ff276 312
pedlerw 0:2d8d828ff276 313 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 314 //////////////////////////////get_Lsensor_max//////////////////////////////////
pedlerw 0:2d8d828ff276 315 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 316 //returns the max distance value of the left sensor when called//
pedlerw 3:1d681d86f1dd 317 float get_Lsensor_max()
pedlerw 3:1d681d86f1dd 318 {
pedlerw 0:2d8d828ff276 319 //Declarations and initializations//
pedlerw 0:2d8d828ff276 320 float max = MIN;
pedlerw 0:2d8d828ff276 321 float read;
pedlerw 3:1d681d86f1dd 322 for(int i=0; i<large_num; i++) {
pedlerw 3:1d681d86f1dd 323 wait(sensor_set);
pedlerw 3:1d681d86f1dd 324 read = distanceL.read();
pedlerw 3:1d681d86f1dd 325 if(read > max) {
pedlerw 3:1d681d86f1dd 326 max = read;
pedlerw 3:1d681d86f1dd 327 }//End of if
pedlerw 3:1d681d86f1dd 328 else {
pedlerw 3:1d681d86f1dd 329 wait(MIN);
pedlerw 3:1d681d86f1dd 330 }//End of else
pedlerw 3:1d681d86f1dd 331 }//End of for
pedlerw 0:2d8d828ff276 332 return(max);
pedlerw 0:2d8d828ff276 333 }//End of function
pedlerw 0:2d8d828ff276 334
pedlerw 0:2d8d828ff276 335 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 336 /////////////////////////////get_Rsensor_min///////////////////////////////////
pedlerw 0:2d8d828ff276 337 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 338 //Function returns the min value of the right sensor when called//
pedlerw 3:1d681d86f1dd 339 float get_Rsensor_min()
pedlerw 3:1d681d86f1dd 340 {
pedlerw 0:2d8d828ff276 341 //Declarations and initializations//
pedlerw 0:2d8d828ff276 342 float min = MAX;
pedlerw 0:2d8d828ff276 343 float read;
pedlerw 3:1d681d86f1dd 344 for(int i=0; i<large_num; i++) {
pedlerw 3:1d681d86f1dd 345 wait(sensor_set);
pedlerw 3:1d681d86f1dd 346 read = distanceR.read();
pedlerw 3:1d681d86f1dd 347 if(read < min) {
pedlerw 3:1d681d86f1dd 348 min = read;
pedlerw 3:1d681d86f1dd 349 }//End of if
pedlerw 3:1d681d86f1dd 350 else {
pedlerw 3:1d681d86f1dd 351 wait(MIN);
pedlerw 3:1d681d86f1dd 352 }//End of else
pedlerw 3:1d681d86f1dd 353 }//End of for
pedlerw 0:2d8d828ff276 354 return(min);
pedlerw 0:2d8d828ff276 355 }//End of function
pedlerw 0:2d8d828ff276 356
pedlerw 0:2d8d828ff276 357 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 358 ///////////////////////////////get_Rsensor_max/////////////////////////////////
pedlerw 0:2d8d828ff276 359 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 360 float get_Rsensor_max()
pedlerw 3:1d681d86f1dd 361 {
pedlerw 0:2d8d828ff276 362 //Declarations and initializations//
pedlerw 0:2d8d828ff276 363 float max = MIN;
pedlerw 0:2d8d828ff276 364 float read;
pedlerw 3:1d681d86f1dd 365 for(int i=0; i<large_num; i++) {
pedlerw 3:1d681d86f1dd 366 wait(sensor_set);
pedlerw 3:1d681d86f1dd 367 read = distanceR.read();
pedlerw 3:1d681d86f1dd 368 if(read > max) {
pedlerw 3:1d681d86f1dd 369 max = read;
pedlerw 3:1d681d86f1dd 370 }//End of if
pedlerw 3:1d681d86f1dd 371 else {
pedlerw 3:1d681d86f1dd 372 wait(MIN);
pedlerw 3:1d681d86f1dd 373 }//End of else
pedlerw 3:1d681d86f1dd 374 }//End of for
pedlerw 0:2d8d828ff276 375 return(max);
pedlerw 0:2d8d828ff276 376 }//End of function
pedlerw 0:2d8d828ff276 377
pedlerw 0:2d8d828ff276 378 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 379 ///////////////////////////////move_one_right//////////////////////////////////
pedlerw 0:2d8d828ff276 380 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 381 //Function takes current position and moves one to the right
pedlerw 3:1d681d86f1dd 382 void move_one_right(int position)
pedlerw 3:1d681d86f1dd 383 {
pedlerw 0:2d8d828ff276 384 //Declarations and initializations//
pedlerw 0:2d8d828ff276 385 int current_coil = abs(position % seven);
pedlerw 3:1d681d86f1dd 386 //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 3:1d681d86f1dd 387 switch(current_coil) {
pedlerw 3:1d681d86f1dd 388 case 0:
pedlerw 3:1d681d86f1dd 389 linear6 = 1;
pedlerw 3:1d681d86f1dd 390 linear5 = 1;
pedlerw 3:1d681d86f1dd 391 linear4 = 1;
pedlerw 3:1d681d86f1dd 392 linear3 = 1;
pedlerw 3:1d681d86f1dd 393 linear2 = 1;
pedlerw 3:1d681d86f1dd 394 linear1 = 1;
pedlerw 3:1d681d86f1dd 395 linear0 = 0; //Fire Coil 1//
pedlerw 0:2d8d828ff276 396 break;
pedlerw 3:1d681d86f1dd 397 case 1:
pedlerw 3:1d681d86f1dd 398 linear6 = 0; //Fire Coil 7//
pedlerw 3:1d681d86f1dd 399 linear5 = 1;
pedlerw 3:1d681d86f1dd 400 linear4 = 1;
pedlerw 3:1d681d86f1dd 401 linear3 = 1;
pedlerw 3:1d681d86f1dd 402 linear2 = 1;
pedlerw 3:1d681d86f1dd 403 linear1 = 1;
pedlerw 3:1d681d86f1dd 404 linear0 = 1;
pedlerw 0:2d8d828ff276 405 break;
pedlerw 3:1d681d86f1dd 406 case 2:
pedlerw 3:1d681d86f1dd 407 linear6 = 1;
pedlerw 3:1d681d86f1dd 408 linear5 = 0; //Fire Coil 6//
pedlerw 3:1d681d86f1dd 409 linear4 = 1;
pedlerw 3:1d681d86f1dd 410 linear3 = 1;
pedlerw 3:1d681d86f1dd 411 linear2 = 1;
pedlerw 3:1d681d86f1dd 412 linear1 = 1;
pedlerw 3:1d681d86f1dd 413 linear0 = 1;
pedlerw 0:2d8d828ff276 414 break;
pedlerw 3:1d681d86f1dd 415 case 3:
pedlerw 3:1d681d86f1dd 416 linear6 = 1;
pedlerw 3:1d681d86f1dd 417 linear5 = 1;
pedlerw 3:1d681d86f1dd 418 linear4 = 0; //Fire Coil 5//
pedlerw 3:1d681d86f1dd 419 linear3 = 1;
pedlerw 3:1d681d86f1dd 420 linear2 = 1;
pedlerw 3:1d681d86f1dd 421 linear1 = 1;
pedlerw 3:1d681d86f1dd 422 linear0 = 1;
pedlerw 0:2d8d828ff276 423 break;
pedlerw 3:1d681d86f1dd 424 case 4:
pedlerw 3:1d681d86f1dd 425 linear6 = 1;
pedlerw 3:1d681d86f1dd 426 linear5 = 1;
pedlerw 3:1d681d86f1dd 427 linear4 = 1;
pedlerw 3:1d681d86f1dd 428 linear3 = 0; //Fire Coil 4//
pedlerw 3:1d681d86f1dd 429 linear2 = 1;
pedlerw 3:1d681d86f1dd 430 linear1 = 1;
pedlerw 3:1d681d86f1dd 431 linear0 = 1;
pedlerw 0:2d8d828ff276 432 break;
pedlerw 3:1d681d86f1dd 433 case 5:
pedlerw 3:1d681d86f1dd 434 linear6 = 1;
pedlerw 3:1d681d86f1dd 435 linear5 = 1;
pedlerw 3:1d681d86f1dd 436 linear4 = 1;
pedlerw 3:1d681d86f1dd 437 linear3 = 1;
pedlerw 3:1d681d86f1dd 438 linear2 = 0; //Fire Coil 3//
pedlerw 3:1d681d86f1dd 439 linear1 = 1;
pedlerw 3:1d681d86f1dd 440 linear0 = 1;
pedlerw 0:2d8d828ff276 441 break;
pedlerw 3:1d681d86f1dd 442 case 6:
pedlerw 3:1d681d86f1dd 443 linear6 = 1;
pedlerw 3:1d681d86f1dd 444 linear5 = 1;
pedlerw 3:1d681d86f1dd 445 linear4 = 1;
pedlerw 3:1d681d86f1dd 446 linear3 = 1;
pedlerw 3:1d681d86f1dd 447 linear2 = 1;
pedlerw 3:1d681d86f1dd 448 linear1 = 0; //Fire Coil 2//
pedlerw 3:1d681d86f1dd 449 linear0 = 1;
pedlerw 0:2d8d828ff276 450 break;
pedlerw 3:1d681d86f1dd 451 default:
pedlerw 3:1d681d86f1dd 452 wait(MIN);
pedlerw 3:1d681d86f1dd 453 }//End of switch
pedlerw 0:2d8d828ff276 454 }//End of function
pedlerw 0:2d8d828ff276 455
pedlerw 0:2d8d828ff276 456 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 457 ///////////////////////////////move_one_left//////////////////////////////////
pedlerw 0:2d8d828ff276 458 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 459 //Function takes current position and moves one to the left
pedlerw 3:1d681d86f1dd 460 void move_one_left(int position)
pedlerw 3:1d681d86f1dd 461 {
pedlerw 0:2d8d828ff276 462 //Declarations and initializations//
pedlerw 0:2d8d828ff276 463 int current_coil = position % seven;
pedlerw 3:1d681d86f1dd 464 //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 3:1d681d86f1dd 465 switch(current_coil) {
pedlerw 3:1d681d86f1dd 466 case 0:
pedlerw 3:1d681d86f1dd 467 linear6 = 1;
pedlerw 3:1d681d86f1dd 468 linear5 = 0; //Fire Coil 6//
pedlerw 3:1d681d86f1dd 469 linear4 = 1;
pedlerw 3:1d681d86f1dd 470 linear3 = 1;
pedlerw 3:1d681d86f1dd 471 linear2 = 1;
pedlerw 3:1d681d86f1dd 472 linear1 = 1;
pedlerw 3:1d681d86f1dd 473 linear0 = 1;
pedlerw 0:2d8d828ff276 474 break;
pedlerw 3:1d681d86f1dd 475 case 1:
pedlerw 3:1d681d86f1dd 476 linear6 = 1;
pedlerw 3:1d681d86f1dd 477 linear5 = 1;
pedlerw 3:1d681d86f1dd 478 linear4 = 0; //Fire Coil 5//
pedlerw 3:1d681d86f1dd 479 linear3 = 1;
pedlerw 3:1d681d86f1dd 480 linear2 = 1;
pedlerw 3:1d681d86f1dd 481 linear1 = 1;
pedlerw 3:1d681d86f1dd 482 linear0 = 1;
pedlerw 0:2d8d828ff276 483 break;
pedlerw 3:1d681d86f1dd 484 case 2:
pedlerw 3:1d681d86f1dd 485 linear6 = 1;
pedlerw 3:1d681d86f1dd 486 linear5 = 1;
pedlerw 3:1d681d86f1dd 487 linear4 = 1;
pedlerw 3:1d681d86f1dd 488 linear3 = 0; //Fire Coil 4//
pedlerw 3:1d681d86f1dd 489 linear2 = 1;
pedlerw 3:1d681d86f1dd 490 linear1 = 1;
pedlerw 3:1d681d86f1dd 491 linear0 = 1;
pedlerw 0:2d8d828ff276 492 break;
pedlerw 3:1d681d86f1dd 493 case 3:
pedlerw 3:1d681d86f1dd 494 linear6 = 1;
pedlerw 3:1d681d86f1dd 495 linear5 = 1;
pedlerw 3:1d681d86f1dd 496 linear4 = 1;
pedlerw 3:1d681d86f1dd 497 linear3 = 1;
pedlerw 3:1d681d86f1dd 498 linear2 = 0; //Fire Coil 3//
pedlerw 3:1d681d86f1dd 499 linear1 = 1;
pedlerw 3:1d681d86f1dd 500 linear0 = 1;
pedlerw 0:2d8d828ff276 501 break;
pedlerw 3:1d681d86f1dd 502 case 4:
pedlerw 3:1d681d86f1dd 503 linear6 = 1;
pedlerw 3:1d681d86f1dd 504 linear5 = 1;
pedlerw 3:1d681d86f1dd 505 linear4 = 1;
pedlerw 3:1d681d86f1dd 506 linear3 = 1;
pedlerw 3:1d681d86f1dd 507 linear2 = 1;
pedlerw 3:1d681d86f1dd 508 linear1 = 0; //Fire Coil 2//
pedlerw 3:1d681d86f1dd 509 linear0 = 1;
pedlerw 0:2d8d828ff276 510 break;
pedlerw 3:1d681d86f1dd 511 case 5:
pedlerw 3:1d681d86f1dd 512 linear6 = 1;
pedlerw 3:1d681d86f1dd 513 linear5 = 1;
pedlerw 3:1d681d86f1dd 514 linear4 = 1;
pedlerw 3:1d681d86f1dd 515 linear3 = 1;
pedlerw 3:1d681d86f1dd 516 linear2 = 1;
pedlerw 3:1d681d86f1dd 517 linear1 = 1;
pedlerw 3:1d681d86f1dd 518 linear0 = 0; //Fire Coil 1//
pedlerw 0:2d8d828ff276 519 break;
pedlerw 3:1d681d86f1dd 520 case 6:
pedlerw 3:1d681d86f1dd 521 linear6 = 0; //Fire Coil 7//
pedlerw 3:1d681d86f1dd 522 linear5 = 1;
pedlerw 3:1d681d86f1dd 523 linear4 = 1;
pedlerw 3:1d681d86f1dd 524 linear3 = 1;
pedlerw 3:1d681d86f1dd 525 linear2 = 1;
pedlerw 3:1d681d86f1dd 526 linear1 = 1;
pedlerw 3:1d681d86f1dd 527 linear0 = 1;
pedlerw 0:2d8d828ff276 528 break;
pedlerw 3:1d681d86f1dd 529 default:
pedlerw 3:1d681d86f1dd 530 wait(MIN);
pedlerw 3:1d681d86f1dd 531 }//End of switch
pedlerw 0:2d8d828ff276 532 }//End of function
pedlerw 0:2d8d828ff276 533
pedlerw 0:2d8d828ff276 534 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 535 /////////////////////////////get_position//////////////////////////////////////
pedlerw 0:2d8d828ff276 536 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 537
pedlerw 0:2d8d828ff276 538 //Function reads current position on track and returns the mod 7//
pedlerw 0:2d8d828ff276 539 //Function uses calibration data to determin current position//
pedlerw 3:1d681d86f1dd 540 int get_position()
pedlerw 3:1d681d86f1dd 541 {
pedlerw 0:2d8d828ff276 542 //Declrations and Initializaitons//
pedlerw 0:2d8d828ff276 543 int position = -1;
pedlerw 0:2d8d828ff276 544 int count = 0;
pedlerw 3:1d681d86f1dd 545 while(position == -1) {
pedlerw 0:2d8d828ff276 546 wait(slow);
pedlerw 0:2d8d828ff276 547 float left_sensor = distanceL.read();
pedlerw 0:2d8d828ff276 548 float right_sensor = distanceR.read();
pedlerw 0:2d8d828ff276 549 //xbee.printf("\r\nleft_sensor:%f",left_sensor);
pedlerw 3:1d681d86f1dd 550 //xbee.printf("\r\nright_sensor:%f",right_sensor);
pedlerw 0:2d8d828ff276 551 //Use closest sensor//
pedlerw 3:1d681d86f1dd 552 if(left_sensor > right_sensor) {
pedlerw 0:2d8d828ff276 553 //xbee.printf("\r\nleft > right");
pedlerw 0:2d8d828ff276 554 //Look at Lsensor data//
pedlerw 3:1d681d86f1dd 555 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 556 if(left_sensor <= left_max[i] && left_sensor >= left_min[i]) {
pedlerw 0:2d8d828ff276 557 position = i;
pedlerw 0:2d8d828ff276 558 xbee.printf("\r\nPosition L1 %d",position);
pedlerw 0:2d8d828ff276 559 return position;
pedlerw 0:2d8d828ff276 560 }//End of if//
pedlerw 3:1d681d86f1dd 561 else if(left_sensor < left_max[i] && left_sensor > left_max[i+one]) {
pedlerw 0:2d8d828ff276 562 position = i+one;
pedlerw 0:2d8d828ff276 563 xbee.printf("\r\nPosition L2 %d",position);
pedlerw 0:2d8d828ff276 564 return position;
pedlerw 0:2d8d828ff276 565 }//End of else if//
pedlerw 3:1d681d86f1dd 566 else if(left_sensor < left_max[i] && left_sensor > left_min[i+one]) {
pedlerw 0:2d8d828ff276 567 position = i;
pedlerw 0:2d8d828ff276 568 xbee.printf("\r\nPosition L3 %d",position);
pedlerw 0:2d8d828ff276 569 return position;
pedlerw 0:2d8d828ff276 570 }//End of else if//
pedlerw 0:2d8d828ff276 571 }//End of for//
pedlerw 0:2d8d828ff276 572 }//End of if//
pedlerw 3:1d681d86f1dd 573
pedlerw 3:1d681d86f1dd 574 else if(right_sensor >= left_sensor) {
pedlerw 0:2d8d828ff276 575 xbee.printf("\r\nright > left");
pedlerw 0:2d8d828ff276 576 //Look at Rsensor data//
pedlerw 3:1d681d86f1dd 577 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 578 if(right_sensor <= right_max[i] && right_sensor >= right_min[i]) {
pedlerw 0:2d8d828ff276 579 position = i;
pedlerw 0:2d8d828ff276 580 xbee.printf("\r\nPosition R1 %d",position);
pedlerw 0:2d8d828ff276 581 return position;
pedlerw 0:2d8d828ff276 582 }//End of if//
pedlerw 3:1d681d86f1dd 583 else if(right_sensor > right_max[i] && right_sensor < right_max[i+one]) {
pedlerw 0:2d8d828ff276 584 position = i+one;
pedlerw 0:2d8d828ff276 585 xbee.printf("\r\nPosition R2 %d",position);
pedlerw 0:2d8d828ff276 586 return position;
pedlerw 0:2d8d828ff276 587 }//End of else if//
pedlerw 3:1d681d86f1dd 588 else if(right_sensor > right_max[i] && right_sensor < right_min[i+one]) {
pedlerw 0:2d8d828ff276 589 position = i;
pedlerw 0:2d8d828ff276 590 xbee.printf("\r\nPosition R3 %d",position);
pedlerw 0:2d8d828ff276 591 return position;
pedlerw 0:2d8d828ff276 592 }//End of else if//
pedlerw 0:2d8d828ff276 593 }//End of for//
pedlerw 3:1d681d86f1dd 594 }//End of else if//
pedlerw 0:2d8d828ff276 595 count++;
pedlerw 3:1d681d86f1dd 596 if(count > five) {
pedlerw 3:1d681d86f1dd 597 return(-2);
pedlerw 0:2d8d828ff276 598 }//End of if//
pedlerw 0:2d8d828ff276 599 }//End of while(position....
pedlerw 0:2d8d828ff276 600 return position;
pedlerw 0:2d8d828ff276 601 }//End of function
pedlerw 0:2d8d828ff276 602
pedlerw 0:2d8d828ff276 603 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 604 ////////////////////////////////CALIBRATE//////////////////////////////////////
pedlerw 0:2d8d828ff276 605 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 606 /*
pedlerw 0:2d8d828ff276 607 -Calibrate Motor
pedlerw 0:2d8d828ff276 608 1. Move all the way to the LEFT side of the track.
pedlerw 0:2d8d828ff276 609 2. Get max and mins from both distance sensors.
pedlerw 0:2d8d828ff276 610 3. Store the max and mins in various arrays.
pedlerw 0:2d8d828ff276 611 4. Move one position to the RIGHT and repeat for entire track.
pedlerw 0:2d8d828ff276 612 5. Once the entire track has been recorded export to a txt file on the mbed.
pedlerw 0:2d8d828ff276 613 Format for txt file:
pedlerw 0:2d8d828ff276 614 Position # :Left Low :Left High :Right Low :Right High
pedlerw 0:2d8d828ff276 615 0 :#### :#### :#### :####
pedlerw 0:2d8d828ff276 616 1 (3 \t) :#### (2 \t)...
pedlerw 0:2d8d828ff276 617 .
pedlerw 0:2d8d828ff276 618 .
pedlerw 0:2d8d828ff276 619 #EOF
pedlerw 0:2d8d828ff276 620 */
pedlerw 3:1d681d86f1dd 621 void calibrate_linear()
pedlerw 3:1d681d86f1dd 622 {
pedlerw 0:2d8d828ff276 623 //1.Move all the way to the LEFT side of the track//
pedlerw 3:1d681d86f1dd 624 move_all_left(six);
pedlerw 0:2d8d828ff276 625 //2.Get Sensor Data//
pedlerw 3:1d681d86f1dd 626 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 627 //3.Store in various arrays
pedlerw 3:1d681d86f1dd 628 wait(slow);
pedlerw 3:1d681d86f1dd 629 left_min[i] = get_Lsensor_min();
pedlerw 3:1d681d86f1dd 630 left_max[i] = get_Lsensor_max();
pedlerw 3:1d681d86f1dd 631 right_min[i] = get_Rsensor_min();
pedlerw 3:1d681d86f1dd 632 right_max[i] = get_Rsensor_max();
pedlerw 3:1d681d86f1dd 633 led4 = !led4;
pedlerw 1:99f06e7e6906 634 //4.Move to the right one//
pedlerw 3:1d681d86f1dd 635 move_one_right(i);
pedlerw 3:1d681d86f1dd 636 }//End of for
pedlerw 3:1d681d86f1dd 637
pedlerw 0:2d8d828ff276 638 //5. Export data to a txt file on the mbed//
pedlerw 3:1d681d86f1dd 639 FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing
pedlerw 3:1d681d86f1dd 640 fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n");
pedlerw 3:1d681d86f1dd 641 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 642 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 3:1d681d86f1dd 643 /*
pedlerw 3:1d681d86f1dd 644 //Example File Handeling Code//
pedlerw 3:1d681d86f1dd 645 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
pedlerw 3:1d681d86f1dd 646 fprintf(fp, "Hello World!");
pedlerw 3:1d681d86f1dd 647 fclose(fp);
pedlerw 3:1d681d86f1dd 648 */
pedlerw 3:1d681d86f1dd 649 }//End of for
pedlerw 3:1d681d86f1dd 650 fclose(fp);
pedlerw 3:1d681d86f1dd 651 //Export each variable to its own file//
pedlerw 3:1d681d86f1dd 652 //Write leftmin.txt file//
pedlerw 3:1d681d86f1dd 653 FILE *fp1 = fopen("/local/leftmin.txt", "w");
pedlerw 3:1d681d86f1dd 654 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 655 fprintf(fp1,"%f\r\n",left_min[i]);
pedlerw 3:1d681d86f1dd 656 }//End of for//
pedlerw 3:1d681d86f1dd 657 fclose(fp1);
pedlerw 3:1d681d86f1dd 658 //Write leftmax.txt file//
pedlerw 3:1d681d86f1dd 659 FILE *fp2 = fopen("/local/leftmax.txt", "w");
pedlerw 3:1d681d86f1dd 660 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 661 fprintf(fp2,"%f\r\n",left_max[i]);
pedlerw 3:1d681d86f1dd 662 }//End of for//
pedlerw 3:1d681d86f1dd 663 fclose(fp2);
pedlerw 3:1d681d86f1dd 664 //Write rightmin.txt file//
pedlerw 3:1d681d86f1dd 665 FILE *fp3 = fopen("/local/rightmin.txt", "w");
pedlerw 3:1d681d86f1dd 666 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 667 fprintf(fp3,"%f\r\n",right_min[i]);
pedlerw 3:1d681d86f1dd 668 }//End of for//
pedlerw 3:1d681d86f1dd 669 fclose(fp3);
pedlerw 3:1d681d86f1dd 670 //Write rightmax.txt file//
pedlerw 3:1d681d86f1dd 671 FILE *fp4 = fopen("/local/rightmax.txt", "w");
pedlerw 3:1d681d86f1dd 672 for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 673 fprintf(fp4,"%f\r\n",right_max[i]);
pedlerw 3:1d681d86f1dd 674 }//End of for//
pedlerw 3:1d681d86f1dd 675 fclose(fp4);
pedlerw 3:1d681d86f1dd 676
pedlerw 3:1d681d86f1dd 677 //Set an LED high so we know calibration is complete//
pedlerw 3:1d681d86f1dd 678 led1 = 1;
pedlerw 3:1d681d86f1dd 679 move_all_left(six);
pedlerw 0:2d8d828ff276 680 }//End of function
pedlerw 0:2d8d828ff276 681
pedlerw 0:2d8d828ff276 682 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 683 ////////////////////////////read_file//////////////////////////////////////////
pedlerw 0:2d8d828ff276 684 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 685 //read_file reads the text files containing calibration data
pedlerw 3:1d681d86f1dd 686 void read_file()
pedlerw 3:1d681d86f1dd 687 {
pedlerw 0:2d8d828ff276 688 char line[80];
pedlerw 0:2d8d828ff276 689 int i = 0;
pedlerw 3:1d681d86f1dd 690 FILE *fr1;
pedlerw 3:1d681d86f1dd 691 fr1 = fopen ("/local/leftmin.txt", "rt");
pedlerw 3:1d681d86f1dd 692 while(fgets(line, 80, fr1) != NULL) {
pedlerw 3:1d681d86f1dd 693 sscanf (line, "%f", &left_min[i]);
pedlerw 3:1d681d86f1dd 694 i++;
pedlerw 3:1d681d86f1dd 695 }//End of while
pedlerw 3:1d681d86f1dd 696 fclose(fr1);
pedlerw 3:1d681d86f1dd 697
pedlerw 3:1d681d86f1dd 698 i = 0;
pedlerw 3:1d681d86f1dd 699 FILE *fr2;
pedlerw 3:1d681d86f1dd 700 fr2 = fopen ("/local/leftmax.txt", "rt");
pedlerw 3:1d681d86f1dd 701 while(fgets(line, 80, fr2) != NULL) {
pedlerw 3:1d681d86f1dd 702 sscanf (line, "%f", &left_max[i]);
pedlerw 3:1d681d86f1dd 703 i++;
pedlerw 3:1d681d86f1dd 704 }//End of while
pedlerw 3:1d681d86f1dd 705 fclose(fr2);
pedlerw 3:1d681d86f1dd 706
pedlerw 3:1d681d86f1dd 707 i = 0;
pedlerw 3:1d681d86f1dd 708 FILE *fr3;
pedlerw 3:1d681d86f1dd 709 fr3 = fopen ("/local/rightmin.txt", "rt");
pedlerw 3:1d681d86f1dd 710 while(fgets(line, 80, fr3) != NULL) {
pedlerw 3:1d681d86f1dd 711 sscanf (line, "%f", &right_min[i]);
pedlerw 3:1d681d86f1dd 712 i++;
pedlerw 3:1d681d86f1dd 713 }//End of while
pedlerw 3:1d681d86f1dd 714 fclose(fr3);
pedlerw 3:1d681d86f1dd 715
pedlerw 3:1d681d86f1dd 716 i = 0;
pedlerw 3:1d681d86f1dd 717 FILE *fr4;
pedlerw 3:1d681d86f1dd 718 fr4 = fopen ("/local/rightmax.txt", "rt");
pedlerw 3:1d681d86f1dd 719 while(fgets(line, 80, fr4) != NULL) {
pedlerw 3:1d681d86f1dd 720 sscanf (line, "%f", &right_max[i]);
pedlerw 3:1d681d86f1dd 721 i++;
pedlerw 3:1d681d86f1dd 722 }//End of while
pedlerw 3:1d681d86f1dd 723 fclose(fr4);
pedlerw 0:2d8d828ff276 724 }//End of function
pedlerw 0:2d8d828ff276 725
pedlerw 0:2d8d828ff276 726 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 727 //////////////////////////////move_linear//////////////////////////////////////
pedlerw 0:2d8d828ff276 728 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 729 //move_linear accepts a desired locaiton and moves the forcer accordingly//
pedlerw 0:2d8d828ff276 730 //Returns 4 on normal exit... prompting the user for a new position//
pedlerw 3:1d681d86f1dd 731 int move_linear(int desired_location)
pedlerw 3:1d681d86f1dd 732 {
pedlerw 0:2d8d828ff276 733 int current_location = get_position();
pedlerw 0:2d8d828ff276 734 int difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 735 int count = 0;
pedlerw 0:2d8d828ff276 736 xbee.printf("\r\nInside move_linear function");
pedlerw 0:2d8d828ff276 737 xbee.printf("\r\nCurrent_location %d", current_location);
pedlerw 3:1d681d86f1dd 738 if(desired_location == -1) {
pedlerw 3:1d681d86f1dd 739 return -1;
pedlerw 3:1d681d86f1dd 740 }//End of if
pedlerw 3:1d681d86f1dd 741 while(current_location != desired_location) {
pedlerw 3:1d681d86f1dd 742 difference = current_location - desired_location;
pedlerw 3:1d681d86f1dd 743 xbee.printf("\r\nDifference %d",difference);
pedlerw 3:1d681d86f1dd 744 while(difference > 0) {
pedlerw 3:1d681d86f1dd 745 if(count > four) {
pedlerw 3:1d681d86f1dd 746 for(int i=0; i<difference; i++) {
pedlerw 3:1d681d86f1dd 747 move_one_left(current_location-one);
pedlerw 3:1d681d86f1dd 748 current_location--;
pedlerw 3:1d681d86f1dd 749 }//End of for
pedlerw 3:1d681d86f1dd 750 }//End of if//
pedlerw 3:1d681d86f1dd 751 //Move LEFT//
pedlerw 0:2d8d828ff276 752 xbee.printf("\r\nMoving Left");
pedlerw 1:99f06e7e6906 753 led2 = 1;
pedlerw 1:99f06e7e6906 754 wait(medium);
pedlerw 3:1d681d86f1dd 755 for(int i=0; i<difference; i++) {
pedlerw 3:1d681d86f1dd 756 move_one_left(current_location);
pedlerw 3:1d681d86f1dd 757 current_location--;
pedlerw 3:1d681d86f1dd 758 wait(fast);
pedlerw 3:1d681d86f1dd 759 }//End of for
pedlerw 3:1d681d86f1dd 760 current_location = get_position();
pedlerw 1:99f06e7e6906 761 difference = current_location - desired_location;
pedlerw 1:99f06e7e6906 762 led2 = 0;
pedlerw 1:99f06e7e6906 763 count++;
pedlerw 3:1d681d86f1dd 764 if(current_location == -2) {
pedlerw 3:1d681d86f1dd 765 return(four);
pedlerw 3:1d681d86f1dd 766 }//End of if//
pedlerw 3:1d681d86f1dd 767 }//End of while(greater than zero)//
pedlerw 3:1d681d86f1dd 768
pedlerw 3:1d681d86f1dd 769 while(difference < 0) {
pedlerw 3:1d681d86f1dd 770 if(count > four) {
pedlerw 3:1d681d86f1dd 771 for(int i=0; i<(abs(difference)); i++) {
pedlerw 3:1d681d86f1dd 772 move_one_right(current_location+one);
pedlerw 3:1d681d86f1dd 773 current_location++;
pedlerw 3:1d681d86f1dd 774 }//End of for//
pedlerw 3:1d681d86f1dd 775 }//End of if//
pedlerw 0:2d8d828ff276 776 //Move RIGHT//
pedlerw 0:2d8d828ff276 777 xbee.printf("\r\nMoving Right");
pedlerw 3:1d681d86f1dd 778 led3 = 1;
pedlerw 1:99f06e7e6906 779 wait(medium);
pedlerw 3:1d681d86f1dd 780 for(int i=0; i<(abs(difference)); i++) {
pedlerw 3:1d681d86f1dd 781 move_one_right(current_location);
pedlerw 3:1d681d86f1dd 782 current_location++;
pedlerw 3:1d681d86f1dd 783 wait(fast);
pedlerw 3:1d681d86f1dd 784 }//End of for
pedlerw 3:1d681d86f1dd 785 current_location = get_position();
pedlerw 0:2d8d828ff276 786 difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 787 led3 = 0;
pedlerw 0:2d8d828ff276 788 count++;
pedlerw 3:1d681d86f1dd 789 if(current_location == -2) {
pedlerw 3:1d681d86f1dd 790 return(four);
pedlerw 3:1d681d86f1dd 791 }//End of if//
pedlerw 3:1d681d86f1dd 792 }//End of while(difference less than 0)//
pedlerw 3:1d681d86f1dd 793 }//End of while(current not desired)//
pedlerw 3:1d681d86f1dd 794 return(four);
pedlerw 0:2d8d828ff276 795 }//End of function//
pedlerw 0:2d8d828ff276 796
pedlerw 0:2d8d828ff276 797 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 798 ////////////////////////////error_check////////////////////////////////////////
pedlerw 0:2d8d828ff276 799 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 800 //Function does scanf error checking//
pedlerw 0:2d8d828ff276 801 //It clears the keyboard buffer incase anything other than numbers a pressed//
pedlerw 3:1d681d86f1dd 802 int error_check()
pedlerw 3:1d681d86f1dd 803 {
pedlerw 0:2d8d828ff276 804 char not_number;
pedlerw 0:2d8d828ff276 805 int number;
pedlerw 3:1d681d86f1dd 806 int flag;
pedlerw 3:1d681d86f1dd 807 while ((flag = xbee.scanf("%d", &number)) != EOF) {
pedlerw 3:1d681d86f1dd 808 if (flag != 1) {
pedlerw 3:1d681d86f1dd 809 not_number = xbee.getc();
pedlerw 0:2d8d828ff276 810 #ifdef DEBUG
pedlerw 3:1d681d86f1dd 811 xbee.printf("\r\ndegub:%c in input stream, discarding", not_number);
pedlerw 0:2d8d828ff276 812 #endif
pedlerw 3:1d681d86f1dd 813 }//End of if
pedlerw 3:1d681d86f1dd 814 else {
pedlerw 3:1d681d86f1dd 815 printf("\r\nRETURN = %d",number);
pedlerw 3:1d681d86f1dd 816 return(number);
pedlerw 3:1d681d86f1dd 817 }//End of else//
pedlerw 3:1d681d86f1dd 818 }//End of while((flag...
pedlerw 0:2d8d828ff276 819 return(number);
pedlerw 0:2d8d828ff276 820 }//End of function//
pedlerw 0:2d8d828ff276 821
pedlerw 0:2d8d828ff276 822 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 823 ////////////////////////////////display_mode///////////////////////////////////
pedlerw 0:2d8d828ff276 824 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 825 //Function set up to run display mode options//
pedlerw 3:1d681d86f1dd 826 void display_mode()
pedlerw 3:1d681d86f1dd 827 {
pedlerw 0:2d8d828ff276 828 int option = -1;
pedlerw 0:2d8d828ff276 829 int exit = 0;
pedlerw 3:1d681d86f1dd 830 //char input = 1;
pedlerw 3:1d681d86f1dd 831 int linear_auto = 3;
pedlerw 3:1d681d86f1dd 832 int linear_oscill = 3;
pedlerw 3:1d681d86f1dd 833 int combined = 3;
pedlerw 3:1d681d86f1dd 834 int high_speed = 3;
pedlerw 3:1d681d86f1dd 835 int rotary_auto = 3;
pedlerw 3:1d681d86f1dd 836 int set_up = 0;
pedlerw 3:1d681d86f1dd 837
pedlerw 3:1d681d86f1dd 838 while(exit != one) {
pedlerw 3:1d681d86f1dd 839 switch (option) {
pedlerw 3:1d681d86f1dd 840
pedlerw 3:1d681d86f1dd 841 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 842 /////////////////////////////DISPLAY OPTIONS///////////////////////////////////
pedlerw 3:1d681d86f1dd 843 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 844
pedlerw 0:2d8d828ff276 845 case 1:
pedlerw 3:1d681d86f1dd 846 //LINEAR AUTO PROGRAM//
pedlerw 3:1d681d86f1dd 847 for(int j=0; j<linear_auto; j++) {
pedlerw 3:1d681d86f1dd 848 for(int i=0; i<12; i++) {
pedlerw 4:29aafde2dcbc 849 move_all_left(abs(five-i));
pedlerw 4:29aafde2dcbc 850 move_all_right(abs(five-i));
pedlerw 0:2d8d828ff276 851 }//End of for//
pedlerw 0:2d8d828ff276 852 }//End of for//
pedlerw 3:1d681d86f1dd 853 option = -1;
pedlerw 3:1d681d86f1dd 854 break;
pedlerw 3:1d681d86f1dd 855
pedlerw 0:2d8d828ff276 856 case 2:
pedlerw 3:1d681d86f1dd 857 //LINEAR OSCILLATION//
pedlerw 3:1d681d86f1dd 858 for(int j=0; j<linear_oscill; j++) {
pedlerw 3:1d681d86f1dd 859 for(int i=0; i<12; i++) {
pedlerw 4:29aafde2dcbc 860 move_all_leftBobby(abs(five-i));
pedlerw 4:29aafde2dcbc 861 move_all_rightBobby(abs(five-i));
pedlerw 0:2d8d828ff276 862 }//End of for//
pedlerw 3:1d681d86f1dd 863 }//End of for//
pedlerw 3:1d681d86f1dd 864 option = -1;
pedlerw 3:1d681d86f1dd 865 break;
pedlerw 3:1d681d86f1dd 866
pedlerw 0:2d8d828ff276 867 case 3:
pedlerw 3:1d681d86f1dd 868 //COMBINED//
pedlerw 3:1d681d86f1dd 869 for(int i=0; i<combined; i++) {
pedlerw 3:1d681d86f1dd 870 //1.Linear//
pedlerw 3:1d681d86f1dd 871 move_all_right(five);
pedlerw 3:1d681d86f1dd 872 linear0 =! linear0; //Coil 1
pedlerw 3:1d681d86f1dd 873 wait(fast);
pedlerw 3:1d681d86f1dd 874 linear0 =! linear0;
pedlerw 3:1d681d86f1dd 875 linear6 =! linear6; //Coil 7
pedlerw 3:1d681d86f1dd 876 wait(fast);
pedlerw 3:1d681d86f1dd 877 linear6 =! linear6;
pedlerw 3:1d681d86f1dd 878 linear5 =! linear5; //Coil 6
pedlerw 3:1d681d86f1dd 879 wait(fast);
pedlerw 3:1d681d86f1dd 880 linear5 =! linear5;
pedlerw 3:1d681d86f1dd 881 linear4 =! linear4; //Coil 5
pedlerw 3:1d681d86f1dd 882 wait(fast);
pedlerw 3:1d681d86f1dd 883 linear4 =! linear4;
pedlerw 3:1d681d86f1dd 884 linear3 =! linear3; //Coil 4
pedlerw 3:1d681d86f1dd 885 wait(fast);
pedlerw 3:1d681d86f1dd 886 linear3 =! linear3;
pedlerw 3:1d681d86f1dd 887 wait(0.10);
pedlerw 3:1d681d86f1dd 888 //2.Rotate//
pedlerw 3:1d681d86f1dd 889 rotate_all_cw(eight, rot_slow);
pedlerw 3:1d681d86f1dd 890 wait(0.10);
pedlerw 3:1d681d86f1dd 891 rotate_all_ccw(eight, rot_slow);
pedlerw 3:1d681d86f1dd 892 //3.Cork Screw//
pedlerw 3:1d681d86f1dd 893 cork_screw();
pedlerw 3:1d681d86f1dd 894 wait(1.00);
pedlerw 3:1d681d86f1dd 895 //move_all_right(six);
pedlerw 3:1d681d86f1dd 896 //wait(0.10);
pedlerw 3:1d681d86f1dd 897 //4.Repeat//
pedlerw 3:1d681d86f1dd 898 }//End of for loop
pedlerw 3:1d681d86f1dd 899 option = -1;
pedlerw 3:1d681d86f1dd 900 break;
pedlerw 3:1d681d86f1dd 901
pedlerw 3:1d681d86f1dd 902 case 4:
pedlerw 3:1d681d86f1dd 903 //HIGH SPEED ROTARY//
pedlerw 3:1d681d86f1dd 904 rotary0 = 1;
pedlerw 3:1d681d86f1dd 905 rotary1 = 1;
pedlerw 3:1d681d86f1dd 906 rotary2 = 1;
pedlerw 3:1d681d86f1dd 907 rotary3 = 1;
pedlerw 3:1d681d86f1dd 908 rotary4 = 1;
pedlerw 3:1d681d86f1dd 909 rotary5 = 1;
pedlerw 3:1d681d86f1dd 910 for(int i=0; i<high_speed; i++) {
pedlerw 3:1d681d86f1dd 911 rotary_speed();
pedlerw 3:1d681d86f1dd 912 }//End of for//
pedlerw 3:1d681d86f1dd 913 option = -1;
pedlerw 3:1d681d86f1dd 914 break;
pedlerw 3:1d681d86f1dd 915 case 5:
pedlerw 3:1d681d86f1dd 916 //ROTARY AUTO PROGRAM//
pedlerw 3:1d681d86f1dd 917 for(int i=0; i<rotary_auto; i++) {
pedlerw 4:29aafde2dcbc 918 rotate_all_ccw(six, rot_fast);
pedlerw 3:1d681d86f1dd 919 rotate_all_cw(six, rot_fast);
pedlerw 3:1d681d86f1dd 920 }//End of for//
pedlerw 3:1d681d86f1dd 921 option = -1;
pedlerw 3:1d681d86f1dd 922 break;
pedlerw 3:1d681d86f1dd 923 case 6:
pedlerw 3:1d681d86f1dd 924 fast_rotary_cw();
pedlerw 3:1d681d86f1dd 925 option = -1;
pedlerw 3:1d681d86f1dd 926 break;
pedlerw 4:29aafde2dcbc 927 case 7:
pedlerw 4:29aafde2dcbc 928 //SMALL FORCER ROTARY AUTO PROGRAM//
pedlerw 4:29aafde2dcbc 929 rotary0 = 1;
pedlerw 4:29aafde2dcbc 930 rotary1 = 1;
pedlerw 4:29aafde2dcbc 931 rotary2 = 1;
pedlerw 4:29aafde2dcbc 932 rotary3 = 1;
pedlerw 4:29aafde2dcbc 933 rotary4 = 1;
pedlerw 4:29aafde2dcbc 934 rotary5 = 1;
pedlerw 4:29aafde2dcbc 935 for(int i=0; i<high_speed; i++) {
pedlerw 4:29aafde2dcbc 936 small_rotary_speed();
pedlerw 4:29aafde2dcbc 937 }//End of for//
pedlerw 4:29aafde2dcbc 938 option = -1;
pedlerw 4:29aafde2dcbc 939 break;
pedlerw 3:1d681d86f1dd 940 case 9:
pedlerw 3:1d681d86f1dd 941 //SET UP CASE//
pedlerw 3:1d681d86f1dd 942 xbee.printf("\r\n****Display Mode Set UP****");
pedlerw 3:1d681d86f1dd 943 xbee.printf("\r\nChoose a program and press <Enter>.");
pedlerw 3:1d681d86f1dd 944 xbee.printf("\r\n1. Linear Auto Program");
pedlerw 3:1d681d86f1dd 945 xbee.printf("\r\n2. Linear Oscillation");
pedlerw 3:1d681d86f1dd 946 xbee.printf("\r\n3. Combined Operation");
pedlerw 3:1d681d86f1dd 947 xbee.printf("\r\n4. High Speed Rotary");
pedlerw 3:1d681d86f1dd 948 xbee.printf("\r\n5. Rotary Auto Program");
pedlerw 3:1d681d86f1dd 949 scroll_up(22);
pedlerw 3:1d681d86f1dd 950 set_up = error_check();
pedlerw 3:1d681d86f1dd 951 switch(set_up) {
pedlerw 3:1d681d86f1dd 952 case 1:
pedlerw 3:1d681d86f1dd 953 //Edit LINEAR AUTO PROGRAM//
pedlerw 3:1d681d86f1dd 954 xbee.printf("\r\nHow many times would you like to run Linear Auto Program?");
pedlerw 4:29aafde2dcbc 955 scroll_up(27);
pedlerw 3:1d681d86f1dd 956 linear_auto = error_check();
pedlerw 3:1d681d86f1dd 957 break;
pedlerw 3:1d681d86f1dd 958 case 2:
pedlerw 3:1d681d86f1dd 959 //Edit LINEAR OSCILLATION//
pedlerw 3:1d681d86f1dd 960 xbee.printf("\r\nHow many times would you like to run Linear Oscillation?");
pedlerw 4:29aafde2dcbc 961 scroll_up(27);
pedlerw 3:1d681d86f1dd 962 linear_oscill = error_check();
pedlerw 3:1d681d86f1dd 963 break;
pedlerw 3:1d681d86f1dd 964 case 3:
pedlerw 3:1d681d86f1dd 965 //Edit COMBINED OPERATION//
pedlerw 3:1d681d86f1dd 966 xbee.printf("\r\nHow many times would you like to run Combined Operation?");
pedlerw 4:29aafde2dcbc 967 scroll_up(27);
pedlerw 3:1d681d86f1dd 968 combined = error_check();
pedlerw 3:1d681d86f1dd 969 break;
pedlerw 3:1d681d86f1dd 970 case 4:
pedlerw 3:1d681d86f1dd 971 //Edit HIGH SPEED ROTARY//
pedlerw 3:1d681d86f1dd 972 xbee.printf("\r\nHow many times would you like to run High Speed Rotary?");
pedlerw 4:29aafde2dcbc 973 scroll_up(27);
pedlerw 3:1d681d86f1dd 974 high_speed = error_check();
pedlerw 3:1d681d86f1dd 975 break;
pedlerw 3:1d681d86f1dd 976 case 5:
pedlerw 3:1d681d86f1dd 977 //Edit ROTARY AUTO PROGRAM//
pedlerw 3:1d681d86f1dd 978 xbee.printf("\r\nHow many times would you like to run Rotary Auto Program?");
pedlerw 4:29aafde2dcbc 979 scroll_up(27);
pedlerw 3:1d681d86f1dd 980 rotary_auto = error_check();
pedlerw 3:1d681d86f1dd 981 break;
pedlerw 3:1d681d86f1dd 982 }//End of switch(setup)
pedlerw 3:1d681d86f1dd 983 option = -1;
pedlerw 3:1d681d86f1dd 984 break;
pedlerw 0:2d8d828ff276 985
pedlerw 3:1d681d86f1dd 986 case 0:
pedlerw 3:1d681d86f1dd 987 //Return to the main menu//
pedlerw 3:1d681d86f1dd 988 exit = one;
pedlerw 3:1d681d86f1dd 989 break;
pedlerw 0:2d8d828ff276 990 default:
pedlerw 3:1d681d86f1dd 991 option = -1;
pedlerw 3:1d681d86f1dd 992 scroll_up(30);
pedlerw 0:2d8d828ff276 993 xbee.printf("\r\n\n****Display Mode****");
pedlerw 4:29aafde2dcbc 994 xbee.printf("\r\nTo run LINEAR AUTO PROGRAM press 1 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 995 xbee.printf("\r\nTo run LINEAR OSCILLATION press 2 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 996 xbee.printf("\r\nTo run COMBINED OPERATION press 3 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 997 xbee.printf("\r\nTo run HIGH SPEED ROTARY press 4 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 998 xbee.printf("\r\nTo run ROTARY AUTO PROGRAM press 5 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 999 xbee.printf("\r\nTo run SMALL FORCER SPEED RUN press 6 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 1000 //xbee.printf("\r\nTo run SMALL FORCER AUTO ROTARY press 7 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 1001 xbee.printf("\r\nFor SET UP press 9 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 1002 xbee.printf("\r\nTo return to the previous menu press 0 and \r\n<Enter>.");
pedlerw 4:29aafde2dcbc 1003 scroll_up(12);
pedlerw 3:1d681d86f1dd 1004 option = error_check();
pedlerw 3:1d681d86f1dd 1005 break;
pedlerw 3:1d681d86f1dd 1006 }//End of switch//
pedlerw 0:2d8d828ff276 1007 }//End of while//
pedlerw 0:2d8d828ff276 1008 return;
pedlerw 0:2d8d828ff276 1009 }//End of function//
pedlerw 0:2d8d828ff276 1010
pedlerw 0:2d8d828ff276 1011 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1012 ////////////////////////////move_all_leftBobby/////////////////////////////////
pedlerw 0:2d8d828ff276 1013 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1014 //This function accepts an interger value and moves the forcer to the left//
pedlerw 0:2d8d828ff276 1015 //The interger number of coil sets//
pedlerw 3:1d681d86f1dd 1016 void move_all_leftBobby(int set)
pedlerw 3:1d681d86f1dd 1017 {
pedlerw 0:2d8d828ff276 1018 //Declartions and initialization//
pedlerw 3:1d681d86f1dd 1019 linear0 = 1; //Coil1 Mod0
pedlerw 3:1d681d86f1dd 1020 linear1 = 1; //Coil2 Mod1
pedlerw 3:1d681d86f1dd 1021 linear2 = 1; //Coil3 Mod2
pedlerw 3:1d681d86f1dd 1022 linear3 = 1; //Coil4 Mod3
pedlerw 3:1d681d86f1dd 1023 linear4 = 1; //Coil5 Mod4
pedlerw 3:1d681d86f1dd 1024 linear5 = 1; //Coil6 Mod5
pedlerw 3:1d681d86f1dd 1025 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 1026
pedlerw 3:1d681d86f1dd 1027 //LEFT//
pedlerw 3:1d681d86f1dd 1028 for(int i=0; i<set; i++) {
pedlerw 0:2d8d828ff276 1029 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 1030 wait(fast);
pedlerw 0:2d8d828ff276 1031 linear4 =! linear4;
pedlerw 0:2d8d828ff276 1032 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 1033 wait(fast);
pedlerw 3:1d681d86f1dd 1034 linear5 =! linear5;
pedlerw 0:2d8d828ff276 1035 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 1036 wait(fast);
pedlerw 3:1d681d86f1dd 1037 linear6 =! linear6;
pedlerw 0:2d8d828ff276 1038 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 1039 wait(fast);
pedlerw 3:1d681d86f1dd 1040 linear0 =! linear0;
pedlerw 0:2d8d828ff276 1041 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 1042 wait(fast);
pedlerw 3:1d681d86f1dd 1043 linear1 =! linear1;
pedlerw 3:1d681d86f1dd 1044 linear2 =! linear2; //Coil 3
pedlerw 3:1d681d86f1dd 1045 wait(fast);
pedlerw 3:1d681d86f1dd 1046 linear2 =! linear2;
pedlerw 3:1d681d86f1dd 1047 linear3 =! linear3; //Coil 4
pedlerw 3:1d681d86f1dd 1048 wait(fast);
pedlerw 3:1d681d86f1dd 1049 linear3 =! linear3;
pedlerw 3:1d681d86f1dd 1050 }//End of For loop
pedlerw 3:1d681d86f1dd 1051
pedlerw 3:1d681d86f1dd 1052 linear4 =! linear4; //Coil 5
pedlerw 3:1d681d86f1dd 1053 wait(fast);
pedlerw 3:1d681d86f1dd 1054 linear4 =! linear4;
pedlerw 3:1d681d86f1dd 1055 linear5 =! linear5; //Coil 6
pedlerw 3:1d681d86f1dd 1056 wait(fast);
pedlerw 3:1d681d86f1dd 1057 linear5 =! linear5;
pedlerw 3:1d681d86f1dd 1058 linear6 =! linear6; //Coil 7
pedlerw 3:1d681d86f1dd 1059 wait(fast);
pedlerw 3:1d681d86f1dd 1060 linear6 =! linear6;
pedlerw 3:1d681d86f1dd 1061 linear0 =! linear0; //Coil 1
pedlerw 3:1d681d86f1dd 1062 wait(fast);
pedlerw 3:1d681d86f1dd 1063 linear0 =! linear0;
pedlerw 4:29aafde2dcbc 1064 //linear1 =! linear1; //Coil 2
pedlerw 4:29aafde2dcbc 1065 //wait(fast);
pedlerw 4:29aafde2dcbc 1066 //linear1 =! linear1;
pedlerw 0:2d8d828ff276 1067
pedlerw 0:2d8d828ff276 1068 return;
pedlerw 0:2d8d828ff276 1069 }//End of Funciton
pedlerw 0:2d8d828ff276 1070
pedlerw 0:2d8d828ff276 1071 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1072 ////////////////////////////move_all_rightBobby////////////////////////////////
pedlerw 0:2d8d828ff276 1073 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1074 //This function accepts an interger value and moves the forcer to the right//
pedlerw 0:2d8d828ff276 1075 //The interger number of coil sets//
pedlerw 3:1d681d86f1dd 1076 void move_all_rightBobby(int set)
pedlerw 3:1d681d86f1dd 1077 {
pedlerw 0:2d8d828ff276 1078 //Declartions and initialization//
pedlerw 3:1d681d86f1dd 1079 linear0 = 1; //Coil1 Mod0
pedlerw 3:1d681d86f1dd 1080 linear1 = 1; //Coil2 Mod1
pedlerw 3:1d681d86f1dd 1081 linear2 = 1; //Coil3 Mod2
pedlerw 3:1d681d86f1dd 1082 linear3 = 1; //Coil4 Mod3
pedlerw 3:1d681d86f1dd 1083 linear4 = 1; //Coil5 Mod4
pedlerw 3:1d681d86f1dd 1084 linear5 = 1; //Coil6 Mod5
pedlerw 3:1d681d86f1dd 1085 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 1086
pedlerw 3:1d681d86f1dd 1087 //RIGHT//
pedlerw 3:1d681d86f1dd 1088 for(int i=0; i<set; i++) {
pedlerw 0:2d8d828ff276 1089 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 1090 wait(fast);
pedlerw 0:2d8d828ff276 1091 linear0 =! linear0;
pedlerw 0:2d8d828ff276 1092 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 1093 wait(fast);
pedlerw 3:1d681d86f1dd 1094 linear6 =! linear6;
pedlerw 0:2d8d828ff276 1095 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 1096 wait(fast);
pedlerw 3:1d681d86f1dd 1097 linear5 =! linear5;
pedlerw 0:2d8d828ff276 1098 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 1099 wait(fast);
pedlerw 3:1d681d86f1dd 1100 linear4 =! linear4;
pedlerw 3:1d681d86f1dd 1101 linear3 =! linear3; //Coil 4
pedlerw 3:1d681d86f1dd 1102 wait(fast);
pedlerw 3:1d681d86f1dd 1103 linear3 =! linear3;
pedlerw 3:1d681d86f1dd 1104 linear2 =! linear2; //Coil 3
pedlerw 3:1d681d86f1dd 1105 wait(fast);
pedlerw 3:1d681d86f1dd 1106 linear2 =! linear2;
pedlerw 3:1d681d86f1dd 1107 linear1 =! linear1; //Coil 2
pedlerw 3:1d681d86f1dd 1108 wait(fast);
pedlerw 3:1d681d86f1dd 1109 linear1 =! linear1;
pedlerw 3:1d681d86f1dd 1110 }//End of For loop
pedlerw 3:1d681d86f1dd 1111
pedlerw 3:1d681d86f1dd 1112 linear0 =! linear0; //Coil 1
pedlerw 3:1d681d86f1dd 1113 wait(fast);
pedlerw 3:1d681d86f1dd 1114 linear0 =! linear0;
pedlerw 3:1d681d86f1dd 1115 linear6 =! linear6; //Coil 7
pedlerw 3:1d681d86f1dd 1116 wait(fast);
pedlerw 3:1d681d86f1dd 1117 linear6 =! linear6;
pedlerw 3:1d681d86f1dd 1118 linear5 =! linear5; //Coil 6
pedlerw 3:1d681d86f1dd 1119 wait(fast);
pedlerw 3:1d681d86f1dd 1120 linear5 =! linear5;
pedlerw 3:1d681d86f1dd 1121 linear4 =! linear4; //Coil 5
pedlerw 3:1d681d86f1dd 1122 wait(fast);
pedlerw 3:1d681d86f1dd 1123 linear4 =! linear4;
pedlerw 4:29aafde2dcbc 1124 //linear3 =! linear3; //Coil 4
pedlerw 4:29aafde2dcbc 1125 //wait(fast);
pedlerw 4:29aafde2dcbc 1126 //linear3 =! linear3;
pedlerw 4:29aafde2dcbc 1127
pedlerw 3:1d681d86f1dd 1128
pedlerw 0:2d8d828ff276 1129 return;
pedlerw 0:2d8d828ff276 1130 }//End of Funciton
pedlerw 0:2d8d828ff276 1131
pedlerw 0:2d8d828ff276 1132 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1133 //////////////////////////////scroll_up()//////////////////////////////////////
pedlerw 0:2d8d828ff276 1134 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 1135 //Function accepts an integer and scrolls the text on the serial terminal that
pedlerw 0:2d8d828ff276 1136 //Number of spaces//
pedlerw 3:1d681d86f1dd 1137 void scroll_up(int space)
pedlerw 3:1d681d86f1dd 1138 {
pedlerw 3:1d681d86f1dd 1139 for(int i=0; i<space; i++) {
pedlerw 0:2d8d828ff276 1140 xbee.printf("\r\n");
pedlerw 3:1d681d86f1dd 1141 }//End of for
pedlerw 3:1d681d86f1dd 1142 }//End of function
pedlerw 3:1d681d86f1dd 1143
pedlerw 3:1d681d86f1dd 1144 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1145 ///////////////////////////////cork_screw()////////////////////////////////////
pedlerw 3:1d681d86f1dd 1146 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1147 //Function moves linear and rotary at the same time... from right to left//////
pedlerw 3:1d681d86f1dd 1148 void cork_screw()
pedlerw 3:1d681d86f1dd 1149 {
pedlerw 3:1d681d86f1dd 1150 rotary0 = 1;
pedlerw 3:1d681d86f1dd 1151 rotary1 = 1;
pedlerw 3:1d681d86f1dd 1152 rotary2 = 1;
pedlerw 3:1d681d86f1dd 1153 rotary3 = 1;
pedlerw 3:1d681d86f1dd 1154 rotary4 = 1;
pedlerw 3:1d681d86f1dd 1155 rotary5 = 1;
pedlerw 3:1d681d86f1dd 1156 int length = track_length;
pedlerw 3:1d681d86f1dd 1157 //for(int i=0; i<track_length; i++) {
pedlerw 3:1d681d86f1dd 1158 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1159 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1160 wait(fast);
pedlerw 3:1d681d86f1dd 1161 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1162 wait(fast);
pedlerw 3:1d681d86f1dd 1163 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1164 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1165 wait(fast);
pedlerw 3:1d681d86f1dd 1166 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1167 wait(fast);
pedlerw 3:1d681d86f1dd 1168 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1169 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1170 wait(fast);
pedlerw 3:1d681d86f1dd 1171 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1172 wait(fast);
pedlerw 3:1d681d86f1dd 1173 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1174 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1175 wait(fast);
pedlerw 3:1d681d86f1dd 1176 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1177 wait(fast);
pedlerw 3:1d681d86f1dd 1178 rotary4 = !rotary4;
pedlerw 3:1d681d86f1dd 1179 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1180 wait(fast);
pedlerw 3:1d681d86f1dd 1181 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1182 wait(fast);
pedlerw 3:1d681d86f1dd 1183 rotary5 = !rotary5;
pedlerw 3:1d681d86f1dd 1184 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1185 wait(fast);
pedlerw 3:1d681d86f1dd 1186 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1187 wait(fast);
pedlerw 3:1d681d86f1dd 1188 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1189 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1190 wait(fast);
pedlerw 3:1d681d86f1dd 1191 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1192 wait(fast);
pedlerw 3:1d681d86f1dd 1193 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1194 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1195 wait(fast);
pedlerw 3:1d681d86f1dd 1196 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1197 wait(fast);
pedlerw 3:1d681d86f1dd 1198 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1199 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1200 wait(fast);
pedlerw 3:1d681d86f1dd 1201 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1202 wait(fast);
pedlerw 3:1d681d86f1dd 1203 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1204 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1205 wait(fast);
pedlerw 3:1d681d86f1dd 1206 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1207 wait(fast);
pedlerw 3:1d681d86f1dd 1208 rotary4 = !rotary4;
pedlerw 3:1d681d86f1dd 1209 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1210 wait(fast);
pedlerw 3:1d681d86f1dd 1211 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1212 wait(fast);
pedlerw 3:1d681d86f1dd 1213 rotary5 = !rotary5;
pedlerw 3:1d681d86f1dd 1214 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1215 wait(fast);
pedlerw 3:1d681d86f1dd 1216 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1217 wait(fast);
pedlerw 3:1d681d86f1dd 1218 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1219 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1220 wait(fast);
pedlerw 3:1d681d86f1dd 1221 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1222 wait(fast);
pedlerw 3:1d681d86f1dd 1223 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1224 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1225 wait(fast);
pedlerw 3:1d681d86f1dd 1226 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1227 wait(fast);
pedlerw 3:1d681d86f1dd 1228 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1229 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1230 wait(fast);
pedlerw 3:1d681d86f1dd 1231 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1232 wait(fast);
pedlerw 3:1d681d86f1dd 1233 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1234 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1235 wait(fast);
pedlerw 3:1d681d86f1dd 1236 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1237 wait(fast);
pedlerw 3:1d681d86f1dd 1238 rotary4 = !rotary4;
pedlerw 3:1d681d86f1dd 1239 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1240 wait(fast);
pedlerw 3:1d681d86f1dd 1241 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1242 wait(fast);
pedlerw 3:1d681d86f1dd 1243 rotary5 = !rotary5;
pedlerw 3:1d681d86f1dd 1244 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1245 wait(fast);
pedlerw 3:1d681d86f1dd 1246 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1247 wait(fast);
pedlerw 3:1d681d86f1dd 1248 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1249 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1250 wait(fast);
pedlerw 3:1d681d86f1dd 1251 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1252 wait(fast);
pedlerw 3:1d681d86f1dd 1253 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1254 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1255 wait(fast);
pedlerw 3:1d681d86f1dd 1256 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1257 wait(fast);
pedlerw 3:1d681d86f1dd 1258 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1259 move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1260 wait(fast);
pedlerw 3:1d681d86f1dd 1261 /*move_one_left(abs(length--));
pedlerw 3:1d681d86f1dd 1262 wait(fast);
pedlerw 3:1d681d86f1dd 1263 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1264 move_one_left(abs(length--)); // */
pedlerw 3:1d681d86f1dd 1265 //}//End of for//
pedlerw 3:1d681d86f1dd 1266 }//End of function//
pedlerw 3:1d681d86f1dd 1267
pedlerw 3:1d681d86f1dd 1268 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1269 /////////////////////////rotate_all_cw(int )///////////////////////////////////
pedlerw 3:1d681d86f1dd 1270 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1271 void rotate_all_cw(int set, float speed)
pedlerw 3:1d681d86f1dd 1272 {
pedlerw 4:29aafde2dcbc 1273 rotary0 = 1;
pedlerw 4:29aafde2dcbc 1274 rotary1 = 1;
pedlerw 4:29aafde2dcbc 1275 rotary2 = 1;
pedlerw 4:29aafde2dcbc 1276 rotary3 = 1;
pedlerw 4:29aafde2dcbc 1277 rotary4 = 1;
pedlerw 4:29aafde2dcbc 1278 rotary5 = 1;
pedlerw 3:1d681d86f1dd 1279 for(int i=0; i<set; i++) {
pedlerw 3:1d681d86f1dd 1280 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1281 wait(speed);
pedlerw 3:1d681d86f1dd 1282 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1283 wait(speed);
pedlerw 3:1d681d86f1dd 1284 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1285 wait(speed);
pedlerw 3:1d681d86f1dd 1286 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1287 wait(speed);
pedlerw 3:1d681d86f1dd 1288 rotary4 = !rotary4;
pedlerw 3:1d681d86f1dd 1289 wait(speed);
pedlerw 3:1d681d86f1dd 1290 rotary5 = !rotary5;
pedlerw 3:1d681d86f1dd 1291 wait(speed);
pedlerw 3:1d681d86f1dd 1292 }//End of for
pedlerw 3:1d681d86f1dd 1293 }//End of function//
pedlerw 3:1d681d86f1dd 1294
pedlerw 3:1d681d86f1dd 1295 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1296 ///////////////////////////rotate_all_ccw(int s)///////////////////////////////
pedlerw 3:1d681d86f1dd 1297 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1298 //Function rotate the forcer
pedlerw 3:1d681d86f1dd 1299 void rotate_all_ccw(int set, float speed)
pedlerw 3:1d681d86f1dd 1300 {
pedlerw 4:29aafde2dcbc 1301 rotary0 = 1;
pedlerw 4:29aafde2dcbc 1302 rotary1 = 1;
pedlerw 4:29aafde2dcbc 1303 rotary2 = 1;
pedlerw 4:29aafde2dcbc 1304 rotary3 = 1;
pedlerw 4:29aafde2dcbc 1305 rotary4 = 1;
pedlerw 4:29aafde2dcbc 1306 rotary5 = 1;
pedlerw 3:1d681d86f1dd 1307 for(int i=0; i<set; i++) {
pedlerw 3:1d681d86f1dd 1308 rotary5 = !rotary5;
pedlerw 3:1d681d86f1dd 1309 wait(speed);
pedlerw 3:1d681d86f1dd 1310 rotary4 = !rotary4;
pedlerw 3:1d681d86f1dd 1311 wait(speed);
pedlerw 3:1d681d86f1dd 1312 rotary3 = !rotary3;
pedlerw 3:1d681d86f1dd 1313 wait(speed);
pedlerw 3:1d681d86f1dd 1314 rotary2 = !rotary2;
pedlerw 3:1d681d86f1dd 1315 wait(speed);
pedlerw 3:1d681d86f1dd 1316 rotary1 = !rotary1;
pedlerw 3:1d681d86f1dd 1317 wait(speed);
pedlerw 3:1d681d86f1dd 1318 rotary0 = !rotary0;
pedlerw 3:1d681d86f1dd 1319 wait(speed);
pedlerw 3:1d681d86f1dd 1320 }//End of for
pedlerw 3:1d681d86f1dd 1321 }//End of function//
pedlerw 3:1d681d86f1dd 1322
pedlerw 3:1d681d86f1dd 1323 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1324 //////////////////////////////////////rotary_speed();//////////////////////////
pedlerw 3:1d681d86f1dd 1325 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1326 //rotary_speed() rotates the forcer faster and faster//
pedlerw 3:1d681d86f1dd 1327 void rotary_speed()
pedlerw 3:1d681d86f1dd 1328 {
pedlerw 3:1d681d86f1dd 1329 float time = 0.160;
pedlerw 3:1d681d86f1dd 1330 float step = 0.010;
pedlerw 3:1d681d86f1dd 1331 int a = 0;
pedlerw 3:1d681d86f1dd 1332 int cycle = 16;
pedlerw 3:1d681d86f1dd 1333 int change = 0;
pedlerw 3:1d681d86f1dd 1334 int turns = 4;
pedlerw 3:1d681d86f1dd 1335 int maxint = .08;
pedlerw 3:1d681d86f1dd 1336
pedlerw 3:1d681d86f1dd 1337 while(a<2) {
pedlerw 3:1d681d86f1dd 1338 for(int i=cycle; i>0; i--) {
pedlerw 3:1d681d86f1dd 1339 if(time == maxint) {
pedlerw 3:1d681d86f1dd 1340 turns = 80;
pedlerw 3:1d681d86f1dd 1341 } else if(time != maxint) {
pedlerw 3:1d681d86f1dd 1342 turns = 4;
pedlerw 3:1d681d86f1dd 1343 }
pedlerw 3:1d681d86f1dd 1344 if(change == 0) {
pedlerw 3:1d681d86f1dd 1345 if(i > cycle/2) {
pedlerw 3:1d681d86f1dd 1346 rotate_all_cw(turns,time);
pedlerw 3:1d681d86f1dd 1347 time = time - step;
pedlerw 3:1d681d86f1dd 1348 }//End of if
pedlerw 3:1d681d86f1dd 1349 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 3:1d681d86f1dd 1350 rotate_all_cw(turns,time);
pedlerw 3:1d681d86f1dd 1351 time = time + step;
pedlerw 3:1d681d86f1dd 1352 }//End of if
pedlerw 3:1d681d86f1dd 1353 } else if(change==1) {
pedlerw 3:1d681d86f1dd 1354 if(i > cycle/2) {
pedlerw 3:1d681d86f1dd 1355
pedlerw 3:1d681d86f1dd 1356 rotate_all_ccw(turns,time);
pedlerw 3:1d681d86f1dd 1357 time = time - step;
pedlerw 3:1d681d86f1dd 1358 }//End of if
pedlerw 3:1d681d86f1dd 1359 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 3:1d681d86f1dd 1360 rotate_all_ccw(turns,time);
pedlerw 3:1d681d86f1dd 1361 time = time + step;
pedlerw 3:1d681d86f1dd 1362 }//End of if
pedlerw 3:1d681d86f1dd 1363 }//End of else if//
pedlerw 3:1d681d86f1dd 1364 }//End of for//
pedlerw 3:1d681d86f1dd 1365 change = !change;
pedlerw 3:1d681d86f1dd 1366 a++;
pedlerw 3:1d681d86f1dd 1367 }//End of while
pedlerw 3:1d681d86f1dd 1368 }//End of function
pedlerw 3:1d681d86f1dd 1369
pedlerw 3:1d681d86f1dd 1370 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1371 ///////////////////////////fast_rotary_cw()////////////////////////////////////
pedlerw 3:1d681d86f1dd 1372 ///////////////////////////////////////////////////////////////////////////////
pedlerw 3:1d681d86f1dd 1373 //Special function for little forcer
pedlerw 3:1d681d86f1dd 1374 void fast_rotary_cw(){
pedlerw 3:1d681d86f1dd 1375 rotate_all_cw(400, 0.01);
pedlerw 4:29aafde2dcbc 1376 }//End of Function//
pedlerw 4:29aafde2dcbc 1377
pedlerw 4:29aafde2dcbc 1378 ///////////////////////////////////////////////////////////////////////////////
pedlerw 4:29aafde2dcbc 1379 ////////////////////////////////small_rotary_speed();//////////////////////////
pedlerw 4:29aafde2dcbc 1380 ///////////////////////////////////////////////////////////////////////////////
pedlerw 4:29aafde2dcbc 1381 //small_rotary_speed() rotates the small forcer faster and faster//
pedlerw 4:29aafde2dcbc 1382 void small_rotary_speed()
pedlerw 4:29aafde2dcbc 1383 {
pedlerw 4:29aafde2dcbc 1384 float time = 0.080;
pedlerw 4:29aafde2dcbc 1385 float step = 0.005;
pedlerw 4:29aafde2dcbc 1386 int a = 0;
pedlerw 4:29aafde2dcbc 1387 int cycle = 32;
pedlerw 4:29aafde2dcbc 1388 int change = 0;
pedlerw 4:29aafde2dcbc 1389 int turns = 8;
pedlerw 4:29aafde2dcbc 1390 int maxint = 0.010;
pedlerw 4:29aafde2dcbc 1391
pedlerw 4:29aafde2dcbc 1392 while(a<2) {
pedlerw 4:29aafde2dcbc 1393 for(int i=cycle; i>0; i--) {
pedlerw 4:29aafde2dcbc 1394 if(time == maxint) {
pedlerw 4:29aafde2dcbc 1395 turns = 400;
pedlerw 4:29aafde2dcbc 1396 } else if(time != maxint) {
pedlerw 4:29aafde2dcbc 1397 turns = 8;
pedlerw 4:29aafde2dcbc 1398 }
pedlerw 4:29aafde2dcbc 1399 if(change == 0) {
pedlerw 4:29aafde2dcbc 1400 if(i > cycle/2) {
pedlerw 4:29aafde2dcbc 1401 rotate_all_cw(turns,time);
pedlerw 4:29aafde2dcbc 1402 time = time - step;
pedlerw 4:29aafde2dcbc 1403 }//End of if
pedlerw 4:29aafde2dcbc 1404 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 4:29aafde2dcbc 1405 rotate_all_cw(turns,time);
pedlerw 4:29aafde2dcbc 1406 time = time + step;
pedlerw 4:29aafde2dcbc 1407 }//End of if
pedlerw 4:29aafde2dcbc 1408 } else if(change==1) {
pedlerw 4:29aafde2dcbc 1409 if(i > cycle/2) {
pedlerw 4:29aafde2dcbc 1410 rotate_all_ccw(turns,time);
pedlerw 4:29aafde2dcbc 1411 time = time - step;
pedlerw 4:29aafde2dcbc 1412 }//End of if
pedlerw 4:29aafde2dcbc 1413 if(i < ((cycle/2) + 1 ) && i > 0) {
pedlerw 4:29aafde2dcbc 1414 rotate_all_ccw(turns,time);
pedlerw 4:29aafde2dcbc 1415 time = time + step;
pedlerw 4:29aafde2dcbc 1416 }//End of if
pedlerw 4:29aafde2dcbc 1417 }//End of else if//
pedlerw 4:29aafde2dcbc 1418 }//End of for//
pedlerw 4:29aafde2dcbc 1419 change = !change;
pedlerw 4:29aafde2dcbc 1420 a++;
pedlerw 4:29aafde2dcbc 1421 }//End of while
pedlerw 4:29aafde2dcbc 1422 }//End of function