William Pedler
2013 VCU Senior Design Expo Linear Rotary Motor Control Program. This is the actual program used by the team at the Expo.
main.cpp@1:99f06e7e6906, 2013-04-16 (annotated)
- Committer:
- pedlerw
- Date:
- Tue Apr 16 19:50:19 2013 +0000
- Revision:
- 1:99f06e7e6906
- Parent:
- 0:2d8d828ff276
- Child:
- 2:e46a0432d96b
4/16/2013; William Pedler; Nearly the final program.
Who changed what in which revision?
| User | Revision | Line number | New 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 | 0:2d8d828ff276 | 29 | |
| pedlerw | 0:2d8d828ff276 | 30 | //Declar global variables// |
| pedlerw | 0:2d8d828ff276 | 31 | float left_min[track_length] = {0}; |
| pedlerw | 0:2d8d828ff276 | 32 | float left_max[track_length] = {0}; |
| pedlerw | 0:2d8d828ff276 | 33 | float right_min[track_length] = {0}; |
| pedlerw | 0:2d8d828ff276 | 34 | float right_max[track_length] = {0}; |
| pedlerw | 0:2d8d828ff276 | 35 | |
| pedlerw | 0:2d8d828ff276 | 36 | //Labeling the Local file system// |
| pedlerw | 0:2d8d828ff276 | 37 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
| pedlerw | 0:2d8d828ff276 | 38 | //Labeling onboard LED's// |
| pedlerw | 0:2d8d828ff276 | 39 | DigitalOut led1(LED1); |
| pedlerw | 0:2d8d828ff276 | 40 | DigitalOut led2(LED2); |
| pedlerw | 0:2d8d828ff276 | 41 | DigitalOut led3(LED3); |
| pedlerw | 0:2d8d828ff276 | 42 | DigitalOut led4(LED4); |
| pedlerw | 0:2d8d828ff276 | 43 | //Label linear coils// |
| pedlerw | 0:2d8d828ff276 | 44 | DigitalOut linear0(p5); //coil 1 |
| pedlerw | 0:2d8d828ff276 | 45 | DigitalOut linear1(p6); //coil 2 |
| pedlerw | 0:2d8d828ff276 | 46 | DigitalOut linear2(p7); //coil 3 |
| pedlerw | 0:2d8d828ff276 | 47 | DigitalOut linear3(p8); //coil 4 |
| pedlerw | 0:2d8d828ff276 | 48 | DigitalOut linear4(p9); //coil 5 |
| pedlerw | 0:2d8d828ff276 | 49 | DigitalOut linear5(p10); //coil 6 |
| pedlerw | 0:2d8d828ff276 | 50 | DigitalOut linear6(p11); //coil 7 |
| pedlerw | 0:2d8d828ff276 | 51 | //Reduce noise by declaring all unused Analog pins as DigitalOut// |
| pedlerw | 0:2d8d828ff276 | 52 | DigitalOut left15(p15); |
| pedlerw | 0:2d8d828ff276 | 53 | DigitalOut left16(p16); |
| pedlerw | 0:2d8d828ff276 | 54 | DigitalOut left17(p17); |
| pedlerw | 0:2d8d828ff276 | 55 | DigitalOut left18(p18); |
| pedlerw | 0:2d8d828ff276 | 56 | AnalogIn distanceR(p19); //Vout yellow GND black Vss red |
| pedlerw | 0:2d8d828ff276 | 57 | AnalogIn distanceL(p20); //Vout yellow GND black Vss red |
| pedlerw | 0:2d8d828ff276 | 58 | //Set serial com// |
| pedlerw | 0:2d8d828ff276 | 59 | Serial xbee(p13,p14); |
| pedlerw | 0:2d8d828ff276 | 60 | |
| pedlerw | 0:2d8d828ff276 | 61 | //Function Prototypes// |
| pedlerw | 0:2d8d828ff276 | 62 | void move_all_left(int num); //num for # of coil sets left// |
| pedlerw | 0:2d8d828ff276 | 63 | void move_all_right(int num); //num for # of coil sets right// |
| pedlerw | 0:2d8d828ff276 | 64 | float get_Lsensor_min(); |
| pedlerw | 0:2d8d828ff276 | 65 | float get_Lsensor_max(); |
| pedlerw | 0:2d8d828ff276 | 66 | float get_Rsensor_min(); |
| pedlerw | 0:2d8d828ff276 | 67 | float get_Rsensor_max(); |
| pedlerw | 0:2d8d828ff276 | 68 | void move_one_right(int p); //p for position// |
| pedlerw | 0:2d8d828ff276 | 69 | void move_one_left(int p); //p for position// |
| pedlerw | 0:2d8d828ff276 | 70 | int get_position(); |
| pedlerw | 0:2d8d828ff276 | 71 | void calibrate_linear(); |
| pedlerw | 0:2d8d828ff276 | 72 | void read_file(); |
| pedlerw | 0:2d8d828ff276 | 73 | int move_linear(int l); //l for location// |
| pedlerw | 0:2d8d828ff276 | 74 | int error_check(); |
| pedlerw | 0:2d8d828ff276 | 75 | void display_mode(); |
| pedlerw | 0:2d8d828ff276 | 76 | void move_all_leftBobby(int num); |
| pedlerw | 0:2d8d828ff276 | 77 | void move_all_rightBobby(int num); |
| pedlerw | 1:99f06e7e6906 | 78 | void scroll_up(int line); //line for number lines to scroll// |
| pedlerw | 0:2d8d828ff276 | 79 | |
| pedlerw | 0:2d8d828ff276 | 80 | |
| pedlerw | 0:2d8d828ff276 | 81 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 82 | /////////////////////////////Main Program////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 83 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 84 | |
| pedlerw | 0:2d8d828ff276 | 85 | /*//////////////////////Pseudo code for Main:////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 86 | -Prompt user with 2 options- |
| pedlerw | 0:2d8d828ff276 | 87 | 1.Calibrate Motor |
| pedlerw | 0:2d8d828ff276 | 88 | a.Call calibration function... write new data to local files |
| pedlerw | 0:2d8d828ff276 | 89 | 2.Use old calibration file |
| pedlerw | 0:2d8d828ff276 | 90 | a.Read in files |
| pedlerw | 0:2d8d828ff276 | 91 | b.Prompt for desired location |
| pedlerw | 0:2d8d828ff276 | 92 | -Operate Motor |
| pedlerw | 0:2d8d828ff276 | 93 | 1. Get current location |
| pedlerw | 0:2d8d828ff276 | 94 | 2. Get desired location |
| pedlerw | 0:2d8d828ff276 | 95 | 3. Move to location |
| pedlerw | 0:2d8d828ff276 | 96 | 4. Repeat |
| pedlerw | 0:2d8d828ff276 | 97 | *////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 98 | //////////////////////////////////MAIN///////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 99 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 100 | int main() { |
| pedlerw | 0:2d8d828ff276 | 101 | //Declarations & Initializations// |
| pedlerw | 0:2d8d828ff276 | 102 | int desired_location = 0; |
| pedlerw | 0:2d8d828ff276 | 103 | int user_instruction = -1; |
| pedlerw | 0:2d8d828ff276 | 104 | |
| pedlerw | 0:2d8d828ff276 | 105 | //Initialize Linear Coils// |
| pedlerw | 0:2d8d828ff276 | 106 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 107 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 108 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 109 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 110 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 111 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 112 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 113 | //Initialize onbord led// |
| pedlerw | 0:2d8d828ff276 | 114 | led1 = 0; |
| pedlerw | 0:2d8d828ff276 | 115 | led2 = 0; |
| pedlerw | 0:2d8d828ff276 | 116 | led3 = 0; |
| pedlerw | 0:2d8d828ff276 | 117 | led4 = 0; |
| pedlerw | 0:2d8d828ff276 | 118 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 119 | ///////////////////////////New User Interface////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 120 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 121 | |
| pedlerw | 0:2d8d828ff276 | 122 | while(1){ |
| pedlerw | 0:2d8d828ff276 | 123 | //-Prompt user with 2 options- |
| pedlerw | 0:2d8d828ff276 | 124 | switch(user_instruction){ |
| pedlerw | 0:2d8d828ff276 | 125 | case 1: |
| pedlerw | 0:2d8d828ff276 | 126 | //Calibrate Motor// |
| pedlerw | 0:2d8d828ff276 | 127 | calibrate_linear(); |
| pedlerw | 0:2d8d828ff276 | 128 | //Go to operate motor// |
| pedlerw | 0:2d8d828ff276 | 129 | user_instruction = 4; //Go to operation menu// |
| pedlerw | 0:2d8d828ff276 | 130 | break; |
| pedlerw | 0:2d8d828ff276 | 131 | case 2: |
| pedlerw | 0:2d8d828ff276 | 132 | //Use old calibration file and begin operation |
| pedlerw | 0:2d8d828ff276 | 133 | //Read file to fill in max and min// |
| pedlerw | 0:2d8d828ff276 | 134 | read_file(); |
| pedlerw | 0:2d8d828ff276 | 135 | user_instruction = 4; //Go to operation menu// |
| pedlerw | 0:2d8d828ff276 | 136 | break; |
| pedlerw | 0:2d8d828ff276 | 137 | case 3: |
| pedlerw | 0:2d8d828ff276 | 138 | display_mode(); |
| pedlerw | 0:2d8d828ff276 | 139 | user_instruction = -1; //Go to main menu// |
| pedlerw | 0:2d8d828ff276 | 140 | break; |
| pedlerw | 0:2d8d828ff276 | 141 | case 4: |
| pedlerw | 0:2d8d828ff276 | 142 | //Operate Linear Part// |
| pedlerw | 0:2d8d828ff276 | 143 | for(int i=0;i<30;i++){ |
| pedlerw | 0:2d8d828ff276 | 144 | xbee.printf("\r\n"); |
| pedlerw | 0:2d8d828ff276 | 145 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 146 | scroll_up(30); |
| pedlerw | 0:2d8d828ff276 | 147 | xbee.printf("\r\n\n****Operation Menu****"); |
| pedlerw | 0:2d8d828ff276 | 148 | xbee.printf("\r\nEnter a position number from 0 to 40: "); |
| pedlerw | 0:2d8d828ff276 | 149 | xbee.printf("\r\nTo get back to the Main Menu press -1 and <Enter>."); |
| pedlerw | 0:2d8d828ff276 | 150 | scroll_up(25); |
| pedlerw | 0:2d8d828ff276 | 151 | desired_location = error_check(); |
| pedlerw | 0:2d8d828ff276 | 152 | if(desired_location == -1){ |
| pedlerw | 0:2d8d828ff276 | 153 | user_instruction = move_linear(desired_location); |
| pedlerw | 0:2d8d828ff276 | 154 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 155 | else if(desired_location > 40){ |
| pedlerw | 0:2d8d828ff276 | 156 | user_instruction = 4; |
| pedlerw | 0:2d8d828ff276 | 157 | xbee.printf("\r\nUser input was out of bounds"); |
| pedlerw | 0:2d8d828ff276 | 158 | xbee.printf("\r\nPlease try again"); |
| pedlerw | 0:2d8d828ff276 | 159 | scroll_up(20); |
| pedlerw | 0:2d8d828ff276 | 160 | wait(slow); |
| pedlerw | 0:2d8d828ff276 | 161 | break; |
| pedlerw | 0:2d8d828ff276 | 162 | }//End of else if |
| pedlerw | 0:2d8d828ff276 | 163 | else if(desired_location < 0){ |
| pedlerw | 0:2d8d828ff276 | 164 | user_instruction = 4; |
| pedlerw | 0:2d8d828ff276 | 165 | xbee.printf("\r\nUser input was out of bounds"); |
| pedlerw | 0:2d8d828ff276 | 166 | xbee.printf("\r\nPlease try again"); |
| pedlerw | 0:2d8d828ff276 | 167 | scroll_up(20); |
| pedlerw | 0:2d8d828ff276 | 168 | wait(slow); |
| pedlerw | 0:2d8d828ff276 | 169 | break; |
| pedlerw | 0:2d8d828ff276 | 170 | }//End of else if |
| pedlerw | 1:99f06e7e6906 | 171 | xbee.printf("\r\nGoing to position %d", desired_location); |
| pedlerw | 0:2d8d828ff276 | 172 | //Pass desired_location to move_linear(); |
| pedlerw | 0:2d8d828ff276 | 173 | //On normal exit move_linear returns 4... causes repeat of operation menu// |
| pedlerw | 0:2d8d828ff276 | 174 | user_instruction = move_linear(desired_location); |
| pedlerw | 0:2d8d828ff276 | 175 | break; |
| pedlerw | 0:2d8d828ff276 | 176 | default: |
| pedlerw | 0:2d8d828ff276 | 177 | //Get valid user_input// |
| pedlerw | 1:99f06e7e6906 | 178 | scroll_up(30); |
| pedlerw | 1:99f06e7e6906 | 179 | xbee.printf("\r\n\n****Main Menu****"); |
| pedlerw | 1:99f06e7e6906 | 180 | xbee.printf("\r\nFor Calibration press 1 and <Enter>."); |
| pedlerw | 1:99f06e7e6906 | 181 | xbee.printf("\r\nTo enter a desired location press 2 and <Enter>."); |
| pedlerw | 1:99f06e7e6906 | 182 | xbee.printf("\r\nFor display mode press 3 and <Enter>."); |
| pedlerw | 1:99f06e7e6906 | 183 | scroll_up(24); |
| pedlerw | 1:99f06e7e6906 | 184 | user_instruction = error_check(); |
| pedlerw | 0:2d8d828ff276 | 185 | break; |
| pedlerw | 0:2d8d828ff276 | 186 | }//End of switch |
| pedlerw | 0:2d8d828ff276 | 187 | }//End of while(1)// |
| pedlerw | 0:2d8d828ff276 | 188 | }//End of main |
| pedlerw | 0:2d8d828ff276 | 189 | /******************************************************************************* |
| pedlerw | 0:2d8d828ff276 | 190 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 191 | /////////////////////////////FUNCITONS///////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 192 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 193 | *******************************************************************************/ |
| pedlerw | 0:2d8d828ff276 | 194 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 195 | ////////////////////////////move_all_left////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 196 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 197 | //This function accepts an interger value and moves the forcer to the left// |
| pedlerw | 0:2d8d828ff276 | 198 | //The interger number of coil sets// |
| pedlerw | 0:2d8d828ff276 | 199 | void move_all_left(int set){ |
| pedlerw | 0:2d8d828ff276 | 200 | //Declartions and initialization// |
| pedlerw | 0:2d8d828ff276 | 201 | linear0 = 1; //Coil1 Mod0 |
| pedlerw | 0:2d8d828ff276 | 202 | linear1 = 1; //Coil2 Mod1 |
| pedlerw | 0:2d8d828ff276 | 203 | linear2 = 1; //Coil3 Mod2 |
| pedlerw | 0:2d8d828ff276 | 204 | linear3 = 1; //Coil4 Mod3 |
| pedlerw | 0:2d8d828ff276 | 205 | linear4 = 1; //Coil5 Mod4 |
| pedlerw | 0:2d8d828ff276 | 206 | linear5 = 1; //Coil6 Mod5 |
| pedlerw | 0:2d8d828ff276 | 207 | linear6 = 1; //Coil7 Mod6 |
| pedlerw | 0:2d8d828ff276 | 208 | |
| pedlerw | 0:2d8d828ff276 | 209 | //LEFT// |
| pedlerw | 0:2d8d828ff276 | 210 | for(int i=0;i<set;i++){ |
| pedlerw | 1:99f06e7e6906 | 211 | linear4 =! linear4; //Coil 5 |
| pedlerw | 1:99f06e7e6906 | 212 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 213 | linear4 =! linear4; |
| pedlerw | 1:99f06e7e6906 | 214 | linear5 =! linear5; //Coil 6 |
| pedlerw | 1:99f06e7e6906 | 215 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 216 | linear5 =! linear5; |
| pedlerw | 1:99f06e7e6906 | 217 | linear6 =! linear6; //Coil 7 |
| pedlerw | 1:99f06e7e6906 | 218 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 219 | linear6 =! linear6; |
| pedlerw | 1:99f06e7e6906 | 220 | linear0 =! linear0; //Coil 1 |
| pedlerw | 1:99f06e7e6906 | 221 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 222 | linear0 =! linear0; |
| pedlerw | 1:99f06e7e6906 | 223 | linear1 =! linear1; //Coil 2 |
| pedlerw | 1:99f06e7e6906 | 224 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 225 | linear1 =! linear1; |
| pedlerw | 1:99f06e7e6906 | 226 | linear2 =! linear2; //Coil 3 |
| pedlerw | 1:99f06e7e6906 | 227 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 228 | linear2 =! linear2; |
| pedlerw | 1:99f06e7e6906 | 229 | linear3 =! linear3; //Coil 4 |
| pedlerw | 1:99f06e7e6906 | 230 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 231 | linear3 =! linear3; |
| pedlerw | 0:2d8d828ff276 | 232 | }//End of For loop |
| pedlerw | 0:2d8d828ff276 | 233 | return; |
| pedlerw | 0:2d8d828ff276 | 234 | }//End of Funciton |
| pedlerw | 0:2d8d828ff276 | 235 | |
| pedlerw | 0:2d8d828ff276 | 236 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 237 | ////////////////////////////move_all_right///////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 238 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 239 | //This function accepts an interger value and moves the forcer to the right// |
| pedlerw | 0:2d8d828ff276 | 240 | //The interger number of coil sets// |
| pedlerw | 0:2d8d828ff276 | 241 | void move_all_right(int set){ |
| pedlerw | 0:2d8d828ff276 | 242 | //Declartions and initialization// |
| pedlerw | 0:2d8d828ff276 | 243 | linear0 = 1; //Coil1 Mod0 |
| pedlerw | 0:2d8d828ff276 | 244 | linear1 = 1; //Coil2 Mod1 |
| pedlerw | 0:2d8d828ff276 | 245 | linear2 = 1; //Coil3 Mod2 |
| pedlerw | 0:2d8d828ff276 | 246 | linear3 = 1; //Coil4 Mod3 |
| pedlerw | 0:2d8d828ff276 | 247 | linear4 = 1; //Coil5 Mod4 |
| pedlerw | 0:2d8d828ff276 | 248 | linear5 = 1; //Coil6 Mod5 |
| pedlerw | 0:2d8d828ff276 | 249 | linear6 = 1; //Coil7 Mod6 |
| pedlerw | 0:2d8d828ff276 | 250 | |
| pedlerw | 0:2d8d828ff276 | 251 | //RIGHT// |
| pedlerw | 0:2d8d828ff276 | 252 | for(int i=0;i<set;i++){ |
| pedlerw | 1:99f06e7e6906 | 253 | linear0 =! linear0; //Coil 1 |
| pedlerw | 1:99f06e7e6906 | 254 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 255 | linear0 =! linear0; |
| pedlerw | 1:99f06e7e6906 | 256 | linear6 =! linear6; //Coil 7 |
| pedlerw | 1:99f06e7e6906 | 257 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 258 | linear6 =! linear6; |
| pedlerw | 1:99f06e7e6906 | 259 | linear5 =! linear5; //Coil 6 |
| pedlerw | 1:99f06e7e6906 | 260 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 261 | linear5 =! linear5; |
| pedlerw | 1:99f06e7e6906 | 262 | linear4 =! linear4; //Coil 5 |
| pedlerw | 1:99f06e7e6906 | 263 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 264 | linear4 =! linear4; |
| pedlerw | 1:99f06e7e6906 | 265 | linear3 =! linear3; //Coil 4 |
| pedlerw | 1:99f06e7e6906 | 266 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 267 | linear3 =! linear3; |
| pedlerw | 1:99f06e7e6906 | 268 | linear2 =! linear2; //Coil 3 |
| pedlerw | 1:99f06e7e6906 | 269 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 270 | linear2 =! linear2; |
| pedlerw | 1:99f06e7e6906 | 271 | linear1 =! linear1; //Coil 2 |
| pedlerw | 1:99f06e7e6906 | 272 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 273 | linear1 =! linear1; |
| pedlerw | 0:2d8d828ff276 | 274 | }//End of For loop |
| pedlerw | 0:2d8d828ff276 | 275 | return; |
| pedlerw | 0:2d8d828ff276 | 276 | }//End of Funciton |
| pedlerw | 0:2d8d828ff276 | 277 | |
| pedlerw | 0:2d8d828ff276 | 278 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 279 | ////////////////////////////get_Lsensor_min//////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 280 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 281 | //returns the min distance value of left sensor when called// |
| pedlerw | 0:2d8d828ff276 | 282 | float get_Lsensor_min(){ |
| pedlerw | 0:2d8d828ff276 | 283 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 284 | float min = MAX; |
| pedlerw | 0:2d8d828ff276 | 285 | float read; |
| pedlerw | 0:2d8d828ff276 | 286 | for(int i=0;i<large_num;i++){ |
| pedlerw | 0:2d8d828ff276 | 287 | wait(sensor_set); |
| pedlerw | 0:2d8d828ff276 | 288 | read = distanceL.read(); |
| pedlerw | 0:2d8d828ff276 | 289 | if(read < min){ |
| pedlerw | 0:2d8d828ff276 | 290 | min = read; |
| pedlerw | 0:2d8d828ff276 | 291 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 292 | else{ |
| pedlerw | 0:2d8d828ff276 | 293 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 294 | }//End of else |
| pedlerw | 0:2d8d828ff276 | 295 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 296 | return(min); |
| pedlerw | 0:2d8d828ff276 | 297 | }//End of Function |
| pedlerw | 0:2d8d828ff276 | 298 | |
| pedlerw | 0:2d8d828ff276 | 299 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 300 | //////////////////////////////get_Lsensor_max////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 301 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 302 | //returns the max distance value of the left sensor when called// |
| pedlerw | 0:2d8d828ff276 | 303 | float get_Lsensor_max(){ |
| pedlerw | 0:2d8d828ff276 | 304 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 305 | float max = MIN; |
| pedlerw | 0:2d8d828ff276 | 306 | float read; |
| pedlerw | 0:2d8d828ff276 | 307 | for(int i=0;i<large_num;i++){ |
| pedlerw | 0:2d8d828ff276 | 308 | wait(sensor_set); |
| pedlerw | 0:2d8d828ff276 | 309 | read = distanceL.read(); |
| pedlerw | 0:2d8d828ff276 | 310 | if(read > max){ |
| pedlerw | 0:2d8d828ff276 | 311 | max = read; |
| pedlerw | 0:2d8d828ff276 | 312 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 313 | else{ |
| pedlerw | 0:2d8d828ff276 | 314 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 315 | }//End of else |
| pedlerw | 0:2d8d828ff276 | 316 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 317 | return(max); |
| pedlerw | 0:2d8d828ff276 | 318 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 319 | |
| pedlerw | 0:2d8d828ff276 | 320 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 321 | /////////////////////////////get_Rsensor_min/////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 322 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 323 | //Function returns the min value of the right sensor when called// |
| pedlerw | 0:2d8d828ff276 | 324 | float get_Rsensor_min(){ |
| pedlerw | 0:2d8d828ff276 | 325 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 326 | float min = MAX; |
| pedlerw | 0:2d8d828ff276 | 327 | float read; |
| pedlerw | 0:2d8d828ff276 | 328 | for(int i=0;i<large_num;i++){ |
| pedlerw | 0:2d8d828ff276 | 329 | wait(sensor_set); |
| pedlerw | 0:2d8d828ff276 | 330 | read = distanceR.read(); |
| pedlerw | 0:2d8d828ff276 | 331 | if(read < min){ |
| pedlerw | 0:2d8d828ff276 | 332 | min = read; |
| pedlerw | 0:2d8d828ff276 | 333 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 334 | else{ |
| pedlerw | 0:2d8d828ff276 | 335 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 336 | }//End of else |
| pedlerw | 0:2d8d828ff276 | 337 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 338 | return(min); |
| pedlerw | 0:2d8d828ff276 | 339 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 340 | |
| pedlerw | 0:2d8d828ff276 | 341 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 342 | ///////////////////////////////get_Rsensor_max///////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 343 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 344 | float get_Rsensor_max(){ |
| pedlerw | 0:2d8d828ff276 | 345 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 346 | float max = MIN; |
| pedlerw | 0:2d8d828ff276 | 347 | float read; |
| pedlerw | 0:2d8d828ff276 | 348 | for(int i=0;i<large_num;i++){ |
| pedlerw | 0:2d8d828ff276 | 349 | wait(sensor_set); |
| pedlerw | 0:2d8d828ff276 | 350 | read = distanceR.read(); |
| pedlerw | 0:2d8d828ff276 | 351 | if(read > max){ |
| pedlerw | 0:2d8d828ff276 | 352 | max = read; |
| pedlerw | 0:2d8d828ff276 | 353 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 354 | else{ |
| pedlerw | 0:2d8d828ff276 | 355 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 356 | }//End of else |
| pedlerw | 0:2d8d828ff276 | 357 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 358 | return(max); |
| pedlerw | 0:2d8d828ff276 | 359 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 360 | |
| pedlerw | 0:2d8d828ff276 | 361 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 362 | ///////////////////////////////move_one_right////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 363 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 364 | //Function takes current position and moves one to the right |
| pedlerw | 0:2d8d828ff276 | 365 | void move_one_right(int position){ |
| pedlerw | 0:2d8d828ff276 | 366 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 367 | int current_coil = abs(position % seven); |
| pedlerw | 0:2d8d828ff276 | 368 | //xbee.printf("\r\nCurrent Coil:\t%d",current_coil); |
| pedlerw | 0:2d8d828ff276 | 369 | switch(current_coil){ |
| pedlerw | 0:2d8d828ff276 | 370 | case 0: |
| pedlerw | 0:2d8d828ff276 | 371 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 372 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 373 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 374 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 375 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 376 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 377 | linear0 = 0; //Fire Coil 1// |
| pedlerw | 0:2d8d828ff276 | 378 | break; |
| pedlerw | 0:2d8d828ff276 | 379 | case 1: |
| pedlerw | 0:2d8d828ff276 | 380 | linear6 = 0; //Fire Coil 7// |
| pedlerw | 0:2d8d828ff276 | 381 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 382 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 383 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 384 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 385 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 386 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 387 | break; |
| pedlerw | 0:2d8d828ff276 | 388 | case 2: |
| pedlerw | 0:2d8d828ff276 | 389 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 390 | linear5 = 0; //Fire Coil 6// |
| pedlerw | 0:2d8d828ff276 | 391 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 392 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 393 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 394 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 395 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 396 | break; |
| pedlerw | 0:2d8d828ff276 | 397 | case 3: |
| pedlerw | 0:2d8d828ff276 | 398 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 399 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 400 | linear4 = 0; //Fire Coil 5// |
| pedlerw | 0:2d8d828ff276 | 401 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 402 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 403 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 404 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 405 | break; |
| pedlerw | 0:2d8d828ff276 | 406 | case 4: |
| pedlerw | 0:2d8d828ff276 | 407 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 408 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 409 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 410 | linear3 = 0; //Fire Coil 4// |
| pedlerw | 0:2d8d828ff276 | 411 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 412 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 413 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 414 | break; |
| pedlerw | 0:2d8d828ff276 | 415 | case 5: |
| pedlerw | 0:2d8d828ff276 | 416 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 417 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 418 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 419 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 420 | linear2 = 0; //Fire Coil 3// |
| pedlerw | 0:2d8d828ff276 | 421 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 422 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 423 | break; |
| pedlerw | 0:2d8d828ff276 | 424 | case 6: |
| pedlerw | 0:2d8d828ff276 | 425 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 426 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 427 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 428 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 429 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 430 | linear1 = 0; //Fire Coil 2// |
| pedlerw | 0:2d8d828ff276 | 431 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 432 | break; |
| pedlerw | 0:2d8d828ff276 | 433 | default: |
| pedlerw | 0:2d8d828ff276 | 434 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 435 | }//End of switch |
| pedlerw | 0:2d8d828ff276 | 436 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 437 | |
| pedlerw | 0:2d8d828ff276 | 438 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 439 | ///////////////////////////////move_one_left////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 440 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 441 | //Function takes current position and moves one to the left |
| pedlerw | 0:2d8d828ff276 | 442 | void move_one_left(int position){ |
| pedlerw | 0:2d8d828ff276 | 443 | //Declarations and initializations// |
| pedlerw | 0:2d8d828ff276 | 444 | int current_coil = position % seven; |
| pedlerw | 0:2d8d828ff276 | 445 | //xbee.printf("\r\nCurrent Coil:\t%d",current_coil); |
| pedlerw | 0:2d8d828ff276 | 446 | switch(current_coil){ |
| pedlerw | 0:2d8d828ff276 | 447 | case 0: |
| pedlerw | 0:2d8d828ff276 | 448 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 449 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 450 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 451 | linear3 = 0; //Fire Coil 4// |
| pedlerw | 0:2d8d828ff276 | 452 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 453 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 454 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 455 | break; |
| pedlerw | 0:2d8d828ff276 | 456 | case 1: |
| pedlerw | 0:2d8d828ff276 | 457 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 458 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 459 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 460 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 461 | linear2 = 0; //Fire Coil 3// |
| pedlerw | 0:2d8d828ff276 | 462 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 463 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 464 | break; |
| pedlerw | 0:2d8d828ff276 | 465 | case 2: |
| pedlerw | 0:2d8d828ff276 | 466 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 467 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 468 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 469 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 470 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 471 | linear1 = 0; //Fire Coil 2// |
| pedlerw | 0:2d8d828ff276 | 472 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 473 | break; |
| pedlerw | 0:2d8d828ff276 | 474 | case 3: |
| pedlerw | 0:2d8d828ff276 | 475 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 476 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 477 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 478 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 479 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 480 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 481 | linear0 = 0; //Fire Coil 1// |
| pedlerw | 0:2d8d828ff276 | 482 | break; |
| pedlerw | 0:2d8d828ff276 | 483 | case 4: |
| pedlerw | 0:2d8d828ff276 | 484 | linear6 = 0; //Fire Coil 7// |
| pedlerw | 0:2d8d828ff276 | 485 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 486 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 487 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 488 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 489 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 490 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 491 | break; |
| pedlerw | 0:2d8d828ff276 | 492 | case 5: |
| pedlerw | 0:2d8d828ff276 | 493 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 494 | linear5 = 0; //Fire Coil 6// |
| pedlerw | 0:2d8d828ff276 | 495 | linear4 = 1; |
| pedlerw | 0:2d8d828ff276 | 496 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 497 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 498 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 499 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 500 | break; |
| pedlerw | 0:2d8d828ff276 | 501 | case 6: |
| pedlerw | 0:2d8d828ff276 | 502 | linear6 = 1; |
| pedlerw | 0:2d8d828ff276 | 503 | linear5 = 1; |
| pedlerw | 0:2d8d828ff276 | 504 | linear4 = 0; //Fire Coil 5// |
| pedlerw | 0:2d8d828ff276 | 505 | linear3 = 1; |
| pedlerw | 0:2d8d828ff276 | 506 | linear2 = 1; |
| pedlerw | 0:2d8d828ff276 | 507 | linear1 = 1; |
| pedlerw | 0:2d8d828ff276 | 508 | linear0 = 1; |
| pedlerw | 0:2d8d828ff276 | 509 | break; |
| pedlerw | 0:2d8d828ff276 | 510 | default: |
| pedlerw | 0:2d8d828ff276 | 511 | wait(MIN); |
| pedlerw | 0:2d8d828ff276 | 512 | }//End of switch |
| pedlerw | 0:2d8d828ff276 | 513 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 514 | |
| pedlerw | 0:2d8d828ff276 | 515 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 516 | /////////////////////////////get_position////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 517 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 518 | |
| pedlerw | 0:2d8d828ff276 | 519 | //Function reads current position on track and returns the mod 7// |
| pedlerw | 0:2d8d828ff276 | 520 | //Function uses calibration data to determin current position// |
| pedlerw | 0:2d8d828ff276 | 521 | int get_position(){ |
| pedlerw | 0:2d8d828ff276 | 522 | //Declrations and Initializaitons// |
| pedlerw | 0:2d8d828ff276 | 523 | int position = -1; |
| pedlerw | 0:2d8d828ff276 | 524 | int count = 0; |
| pedlerw | 0:2d8d828ff276 | 525 | while(position == -1){ |
| pedlerw | 0:2d8d828ff276 | 526 | wait(slow); |
| pedlerw | 0:2d8d828ff276 | 527 | float left_sensor = distanceL.read(); |
| pedlerw | 0:2d8d828ff276 | 528 | float right_sensor = distanceR.read(); |
| pedlerw | 0:2d8d828ff276 | 529 | //xbee.printf("\r\nleft_sensor:%f",left_sensor); |
| pedlerw | 0:2d8d828ff276 | 530 | //xbee.printf("\r\nright_sensor:%f",right_sensor); |
| pedlerw | 0:2d8d828ff276 | 531 | //Use closest sensor// |
| pedlerw | 0:2d8d828ff276 | 532 | if(left_sensor > right_sensor){ |
| pedlerw | 0:2d8d828ff276 | 533 | //xbee.printf("\r\nleft > right"); |
| pedlerw | 0:2d8d828ff276 | 534 | //Look at Lsensor data// |
| pedlerw | 0:2d8d828ff276 | 535 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 536 | if(left_sensor <= left_max[i] && left_sensor >= left_min[i]){ |
| pedlerw | 0:2d8d828ff276 | 537 | position = i; |
| pedlerw | 0:2d8d828ff276 | 538 | xbee.printf("\r\nPosition L1 %d",position); |
| pedlerw | 0:2d8d828ff276 | 539 | return position; |
| pedlerw | 0:2d8d828ff276 | 540 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 541 | else if(left_sensor < left_max[i] && left_sensor > left_max[i+one]){ |
| pedlerw | 0:2d8d828ff276 | 542 | position = i+one; |
| pedlerw | 0:2d8d828ff276 | 543 | xbee.printf("\r\nPosition L2 %d",position); |
| pedlerw | 0:2d8d828ff276 | 544 | return position; |
| pedlerw | 0:2d8d828ff276 | 545 | }//End of else if// |
| pedlerw | 0:2d8d828ff276 | 546 | else if(left_sensor < left_max[i] && left_sensor > left_min[i+one]){ |
| pedlerw | 0:2d8d828ff276 | 547 | position = i; |
| pedlerw | 0:2d8d828ff276 | 548 | xbee.printf("\r\nPosition L3 %d",position); |
| pedlerw | 0:2d8d828ff276 | 549 | return position; |
| pedlerw | 0:2d8d828ff276 | 550 | }//End of else if// |
| pedlerw | 0:2d8d828ff276 | 551 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 552 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 553 | |
| pedlerw | 0:2d8d828ff276 | 554 | else if(right_sensor >= left_sensor){ |
| pedlerw | 0:2d8d828ff276 | 555 | xbee.printf("\r\nright > left"); |
| pedlerw | 0:2d8d828ff276 | 556 | //Look at Rsensor data// |
| pedlerw | 0:2d8d828ff276 | 557 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 558 | if(right_sensor <= right_max[i] && right_sensor >= right_min[i]){ |
| pedlerw | 0:2d8d828ff276 | 559 | position = i; |
| pedlerw | 0:2d8d828ff276 | 560 | xbee.printf("\r\nPosition R1 %d",position); |
| pedlerw | 0:2d8d828ff276 | 561 | return position; |
| pedlerw | 0:2d8d828ff276 | 562 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 563 | else if(right_sensor > right_max[i] && right_sensor < right_max[i+one]){ |
| pedlerw | 0:2d8d828ff276 | 564 | position = i+one; |
| pedlerw | 0:2d8d828ff276 | 565 | xbee.printf("\r\nPosition R2 %d",position); |
| pedlerw | 0:2d8d828ff276 | 566 | return position; |
| pedlerw | 0:2d8d828ff276 | 567 | }//End of else if// |
| pedlerw | 0:2d8d828ff276 | 568 | else if(right_sensor > right_max[i] && right_sensor < right_min[i+one]){ |
| pedlerw | 0:2d8d828ff276 | 569 | position = i; |
| pedlerw | 0:2d8d828ff276 | 570 | xbee.printf("\r\nPosition R3 %d",position); |
| pedlerw | 0:2d8d828ff276 | 571 | return position; |
| pedlerw | 0:2d8d828ff276 | 572 | }//End of else if// |
| pedlerw | 0:2d8d828ff276 | 573 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 574 | }//End of else if// |
| pedlerw | 0:2d8d828ff276 | 575 | count++; |
| pedlerw | 0:2d8d828ff276 | 576 | if(count > five){ |
| pedlerw | 0:2d8d828ff276 | 577 | return(-2); |
| pedlerw | 0:2d8d828ff276 | 578 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 579 | }//End of while(position.... |
| pedlerw | 0:2d8d828ff276 | 580 | return position; |
| pedlerw | 0:2d8d828ff276 | 581 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 582 | |
| pedlerw | 0:2d8d828ff276 | 583 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 584 | ////////////////////////////////CALIBRATE////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 585 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 586 | /* |
| pedlerw | 0:2d8d828ff276 | 587 | -Calibrate Motor |
| pedlerw | 0:2d8d828ff276 | 588 | 1. Move all the way to the LEFT side of the track. |
| pedlerw | 0:2d8d828ff276 | 589 | 2. Get max and mins from both distance sensors. |
| pedlerw | 0:2d8d828ff276 | 590 | 3. Store the max and mins in various arrays. |
| pedlerw | 0:2d8d828ff276 | 591 | 4. Move one position to the RIGHT and repeat for entire track. |
| pedlerw | 0:2d8d828ff276 | 592 | 5. Once the entire track has been recorded export to a txt file on the mbed. |
| pedlerw | 0:2d8d828ff276 | 593 | Format for txt file: |
| pedlerw | 0:2d8d828ff276 | 594 | Position # :Left Low :Left High :Right Low :Right High |
| pedlerw | 0:2d8d828ff276 | 595 | 0 :#### :#### :#### :#### |
| pedlerw | 0:2d8d828ff276 | 596 | 1 (3 \t) :#### (2 \t)... |
| pedlerw | 0:2d8d828ff276 | 597 | . |
| pedlerw | 0:2d8d828ff276 | 598 | . |
| pedlerw | 0:2d8d828ff276 | 599 | #EOF |
| pedlerw | 0:2d8d828ff276 | 600 | */ |
| pedlerw | 1:99f06e7e6906 | 601 | void calibrate_linear(){ |
| pedlerw | 0:2d8d828ff276 | 602 | //1.Move all the way to the LEFT side of the track// |
| pedlerw | 0:2d8d828ff276 | 603 | move_all_left(six); |
| pedlerw | 0:2d8d828ff276 | 604 | //2.Get Sensor Data// |
| pedlerw | 0:2d8d828ff276 | 605 | for(int i=0;i<track_length;i++){ |
| pedlerw | 1:99f06e7e6906 | 606 | //3.Store in various arrays |
| pedlerw | 0:2d8d828ff276 | 607 | wait(slow); |
| pedlerw | 0:2d8d828ff276 | 608 | left_min[i] = get_Lsensor_min(); |
| pedlerw | 0:2d8d828ff276 | 609 | left_max[i] = get_Lsensor_max(); |
| pedlerw | 0:2d8d828ff276 | 610 | right_min[i] = get_Rsensor_min(); |
| pedlerw | 0:2d8d828ff276 | 611 | right_max[i] = get_Rsensor_max(); |
| pedlerw | 0:2d8d828ff276 | 612 | led4 = !led4; |
| pedlerw | 1:99f06e7e6906 | 613 | //4.Move to the right one// |
| pedlerw | 0:2d8d828ff276 | 614 | move_one_right(i); |
| pedlerw | 0:2d8d828ff276 | 615 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 616 | |
| pedlerw | 0:2d8d828ff276 | 617 | //5. Export data to a txt file on the mbed// |
| pedlerw | 0:2d8d828ff276 | 618 | FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing |
| pedlerw | 0:2d8d828ff276 | 619 | fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n"); |
| pedlerw | 0:2d8d828ff276 | 620 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 621 | 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:2d8d828ff276 | 622 | /* |
| pedlerw | 0:2d8d828ff276 | 623 | //Example File Handeling Code// |
| pedlerw | 0:2d8d828ff276 | 624 | FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing |
| pedlerw | 0:2d8d828ff276 | 625 | fprintf(fp, "Hello World!"); |
| pedlerw | 0:2d8d828ff276 | 626 | fclose(fp); |
| pedlerw | 0:2d8d828ff276 | 627 | */ |
| pedlerw | 0:2d8d828ff276 | 628 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 629 | fclose(fp); |
| pedlerw | 0:2d8d828ff276 | 630 | //Export each variable to its own file// |
| pedlerw | 0:2d8d828ff276 | 631 | //Write leftmin.txt file// |
| pedlerw | 0:2d8d828ff276 | 632 | FILE *fp1 = fopen("/local/leftmin.txt", "w"); |
| pedlerw | 0:2d8d828ff276 | 633 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 634 | fprintf(fp1,"%f\r\n",left_min[i]); |
| pedlerw | 0:2d8d828ff276 | 635 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 636 | fclose(fp1); |
| pedlerw | 0:2d8d828ff276 | 637 | //Write leftmax.txt file// |
| pedlerw | 0:2d8d828ff276 | 638 | FILE *fp2 = fopen("/local/leftmax.txt", "w"); |
| pedlerw | 0:2d8d828ff276 | 639 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 640 | fprintf(fp2,"%f\r\n",left_max[i]); |
| pedlerw | 0:2d8d828ff276 | 641 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 642 | fclose(fp2); |
| pedlerw | 0:2d8d828ff276 | 643 | //Write rightmin.txt file// |
| pedlerw | 0:2d8d828ff276 | 644 | FILE *fp3 = fopen("/local/rightmin.txt", "w"); |
| pedlerw | 0:2d8d828ff276 | 645 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 646 | fprintf(fp3,"%f\r\n",right_min[i]); |
| pedlerw | 0:2d8d828ff276 | 647 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 648 | fclose(fp3); |
| pedlerw | 0:2d8d828ff276 | 649 | //Write rightmax.txt file// |
| pedlerw | 0:2d8d828ff276 | 650 | FILE *fp4 = fopen("/local/rightmax.txt", "w"); |
| pedlerw | 0:2d8d828ff276 | 651 | for(int i=0;i<track_length;i++){ |
| pedlerw | 0:2d8d828ff276 | 652 | fprintf(fp4,"%f\r\n",right_max[i]); |
| pedlerw | 0:2d8d828ff276 | 653 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 654 | fclose(fp4); |
| pedlerw | 0:2d8d828ff276 | 655 | |
| pedlerw | 0:2d8d828ff276 | 656 | //Set an LED high so we know calibration is complete// |
| pedlerw | 0:2d8d828ff276 | 657 | led1 = 1; |
| pedlerw | 0:2d8d828ff276 | 658 | move_all_left(six); |
| pedlerw | 0:2d8d828ff276 | 659 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 660 | |
| pedlerw | 0:2d8d828ff276 | 661 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 662 | ////////////////////////////read_file////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 663 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 664 | //read_file reads the text files containing calibration data |
| pedlerw | 0:2d8d828ff276 | 665 | void read_file(){ |
| pedlerw | 0:2d8d828ff276 | 666 | char line[80]; |
| pedlerw | 0:2d8d828ff276 | 667 | int i = 0; |
| pedlerw | 0:2d8d828ff276 | 668 | FILE *fr1; |
| pedlerw | 0:2d8d828ff276 | 669 | fr1 = fopen ("/local/leftmin.txt", "rt"); |
| pedlerw | 0:2d8d828ff276 | 670 | while(fgets(line, 80, fr1) != NULL){ |
| pedlerw | 0:2d8d828ff276 | 671 | sscanf (line, "%f", &left_min[i]); |
| pedlerw | 0:2d8d828ff276 | 672 | i++; |
| pedlerw | 0:2d8d828ff276 | 673 | }//End of while |
| pedlerw | 0:2d8d828ff276 | 674 | fclose(fr1); |
| pedlerw | 0:2d8d828ff276 | 675 | |
| pedlerw | 0:2d8d828ff276 | 676 | i = 0; |
| pedlerw | 0:2d8d828ff276 | 677 | FILE *fr2; |
| pedlerw | 0:2d8d828ff276 | 678 | fr2 = fopen ("/local/leftmax.txt", "rt"); |
| pedlerw | 0:2d8d828ff276 | 679 | while(fgets(line, 80, fr2) != NULL){ |
| pedlerw | 0:2d8d828ff276 | 680 | sscanf (line, "%f", &left_max[i]); |
| pedlerw | 0:2d8d828ff276 | 681 | i++; |
| pedlerw | 0:2d8d828ff276 | 682 | }//End of while |
| pedlerw | 0:2d8d828ff276 | 683 | fclose(fr2); |
| pedlerw | 0:2d8d828ff276 | 684 | |
| pedlerw | 0:2d8d828ff276 | 685 | i = 0; |
| pedlerw | 0:2d8d828ff276 | 686 | FILE *fr3; |
| pedlerw | 0:2d8d828ff276 | 687 | fr3 = fopen ("/local/rightmin.txt", "rt"); |
| pedlerw | 0:2d8d828ff276 | 688 | while(fgets(line, 80, fr3) != NULL){ |
| pedlerw | 0:2d8d828ff276 | 689 | sscanf (line, "%f", &right_min[i]); |
| pedlerw | 0:2d8d828ff276 | 690 | i++; |
| pedlerw | 0:2d8d828ff276 | 691 | }//End of while |
| pedlerw | 0:2d8d828ff276 | 692 | fclose(fr3); |
| pedlerw | 0:2d8d828ff276 | 693 | |
| pedlerw | 0:2d8d828ff276 | 694 | i = 0; |
| pedlerw | 0:2d8d828ff276 | 695 | FILE *fr4; |
| pedlerw | 0:2d8d828ff276 | 696 | fr4 = fopen ("/local/rightmax.txt", "rt"); |
| pedlerw | 0:2d8d828ff276 | 697 | while(fgets(line, 80, fr4) != NULL){ |
| pedlerw | 0:2d8d828ff276 | 698 | sscanf (line, "%f", &right_max[i]); |
| pedlerw | 0:2d8d828ff276 | 699 | i++; |
| pedlerw | 0:2d8d828ff276 | 700 | }//End of while |
| pedlerw | 0:2d8d828ff276 | 701 | fclose(fr4); |
| pedlerw | 0:2d8d828ff276 | 702 | }//End of function |
| pedlerw | 0:2d8d828ff276 | 703 | |
| pedlerw | 0:2d8d828ff276 | 704 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 705 | //////////////////////////////move_linear////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 706 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 707 | //move_linear accepts a desired locaiton and moves the forcer accordingly// |
| pedlerw | 0:2d8d828ff276 | 708 | //Returns 4 on normal exit... prompting the user for a new position// |
| pedlerw | 0:2d8d828ff276 | 709 | int move_linear(int desired_location){ |
| pedlerw | 0:2d8d828ff276 | 710 | int current_location = get_position(); |
| pedlerw | 0:2d8d828ff276 | 711 | int difference = current_location - desired_location; |
| pedlerw | 0:2d8d828ff276 | 712 | int count = 0; |
| pedlerw | 0:2d8d828ff276 | 713 | xbee.printf("\r\nInside move_linear function"); |
| pedlerw | 0:2d8d828ff276 | 714 | xbee.printf("\r\nCurrent_location %d", current_location); |
| pedlerw | 0:2d8d828ff276 | 715 | if(desired_location == -1){ |
| pedlerw | 0:2d8d828ff276 | 716 | return -1; |
| pedlerw | 0:2d8d828ff276 | 717 | }//End of if |
| pedlerw | 1:99f06e7e6906 | 718 | while(current_location != desired_location){ |
| pedlerw | 0:2d8d828ff276 | 719 | difference = current_location - desired_location; |
| pedlerw | 0:2d8d828ff276 | 720 | xbee.printf("\r\nDifference %d",difference); |
| pedlerw | 0:2d8d828ff276 | 721 | while(difference > 0){ |
| pedlerw | 1:99f06e7e6906 | 722 | if(count > two){ |
| pedlerw | 1:99f06e7e6906 | 723 | for(int i=0; i<difference;i++){ |
| pedlerw | 1:99f06e7e6906 | 724 | move_one_left(current_location-one); |
| pedlerw | 1:99f06e7e6906 | 725 | current_location--; |
| pedlerw | 1:99f06e7e6906 | 726 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 727 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 728 | //Move LEFT// |
| pedlerw | 0:2d8d828ff276 | 729 | xbee.printf("\r\nMoving Left"); |
| pedlerw | 1:99f06e7e6906 | 730 | led2 = 1; |
| pedlerw | 1:99f06e7e6906 | 731 | wait(medium); |
| pedlerw | 1:99f06e7e6906 | 732 | for(int i=0; i<difference;i++){ |
| pedlerw | 0:2d8d828ff276 | 733 | move_one_left(current_location); |
| pedlerw | 0:2d8d828ff276 | 734 | current_location--; |
| pedlerw | 0:2d8d828ff276 | 735 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 736 | }//End of for |
| pedlerw | 1:99f06e7e6906 | 737 | current_location = get_position(); |
| pedlerw | 1:99f06e7e6906 | 738 | difference = current_location - desired_location; |
| pedlerw | 1:99f06e7e6906 | 739 | led2 = 0; |
| pedlerw | 1:99f06e7e6906 | 740 | count++; |
| pedlerw | 0:2d8d828ff276 | 741 | if(current_location == -2){ |
| pedlerw | 0:2d8d828ff276 | 742 | return(four); |
| pedlerw | 0:2d8d828ff276 | 743 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 744 | }//End of while(greater than zero)// |
| pedlerw | 0:2d8d828ff276 | 745 | |
| pedlerw | 0:2d8d828ff276 | 746 | while(difference < 0){ |
| pedlerw | 1:99f06e7e6906 | 747 | if(count > two){ |
| pedlerw | 1:99f06e7e6906 | 748 | for(int i=0;i<(abs(difference));i++){ |
| pedlerw | 1:99f06e7e6906 | 749 | move_one_right(current_location+one); |
| pedlerw | 1:99f06e7e6906 | 750 | current_location++; |
| pedlerw | 1:99f06e7e6906 | 751 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 752 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 753 | //Move RIGHT// |
| pedlerw | 0:2d8d828ff276 | 754 | xbee.printf("\r\nMoving Right"); |
| pedlerw | 1:99f06e7e6906 | 755 | led3 = 1; |
| pedlerw | 1:99f06e7e6906 | 756 | wait(medium); |
| pedlerw | 1:99f06e7e6906 | 757 | for(int i=0; i<(abs(difference));i++){ |
| pedlerw | 0:2d8d828ff276 | 758 | move_one_right(current_location); |
| pedlerw | 0:2d8d828ff276 | 759 | current_location++; |
| pedlerw | 0:2d8d828ff276 | 760 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 761 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 762 | current_location = get_position(); |
| pedlerw | 0:2d8d828ff276 | 763 | difference = current_location - desired_location; |
| pedlerw | 0:2d8d828ff276 | 764 | led3 = 0; |
| pedlerw | 0:2d8d828ff276 | 765 | count++; |
| pedlerw | 1:99f06e7e6906 | 766 | if(current_location == -2){ |
| pedlerw | 1:99f06e7e6906 | 767 | return(four); |
| pedlerw | 1:99f06e7e6906 | 768 | }//End of if// |
| pedlerw | 0:2d8d828ff276 | 769 | }//End of while(difference less than 0)// |
| pedlerw | 0:2d8d828ff276 | 770 | }//End of while(current not desired)// |
| pedlerw | 0:2d8d828ff276 | 771 | return(four); |
| pedlerw | 0:2d8d828ff276 | 772 | }//End of function// |
| pedlerw | 0:2d8d828ff276 | 773 | |
| pedlerw | 0:2d8d828ff276 | 774 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 775 | ////////////////////////////error_check//////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 776 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 777 | //Function does scanf error checking// |
| pedlerw | 0:2d8d828ff276 | 778 | //It clears the keyboard buffer incase anything other than numbers a pressed// |
| pedlerw | 0:2d8d828ff276 | 779 | int error_check(){ |
| pedlerw | 0:2d8d828ff276 | 780 | char not_number; |
| pedlerw | 0:2d8d828ff276 | 781 | int number; |
| pedlerw | 0:2d8d828ff276 | 782 | int flag; |
| pedlerw | 0:2d8d828ff276 | 783 | while ((flag = xbee.scanf("%d", &number)) != EOF){ |
| pedlerw | 0:2d8d828ff276 | 784 | if (flag != 1){ |
| pedlerw | 0:2d8d828ff276 | 785 | not_number = xbee.getc(); |
| pedlerw | 0:2d8d828ff276 | 786 | #ifdef DEBUG |
| pedlerw | 0:2d8d828ff276 | 787 | xbee.printf("\r\ndegub:%c in input stream, discarding", not_number); |
| pedlerw | 0:2d8d828ff276 | 788 | #endif |
| pedlerw | 0:2d8d828ff276 | 789 | }//End of if |
| pedlerw | 0:2d8d828ff276 | 790 | else { |
| pedlerw | 0:2d8d828ff276 | 791 | printf("\r\nRETURN = %d",number); |
| pedlerw | 0:2d8d828ff276 | 792 | return(number); |
| pedlerw | 0:2d8d828ff276 | 793 | }//End of else// |
| pedlerw | 0:2d8d828ff276 | 794 | }//End of while((flag... |
| pedlerw | 0:2d8d828ff276 | 795 | return(number); |
| pedlerw | 0:2d8d828ff276 | 796 | }//End of function// |
| pedlerw | 0:2d8d828ff276 | 797 | |
| pedlerw | 0:2d8d828ff276 | 798 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 799 | ////////////////////////////////display_mode/////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 800 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 801 | //Function set up to run display mode options// |
| pedlerw | 0:2d8d828ff276 | 802 | void display_mode(){ |
| pedlerw | 0:2d8d828ff276 | 803 | int option = -1; |
| pedlerw | 0:2d8d828ff276 | 804 | int exit = 0; |
| pedlerw | 0:2d8d828ff276 | 805 | while(exit != one){ |
| pedlerw | 0:2d8d828ff276 | 806 | switch (option){ |
| pedlerw | 0:2d8d828ff276 | 807 | case 1: |
| pedlerw | 0:2d8d828ff276 | 808 | //Wind sprints// |
| pedlerw | 0:2d8d828ff276 | 809 | for(int j=0;j<large_num;j++){ |
| pedlerw | 0:2d8d828ff276 | 810 | for(int i=0;i<12;i++){ |
| pedlerw | 1:99f06e7e6906 | 811 | move_all_left(abs(six-i)); |
| pedlerw | 1:99f06e7e6906 | 812 | move_all_right(abs(six-i)); |
| pedlerw | 0:2d8d828ff276 | 813 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 814 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 815 | break; |
| pedlerw | 0:2d8d828ff276 | 816 | case 2: |
| pedlerw | 0:2d8d828ff276 | 817 | //Sucide sprints// |
| pedlerw | 0:2d8d828ff276 | 818 | for(int j=0;j<large_num;j++){ |
| pedlerw | 0:2d8d828ff276 | 819 | for(int i=0;i<12;i++){ |
| pedlerw | 1:99f06e7e6906 | 820 | move_all_leftBobby(abs(six-i)); |
| pedlerw | 1:99f06e7e6906 | 821 | move_all_rightBobby(abs(six-i)); |
| pedlerw | 0:2d8d828ff276 | 822 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 823 | }//End of for// |
| pedlerw | 0:2d8d828ff276 | 824 | break; |
| pedlerw | 0:2d8d828ff276 | 825 | case 3: |
| pedlerw | 0:2d8d828ff276 | 826 | //Option 3// |
| pedlerw | 0:2d8d828ff276 | 827 | |
| pedlerw | 0:2d8d828ff276 | 828 | break; |
| pedlerw | 0:2d8d828ff276 | 829 | case 4: |
| pedlerw | 0:2d8d828ff276 | 830 | //Return to main menu// |
| pedlerw | 0:2d8d828ff276 | 831 | exit = one; |
| pedlerw | 0:2d8d828ff276 | 832 | //break; |
| pedlerw | 0:2d8d828ff276 | 833 | default: |
| pedlerw | 0:2d8d828ff276 | 834 | scroll_up(30); |
| pedlerw | 0:2d8d828ff276 | 835 | xbee.printf("\r\n\n****Display Mode****"); |
| pedlerw | 0:2d8d828ff276 | 836 | xbee.printf("\r\nTo run wind sprints press 1 and <Enter>."); |
| pedlerw | 0:2d8d828ff276 | 837 | xbee.printf("\r\nTo run suicide sprints press 2 and <Enter>."); |
| pedlerw | 0:2d8d828ff276 | 838 | xbee.printf("\r\nTo run option 3 press 3 and <Enter>."); |
| pedlerw | 0:2d8d828ff276 | 839 | xbee.printf("\r\nTo return to the previous menu press 4 and <Enter>."); |
| pedlerw | 0:2d8d828ff276 | 840 | scroll_up(25); |
| pedlerw | 0:2d8d828ff276 | 841 | option = error_check(); |
| pedlerw | 0:2d8d828ff276 | 842 | break; |
| pedlerw | 0:2d8d828ff276 | 843 | }//End of switch// |
| pedlerw | 0:2d8d828ff276 | 844 | }//End of while// |
| pedlerw | 0:2d8d828ff276 | 845 | return; |
| pedlerw | 0:2d8d828ff276 | 846 | }//End of function// |
| pedlerw | 0:2d8d828ff276 | 847 | |
| pedlerw | 0:2d8d828ff276 | 848 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 849 | ////////////////////////////move_all_leftBobby///////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 850 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 851 | //This function accepts an interger value and moves the forcer to the left// |
| pedlerw | 0:2d8d828ff276 | 852 | //The interger number of coil sets// |
| pedlerw | 0:2d8d828ff276 | 853 | void move_all_leftBobby(int set){ |
| pedlerw | 0:2d8d828ff276 | 854 | //Declartions and initialization// |
| pedlerw | 0:2d8d828ff276 | 855 | linear0 = 1; //Coil1 Mod0 |
| pedlerw | 0:2d8d828ff276 | 856 | linear1 = 1; //Coil2 Mod1 |
| pedlerw | 0:2d8d828ff276 | 857 | linear2 = 1; //Coil3 Mod2 |
| pedlerw | 0:2d8d828ff276 | 858 | linear3 = 1; //Coil4 Mod3 |
| pedlerw | 0:2d8d828ff276 | 859 | linear4 = 1; //Coil5 Mod4 |
| pedlerw | 0:2d8d828ff276 | 860 | linear5 = 1; //Coil6 Mod5 |
| pedlerw | 0:2d8d828ff276 | 861 | linear6 = 1; //Coil7 Mod6 |
| pedlerw | 0:2d8d828ff276 | 862 | |
| pedlerw | 0:2d8d828ff276 | 863 | //LEFT// |
| pedlerw | 0:2d8d828ff276 | 864 | for(int i=0;i<set;i++){ |
| pedlerw | 1:99f06e7e6906 | 865 | linear4 =! linear4; //Coil 5 |
| pedlerw | 1:99f06e7e6906 | 866 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 867 | linear4 =! linear4; |
| pedlerw | 1:99f06e7e6906 | 868 | linear5 =! linear5; //Coil 6 |
| pedlerw | 1:99f06e7e6906 | 869 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 870 | linear5 =! linear5; |
| pedlerw | 1:99f06e7e6906 | 871 | linear6 =! linear6; //Coil 7 |
| pedlerw | 1:99f06e7e6906 | 872 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 873 | linear6 =! linear6; |
| pedlerw | 1:99f06e7e6906 | 874 | linear0 =! linear0; //Coil 1 |
| pedlerw | 1:99f06e7e6906 | 875 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 876 | linear0 =! linear0; |
| pedlerw | 1:99f06e7e6906 | 877 | linear1 =! linear1; //Coil 2 |
| pedlerw | 1:99f06e7e6906 | 878 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 879 | linear1 =! linear1; |
| pedlerw | 1:99f06e7e6906 | 880 | linear2 =! linear2; //Coil 3 |
| pedlerw | 1:99f06e7e6906 | 881 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 882 | linear2 =! linear2; |
| pedlerw | 1:99f06e7e6906 | 883 | linear3 =! linear3; //Coil 4 |
| pedlerw | 1:99f06e7e6906 | 884 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 885 | linear3 =! linear3; |
| pedlerw | 0:2d8d828ff276 | 886 | }//End of For loop |
| pedlerw | 0:2d8d828ff276 | 887 | |
| pedlerw | 0:2d8d828ff276 | 888 | linear4 =! linear4; //Coil 5 |
| pedlerw | 0:2d8d828ff276 | 889 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 890 | linear4 =! linear4; |
| pedlerw | 0:2d8d828ff276 | 891 | linear5 =! linear5; //Coil 6 |
| pedlerw | 0:2d8d828ff276 | 892 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 893 | linear5 =! linear5; |
| pedlerw | 0:2d8d828ff276 | 894 | linear6 =! linear6; //Coil 7 |
| pedlerw | 0:2d8d828ff276 | 895 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 896 | linear6 =! linear6; |
| pedlerw | 0:2d8d828ff276 | 897 | linear0 =! linear0; //Coil 1 |
| pedlerw | 0:2d8d828ff276 | 898 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 899 | linear0 =! linear0; |
| pedlerw | 0:2d8d828ff276 | 900 | linear1 =! linear1; //Coil 2 |
| pedlerw | 0:2d8d828ff276 | 901 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 902 | linear1 =! linear1; |
| pedlerw | 0:2d8d828ff276 | 903 | |
| pedlerw | 0:2d8d828ff276 | 904 | return; |
| pedlerw | 0:2d8d828ff276 | 905 | }//End of Funciton |
| pedlerw | 0:2d8d828ff276 | 906 | |
| pedlerw | 0:2d8d828ff276 | 907 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 908 | ////////////////////////////move_all_rightBobby//////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 909 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 910 | //This function accepts an interger value and moves the forcer to the right// |
| pedlerw | 0:2d8d828ff276 | 911 | //The interger number of coil sets// |
| pedlerw | 0:2d8d828ff276 | 912 | void move_all_rightBobby(int set){ |
| pedlerw | 0:2d8d828ff276 | 913 | //Declartions and initialization// |
| pedlerw | 0:2d8d828ff276 | 914 | linear0 = 1; //Coil1 Mod0 |
| pedlerw | 0:2d8d828ff276 | 915 | linear1 = 1; //Coil2 Mod1 |
| pedlerw | 0:2d8d828ff276 | 916 | linear2 = 1; //Coil3 Mod2 |
| pedlerw | 0:2d8d828ff276 | 917 | linear3 = 1; //Coil4 Mod3 |
| pedlerw | 0:2d8d828ff276 | 918 | linear4 = 1; //Coil5 Mod4 |
| pedlerw | 0:2d8d828ff276 | 919 | linear5 = 1; //Coil6 Mod5 |
| pedlerw | 0:2d8d828ff276 | 920 | linear6 = 1; //Coil7 Mod6 |
| pedlerw | 0:2d8d828ff276 | 921 | |
| pedlerw | 0:2d8d828ff276 | 922 | //RIGHT// |
| pedlerw | 0:2d8d828ff276 | 923 | for(int i=0;i<set;i++){ |
| pedlerw | 1:99f06e7e6906 | 924 | linear0 =! linear0; //Coil 1 |
| pedlerw | 1:99f06e7e6906 | 925 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 926 | linear0 =! linear0; |
| pedlerw | 1:99f06e7e6906 | 927 | linear6 =! linear6; //Coil 7 |
| pedlerw | 1:99f06e7e6906 | 928 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 929 | linear6 =! linear6; |
| pedlerw | 1:99f06e7e6906 | 930 | linear5 =! linear5; //Coil 6 |
| pedlerw | 1:99f06e7e6906 | 931 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 932 | linear5 =! linear5; |
| pedlerw | 1:99f06e7e6906 | 933 | linear4 =! linear4; //Coil 5 |
| pedlerw | 1:99f06e7e6906 | 934 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 935 | linear4 =! linear4; |
| pedlerw | 1:99f06e7e6906 | 936 | linear3 =! linear3; //Coil 4 |
| pedlerw | 1:99f06e7e6906 | 937 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 938 | linear3 =! linear3; |
| pedlerw | 1:99f06e7e6906 | 939 | linear2 =! linear2; //Coil 3 |
| pedlerw | 1:99f06e7e6906 | 940 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 941 | linear2 =! linear2; |
| pedlerw | 1:99f06e7e6906 | 942 | linear1 =! linear1; //Coil 2 |
| pedlerw | 1:99f06e7e6906 | 943 | wait(fast); |
| pedlerw | 1:99f06e7e6906 | 944 | linear1 =! linear1; |
| pedlerw | 0:2d8d828ff276 | 945 | }//End of For loop |
| pedlerw | 0:2d8d828ff276 | 946 | |
| pedlerw | 0:2d8d828ff276 | 947 | linear0 =! linear0; //Coil 1 |
| pedlerw | 0:2d8d828ff276 | 948 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 949 | linear0 =! linear0; |
| pedlerw | 0:2d8d828ff276 | 950 | linear6 =! linear6; //Coil 7 |
| pedlerw | 0:2d8d828ff276 | 951 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 952 | linear6 =! linear6; |
| pedlerw | 0:2d8d828ff276 | 953 | linear5 =! linear5; //Coil 6 |
| pedlerw | 0:2d8d828ff276 | 954 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 955 | linear5 =! linear5; |
| pedlerw | 0:2d8d828ff276 | 956 | linear4 =! linear4; //Coil 5 |
| pedlerw | 0:2d8d828ff276 | 957 | wait(fast); |
| pedlerw | 0:2d8d828ff276 | 958 | linear4 =! linear4; |
| pedlerw | 0:2d8d828ff276 | 959 | |
| pedlerw | 0:2d8d828ff276 | 960 | return; |
| pedlerw | 0:2d8d828ff276 | 961 | }//End of Funciton |
| pedlerw | 0:2d8d828ff276 | 962 | |
| pedlerw | 0:2d8d828ff276 | 963 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 964 | //////////////////////////////scroll_up()////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 965 | /////////////////////////////////////////////////////////////////////////////// |
| pedlerw | 0:2d8d828ff276 | 966 | //Function accepts an integer and scrolls the text on the serial terminal that |
| pedlerw | 0:2d8d828ff276 | 967 | //Number of spaces// |
| pedlerw | 0:2d8d828ff276 | 968 | void scroll_up(int space){ |
| pedlerw | 0:2d8d828ff276 | 969 | for(int i=0;i<space;i++){ |
| pedlerw | 0:2d8d828ff276 | 970 | xbee.printf("\r\n"); |
| pedlerw | 0:2d8d828ff276 | 971 | }//End of for |
| pedlerw | 0:2d8d828ff276 | 972 | }//End of function |