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@0:2d8d828ff276, 2013-04-16 (annotated)

Committer:
pedlerw
Date:
Tue Apr 16 19:12:05 2013 +0000
Revision:
0:2d8d828ff276
Child:
1:99f06e7e6906
4/16/2013; William Pedler; Code compiles... user interface works... yeah.

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 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 0:2d8d828ff276 78 void scroll_up(int line);
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 0:2d8d828ff276 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 0:2d8d828ff276 178 scroll_up(30);
pedlerw 0:2d8d828ff276 179 xbee.printf("\r\n\n****Main Menu****");
pedlerw 0:2d8d828ff276 180 xbee.printf("\r\nFor Calibration press 1 and <Enter>.");
pedlerw 0:2d8d828ff276 181 xbee.printf("\r\nTo enter a desired location press 2 and <Enter>.");
pedlerw 0:2d8d828ff276 182 xbee.printf("\r\nFor display mode press 3 and <Enter>.");
pedlerw 0:2d8d828ff276 183 scroll_up(24);
pedlerw 0:2d8d828ff276 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 0:2d8d828ff276 211 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 212 wait(fast);
pedlerw 0:2d8d828ff276 213 linear4 =! linear4;
pedlerw 0:2d8d828ff276 214 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 215 wait(fast);
pedlerw 0:2d8d828ff276 216 linear5 =! linear5;
pedlerw 0:2d8d828ff276 217 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 218 wait(fast);
pedlerw 0:2d8d828ff276 219 linear6 =! linear6;
pedlerw 0:2d8d828ff276 220 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 221 wait(fast);
pedlerw 0:2d8d828ff276 222 linear0 =! linear0;
pedlerw 0:2d8d828ff276 223 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 224 wait(fast);
pedlerw 0:2d8d828ff276 225 linear1 =! linear1;
pedlerw 0:2d8d828ff276 226 linear2 =! linear2; //Coil 3
pedlerw 0:2d8d828ff276 227 wait(fast);
pedlerw 0:2d8d828ff276 228 linear2 =! linear2;
pedlerw 0:2d8d828ff276 229 linear3 =! linear3; //Coil 4
pedlerw 0:2d8d828ff276 230 wait(fast);
pedlerw 0:2d8d828ff276 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 0:2d8d828ff276 253 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 254 wait(fast);
pedlerw 0:2d8d828ff276 255 linear0 =! linear0;
pedlerw 0:2d8d828ff276 256 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 257 wait(fast);
pedlerw 0:2d8d828ff276 258 linear6 =! linear6;
pedlerw 0:2d8d828ff276 259 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 260 wait(fast);
pedlerw 0:2d8d828ff276 261 linear5 =! linear5;
pedlerw 0:2d8d828ff276 262 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 263 wait(fast);
pedlerw 0:2d8d828ff276 264 linear4 =! linear4;
pedlerw 0:2d8d828ff276 265 linear3 =! linear3; //Coil 4
pedlerw 0:2d8d828ff276 266 wait(fast);
pedlerw 0:2d8d828ff276 267 linear3 =! linear3;
pedlerw 0:2d8d828ff276 268 linear2 =! linear2; //Coil 3
pedlerw 0:2d8d828ff276 269 wait(fast);
pedlerw 0:2d8d828ff276 270 linear2 =! linear2;
pedlerw 0:2d8d828ff276 271 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 272 wait(fast);
pedlerw 0:2d8d828ff276 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 0:2d8d828ff276 601 void calibrate_linear(){
pedlerw 0:2d8d828ff276 602 //Speed Control//
pedlerw 0:2d8d828ff276 603 wait(three);
pedlerw 0:2d8d828ff276 604
pedlerw 0:2d8d828ff276 605 //1.Move all the way to the LEFT side of the track//
pedlerw 0:2d8d828ff276 606 move_all_left(six);
pedlerw 0:2d8d828ff276 607 //2.Get Sensor Data//
pedlerw 0:2d8d828ff276 608 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 609 //3.Store in various arrays
pedlerw 0:2d8d828ff276 610 wait(slow);
pedlerw 0:2d8d828ff276 611 left_min[i] = get_Lsensor_min();
pedlerw 0:2d8d828ff276 612 left_max[i] = get_Lsensor_max();
pedlerw 0:2d8d828ff276 613 right_min[i] = get_Rsensor_min();
pedlerw 0:2d8d828ff276 614 right_max[i] = get_Rsensor_max();
pedlerw 0:2d8d828ff276 615 led4 = !led4;
pedlerw 0:2d8d828ff276 616 //4.Move to the right one//
pedlerw 0:2d8d828ff276 617 move_one_right(i);
pedlerw 0:2d8d828ff276 618 }//End of for
pedlerw 0:2d8d828ff276 619
pedlerw 0:2d8d828ff276 620 //5. Export data to a txt file on the mbed//
pedlerw 0:2d8d828ff276 621 FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing
pedlerw 0:2d8d828ff276 622 fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n");
pedlerw 0:2d8d828ff276 623 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 624 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 625 /*
pedlerw 0:2d8d828ff276 626 //Example File Handeling Code//
pedlerw 0:2d8d828ff276 627 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
pedlerw 0:2d8d828ff276 628 fprintf(fp, "Hello World!");
pedlerw 0:2d8d828ff276 629 fclose(fp);
pedlerw 0:2d8d828ff276 630 */
pedlerw 0:2d8d828ff276 631 }//End of for
pedlerw 0:2d8d828ff276 632 fclose(fp);
pedlerw 0:2d8d828ff276 633 //Export each variable to its own file//
pedlerw 0:2d8d828ff276 634 //Write leftmin.txt file//
pedlerw 0:2d8d828ff276 635 FILE *fp1 = fopen("/local/leftmin.txt", "w");
pedlerw 0:2d8d828ff276 636 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 637 fprintf(fp1,"%f\r\n",left_min[i]);
pedlerw 0:2d8d828ff276 638 }//End of for//
pedlerw 0:2d8d828ff276 639 fclose(fp1);
pedlerw 0:2d8d828ff276 640 //Write leftmax.txt file//
pedlerw 0:2d8d828ff276 641 FILE *fp2 = fopen("/local/leftmax.txt", "w");
pedlerw 0:2d8d828ff276 642 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 643 fprintf(fp2,"%f\r\n",left_max[i]);
pedlerw 0:2d8d828ff276 644 }//End of for//
pedlerw 0:2d8d828ff276 645 fclose(fp2);
pedlerw 0:2d8d828ff276 646 //Write rightmin.txt file//
pedlerw 0:2d8d828ff276 647 FILE *fp3 = fopen("/local/rightmin.txt", "w");
pedlerw 0:2d8d828ff276 648 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 649 fprintf(fp3,"%f\r\n",right_min[i]);
pedlerw 0:2d8d828ff276 650 }//End of for//
pedlerw 0:2d8d828ff276 651 fclose(fp3);
pedlerw 0:2d8d828ff276 652 //Write rightmax.txt file//
pedlerw 0:2d8d828ff276 653 FILE *fp4 = fopen("/local/rightmax.txt", "w");
pedlerw 0:2d8d828ff276 654 for(int i=0;i<track_length;i++){
pedlerw 0:2d8d828ff276 655 fprintf(fp4,"%f\r\n",right_max[i]);
pedlerw 0:2d8d828ff276 656 }//End of for//
pedlerw 0:2d8d828ff276 657 fclose(fp4);
pedlerw 0:2d8d828ff276 658
pedlerw 0:2d8d828ff276 659 //Set an LED high so we know calibration is complete//
pedlerw 0:2d8d828ff276 660 led1 = 1;
pedlerw 0:2d8d828ff276 661 move_all_left(six);
pedlerw 0:2d8d828ff276 662 }//End of function
pedlerw 0:2d8d828ff276 663
pedlerw 0:2d8d828ff276 664 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 665 ////////////////////////////read_file//////////////////////////////////////////
pedlerw 0:2d8d828ff276 666 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 667 //read_file reads the text files containing calibration data
pedlerw 0:2d8d828ff276 668 void read_file(){
pedlerw 0:2d8d828ff276 669 char line[80];
pedlerw 0:2d8d828ff276 670 int i = 0;
pedlerw 0:2d8d828ff276 671 FILE *fr1;
pedlerw 0:2d8d828ff276 672 fr1 = fopen ("/local/leftmin.txt", "rt");
pedlerw 0:2d8d828ff276 673 while(fgets(line, 80, fr1) != NULL){
pedlerw 0:2d8d828ff276 674 sscanf (line, "%f", &left_min[i]);
pedlerw 0:2d8d828ff276 675 i++;
pedlerw 0:2d8d828ff276 676 }//End of while
pedlerw 0:2d8d828ff276 677 fclose(fr1);
pedlerw 0:2d8d828ff276 678
pedlerw 0:2d8d828ff276 679 i = 0;
pedlerw 0:2d8d828ff276 680 FILE *fr2;
pedlerw 0:2d8d828ff276 681 fr2 = fopen ("/local/leftmax.txt", "rt");
pedlerw 0:2d8d828ff276 682 while(fgets(line, 80, fr2) != NULL){
pedlerw 0:2d8d828ff276 683 sscanf (line, "%f", &left_max[i]);
pedlerw 0:2d8d828ff276 684 i++;
pedlerw 0:2d8d828ff276 685 }//End of while
pedlerw 0:2d8d828ff276 686 fclose(fr2);
pedlerw 0:2d8d828ff276 687
pedlerw 0:2d8d828ff276 688 i = 0;
pedlerw 0:2d8d828ff276 689 FILE *fr3;
pedlerw 0:2d8d828ff276 690 fr3 = fopen ("/local/rightmin.txt", "rt");
pedlerw 0:2d8d828ff276 691 while(fgets(line, 80, fr3) != NULL){
pedlerw 0:2d8d828ff276 692 sscanf (line, "%f", &right_min[i]);
pedlerw 0:2d8d828ff276 693 i++;
pedlerw 0:2d8d828ff276 694 }//End of while
pedlerw 0:2d8d828ff276 695 fclose(fr3);
pedlerw 0:2d8d828ff276 696
pedlerw 0:2d8d828ff276 697 i = 0;
pedlerw 0:2d8d828ff276 698 FILE *fr4;
pedlerw 0:2d8d828ff276 699 fr4 = fopen ("/local/rightmax.txt", "rt");
pedlerw 0:2d8d828ff276 700 while(fgets(line, 80, fr4) != NULL){
pedlerw 0:2d8d828ff276 701 sscanf (line, "%f", &right_max[i]);
pedlerw 0:2d8d828ff276 702 i++;
pedlerw 0:2d8d828ff276 703 }//End of while
pedlerw 0:2d8d828ff276 704 fclose(fr4);
pedlerw 0:2d8d828ff276 705 }//End of function
pedlerw 0:2d8d828ff276 706
pedlerw 0:2d8d828ff276 707 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 708 //////////////////////////////move_linear//////////////////////////////////////
pedlerw 0:2d8d828ff276 709 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 710 //move_linear accepts a desired locaiton and moves the forcer accordingly//
pedlerw 0:2d8d828ff276 711 //Returns 4 on normal exit... prompting the user for a new position//
pedlerw 0:2d8d828ff276 712 int move_linear(int desired_location){
pedlerw 0:2d8d828ff276 713 int current_location = get_position();
pedlerw 0:2d8d828ff276 714 int difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 715 int count = 0;
pedlerw 0:2d8d828ff276 716 xbee.printf("\r\nInside move_linear function");
pedlerw 0:2d8d828ff276 717 xbee.printf("\r\nCurrent_location %d", current_location);
pedlerw 0:2d8d828ff276 718 if(desired_location == -1){
pedlerw 0:2d8d828ff276 719 return -1;
pedlerw 0:2d8d828ff276 720 }//End of if
pedlerw 0:2d8d828ff276 721 while(current_location > desired_location || current_location < desired_location){
pedlerw 0:2d8d828ff276 722 difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 723 xbee.printf("\r\nDifference %d",difference);
pedlerw 0:2d8d828ff276 724 while(difference > 0){
pedlerw 0:2d8d828ff276 725 if(count > five){
pedlerw 0:2d8d828ff276 726 move_one_left(--current_location);
pedlerw 0:2d8d828ff276 727 }//End of if//
pedlerw 0:2d8d828ff276 728 //Move LEFT//
pedlerw 0:2d8d828ff276 729 xbee.printf("\r\nMoving Left");
pedlerw 0:2d8d828ff276 730 led2 = 1;
pedlerw 0:2d8d828ff276 731 wait(medium);
pedlerw 0:2d8d828ff276 732 for(int i=0; i<difference+two;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 0:2d8d828ff276 737 current_location = get_position();
pedlerw 0:2d8d828ff276 738 difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 739 led2 = 0;
pedlerw 0:2d8d828ff276 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 0:2d8d828ff276 747 if(count > five){
pedlerw 0:2d8d828ff276 748 move_one_right(++current_location);
pedlerw 0:2d8d828ff276 749 }//End of if//
pedlerw 0:2d8d828ff276 750 //Move RIGHT//
pedlerw 0:2d8d828ff276 751 xbee.printf("\r\nMoving Right");
pedlerw 0:2d8d828ff276 752 led3 = 1;
pedlerw 0:2d8d828ff276 753 wait(medium);
pedlerw 0:2d8d828ff276 754 for(int i=0; i<(abs(difference-three));i++){
pedlerw 0:2d8d828ff276 755 move_one_right(current_location);
pedlerw 0:2d8d828ff276 756 current_location++;
pedlerw 0:2d8d828ff276 757 wait(fast);
pedlerw 0:2d8d828ff276 758 }//End of for
pedlerw 0:2d8d828ff276 759 current_location = get_position();
pedlerw 0:2d8d828ff276 760 difference = current_location - desired_location;
pedlerw 0:2d8d828ff276 761 led3 = 0;
pedlerw 0:2d8d828ff276 762 count++;
pedlerw 0:2d8d828ff276 763 if(current_location == -2){
pedlerw 0:2d8d828ff276 764 return(four);
pedlerw 0:2d8d828ff276 765 }//End of if//
pedlerw 0:2d8d828ff276 766 }//End of while(difference less than 0)//
pedlerw 0:2d8d828ff276 767 }//End of while(current not desired)//
pedlerw 0:2d8d828ff276 768 return(four);
pedlerw 0:2d8d828ff276 769 }//End of function//
pedlerw 0:2d8d828ff276 770
pedlerw 0:2d8d828ff276 771 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 772 ////////////////////////////error_check////////////////////////////////////////
pedlerw 0:2d8d828ff276 773 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 774 //Function does scanf error checking//
pedlerw 0:2d8d828ff276 775 //It clears the keyboard buffer incase anything other than numbers a pressed//
pedlerw 0:2d8d828ff276 776 int error_check(){
pedlerw 0:2d8d828ff276 777 char not_number;
pedlerw 0:2d8d828ff276 778 int number;
pedlerw 0:2d8d828ff276 779 int flag;
pedlerw 0:2d8d828ff276 780 while ((flag = xbee.scanf("%d", &number)) != EOF){
pedlerw 0:2d8d828ff276 781 if (flag != 1){
pedlerw 0:2d8d828ff276 782 not_number = xbee.getc();
pedlerw 0:2d8d828ff276 783 #ifdef DEBUG
pedlerw 0:2d8d828ff276 784 xbee.printf("\r\ndegub:%c in input stream, discarding", not_number);
pedlerw 0:2d8d828ff276 785 #endif
pedlerw 0:2d8d828ff276 786 }//End of if
pedlerw 0:2d8d828ff276 787 else {
pedlerw 0:2d8d828ff276 788 printf("\r\nRETURN = %d",number);
pedlerw 0:2d8d828ff276 789 return(number);
pedlerw 0:2d8d828ff276 790 }//End of else//
pedlerw 0:2d8d828ff276 791 }//End of while((flag...
pedlerw 0:2d8d828ff276 792 return(number);
pedlerw 0:2d8d828ff276 793 }//End of function//
pedlerw 0:2d8d828ff276 794
pedlerw 0:2d8d828ff276 795 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 796 ////////////////////////////////display_mode///////////////////////////////////
pedlerw 0:2d8d828ff276 797 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 798 //Function set up to run display mode options//
pedlerw 0:2d8d828ff276 799 void display_mode(){
pedlerw 0:2d8d828ff276 800 int option = -1;
pedlerw 0:2d8d828ff276 801 int exit = 0;
pedlerw 0:2d8d828ff276 802 while(exit != one){
pedlerw 0:2d8d828ff276 803 switch (option){
pedlerw 0:2d8d828ff276 804 case 1:
pedlerw 0:2d8d828ff276 805 //Wind sprints//
pedlerw 0:2d8d828ff276 806 for(int j=0;j<large_num;j++){
pedlerw 0:2d8d828ff276 807 for(int i=0;i<12;i++){
pedlerw 0:2d8d828ff276 808 move_all_left(abs(six-i));
pedlerw 0:2d8d828ff276 809 move_all_right(abs(six-i));
pedlerw 0:2d8d828ff276 810 }//End of for//
pedlerw 0:2d8d828ff276 811 }//End of for//
pedlerw 0:2d8d828ff276 812 break;
pedlerw 0:2d8d828ff276 813 case 2:
pedlerw 0:2d8d828ff276 814 //Sucide sprints//
pedlerw 0:2d8d828ff276 815 for(int j=0;j<large_num;j++){
pedlerw 0:2d8d828ff276 816 for(int i=0;i<12;i++){
pedlerw 0:2d8d828ff276 817 move_all_leftBobby(abs(six-i));
pedlerw 0:2d8d828ff276 818 move_all_rightBobby(abs(six-i));
pedlerw 0:2d8d828ff276 819 }//End of for//
pedlerw 0:2d8d828ff276 820 }//End of for//
pedlerw 0:2d8d828ff276 821 break;
pedlerw 0:2d8d828ff276 822 case 3:
pedlerw 0:2d8d828ff276 823 //Option 3//
pedlerw 0:2d8d828ff276 824
pedlerw 0:2d8d828ff276 825 break;
pedlerw 0:2d8d828ff276 826 case 4:
pedlerw 0:2d8d828ff276 827 //Return to main menu//
pedlerw 0:2d8d828ff276 828 exit = one;
pedlerw 0:2d8d828ff276 829 //break;
pedlerw 0:2d8d828ff276 830 default:
pedlerw 0:2d8d828ff276 831 scroll_up(30);
pedlerw 0:2d8d828ff276 832 xbee.printf("\r\n\n****Display Mode****");
pedlerw 0:2d8d828ff276 833 xbee.printf("\r\nTo run wind sprints press 1 and <Enter>.");
pedlerw 0:2d8d828ff276 834 xbee.printf("\r\nTo run suicide sprints press 2 and <Enter>.");
pedlerw 0:2d8d828ff276 835 xbee.printf("\r\nTo run option 3 press 3 and <Enter>.");
pedlerw 0:2d8d828ff276 836 xbee.printf("\r\nTo return to the previous menu press 4 and <Enter>.");
pedlerw 0:2d8d828ff276 837 scroll_up(25);
pedlerw 0:2d8d828ff276 838 option = error_check();
pedlerw 0:2d8d828ff276 839 break;
pedlerw 0:2d8d828ff276 840 }//End of switch//
pedlerw 0:2d8d828ff276 841 }//End of while//
pedlerw 0:2d8d828ff276 842 return;
pedlerw 0:2d8d828ff276 843 }//End of function//
pedlerw 0:2d8d828ff276 844
pedlerw 0:2d8d828ff276 845 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 846 ////////////////////////////move_all_leftBobby/////////////////////////////////
pedlerw 0:2d8d828ff276 847 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 848 //This function accepts an interger value and moves the forcer to the left//
pedlerw 0:2d8d828ff276 849 //The interger number of coil sets//
pedlerw 0:2d8d828ff276 850 void move_all_leftBobby(int set){
pedlerw 0:2d8d828ff276 851 //Declartions and initialization//
pedlerw 0:2d8d828ff276 852 linear0 = 1; //Coil1 Mod0
pedlerw 0:2d8d828ff276 853 linear1 = 1; //Coil2 Mod1
pedlerw 0:2d8d828ff276 854 linear2 = 1; //Coil3 Mod2
pedlerw 0:2d8d828ff276 855 linear3 = 1; //Coil4 Mod3
pedlerw 0:2d8d828ff276 856 linear4 = 1; //Coil5 Mod4
pedlerw 0:2d8d828ff276 857 linear5 = 1; //Coil6 Mod5
pedlerw 0:2d8d828ff276 858 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 859
pedlerw 0:2d8d828ff276 860 //LEFT//
pedlerw 0:2d8d828ff276 861 for(int i=0;i<set;i++){
pedlerw 0:2d8d828ff276 862 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 863 wait(fast);
pedlerw 0:2d8d828ff276 864 linear4 =! linear4;
pedlerw 0:2d8d828ff276 865 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 866 wait(fast);
pedlerw 0:2d8d828ff276 867 linear5 =! linear5;
pedlerw 0:2d8d828ff276 868 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 869 wait(fast);
pedlerw 0:2d8d828ff276 870 linear6 =! linear6;
pedlerw 0:2d8d828ff276 871 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 872 wait(fast);
pedlerw 0:2d8d828ff276 873 linear0 =! linear0;
pedlerw 0:2d8d828ff276 874 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 875 wait(fast);
pedlerw 0:2d8d828ff276 876 linear1 =! linear1;
pedlerw 0:2d8d828ff276 877 linear2 =! linear2; //Coil 3
pedlerw 0:2d8d828ff276 878 wait(fast);
pedlerw 0:2d8d828ff276 879 linear2 =! linear2;
pedlerw 0:2d8d828ff276 880 linear3 =! linear3; //Coil 4
pedlerw 0:2d8d828ff276 881 wait(fast);
pedlerw 0:2d8d828ff276 882 linear3 =! linear3;
pedlerw 0:2d8d828ff276 883 }//End of For loop
pedlerw 0:2d8d828ff276 884
pedlerw 0:2d8d828ff276 885 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 886 wait(fast);
pedlerw 0:2d8d828ff276 887 linear4 =! linear4;
pedlerw 0:2d8d828ff276 888 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 889 wait(fast);
pedlerw 0:2d8d828ff276 890 linear5 =! linear5;
pedlerw 0:2d8d828ff276 891 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 892 wait(fast);
pedlerw 0:2d8d828ff276 893 linear6 =! linear6;
pedlerw 0:2d8d828ff276 894 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 895 wait(fast);
pedlerw 0:2d8d828ff276 896 linear0 =! linear0;
pedlerw 0:2d8d828ff276 897 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 898 wait(fast);
pedlerw 0:2d8d828ff276 899 linear1 =! linear1;
pedlerw 0:2d8d828ff276 900
pedlerw 0:2d8d828ff276 901 return;
pedlerw 0:2d8d828ff276 902 }//End of Funciton
pedlerw 0:2d8d828ff276 903
pedlerw 0:2d8d828ff276 904 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 905 ////////////////////////////move_all_rightBobby////////////////////////////////
pedlerw 0:2d8d828ff276 906 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 907 //This function accepts an interger value and moves the forcer to the right//
pedlerw 0:2d8d828ff276 908 //The interger number of coil sets//
pedlerw 0:2d8d828ff276 909 void move_all_rightBobby(int set){
pedlerw 0:2d8d828ff276 910 //Declartions and initialization//
pedlerw 0:2d8d828ff276 911 linear0 = 1; //Coil1 Mod0
pedlerw 0:2d8d828ff276 912 linear1 = 1; //Coil2 Mod1
pedlerw 0:2d8d828ff276 913 linear2 = 1; //Coil3 Mod2
pedlerw 0:2d8d828ff276 914 linear3 = 1; //Coil4 Mod3
pedlerw 0:2d8d828ff276 915 linear4 = 1; //Coil5 Mod4
pedlerw 0:2d8d828ff276 916 linear5 = 1; //Coil6 Mod5
pedlerw 0:2d8d828ff276 917 linear6 = 1; //Coil7 Mod6
pedlerw 0:2d8d828ff276 918
pedlerw 0:2d8d828ff276 919 //RIGHT//
pedlerw 0:2d8d828ff276 920 for(int i=0;i<set;i++){
pedlerw 0:2d8d828ff276 921 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 922 wait(fast);
pedlerw 0:2d8d828ff276 923 linear0 =! linear0;
pedlerw 0:2d8d828ff276 924 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 925 wait(fast);
pedlerw 0:2d8d828ff276 926 linear6 =! linear6;
pedlerw 0:2d8d828ff276 927 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 928 wait(fast);
pedlerw 0:2d8d828ff276 929 linear5 =! linear5;
pedlerw 0:2d8d828ff276 930 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 931 wait(fast);
pedlerw 0:2d8d828ff276 932 linear4 =! linear4;
pedlerw 0:2d8d828ff276 933 linear3 =! linear3; //Coil 4
pedlerw 0:2d8d828ff276 934 wait(fast);
pedlerw 0:2d8d828ff276 935 linear3 =! linear3;
pedlerw 0:2d8d828ff276 936 linear2 =! linear2; //Coil 3
pedlerw 0:2d8d828ff276 937 wait(fast);
pedlerw 0:2d8d828ff276 938 linear2 =! linear2;
pedlerw 0:2d8d828ff276 939 linear1 =! linear1; //Coil 2
pedlerw 0:2d8d828ff276 940 wait(fast);
pedlerw 0:2d8d828ff276 941 linear1 =! linear1;
pedlerw 0:2d8d828ff276 942 }//End of For loop
pedlerw 0:2d8d828ff276 943
pedlerw 0:2d8d828ff276 944 linear0 =! linear0; //Coil 1
pedlerw 0:2d8d828ff276 945 wait(fast);
pedlerw 0:2d8d828ff276 946 linear0 =! linear0;
pedlerw 0:2d8d828ff276 947 linear6 =! linear6; //Coil 7
pedlerw 0:2d8d828ff276 948 wait(fast);
pedlerw 0:2d8d828ff276 949 linear6 =! linear6;
pedlerw 0:2d8d828ff276 950 linear5 =! linear5; //Coil 6
pedlerw 0:2d8d828ff276 951 wait(fast);
pedlerw 0:2d8d828ff276 952 linear5 =! linear5;
pedlerw 0:2d8d828ff276 953 linear4 =! linear4; //Coil 5
pedlerw 0:2d8d828ff276 954 wait(fast);
pedlerw 0:2d8d828ff276 955 linear4 =! linear4;
pedlerw 0:2d8d828ff276 956
pedlerw 0:2d8d828ff276 957 return;
pedlerw 0:2d8d828ff276 958 }//End of Funciton
pedlerw 0:2d8d828ff276 959
pedlerw 0:2d8d828ff276 960 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 961 //////////////////////////////scroll_up()//////////////////////////////////////
pedlerw 0:2d8d828ff276 962 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:2d8d828ff276 963 //Function accepts an integer and scrolls the text on the serial terminal that
pedlerw 0:2d8d828ff276 964 //Number of spaces//
pedlerw 0:2d8d828ff276 965 void scroll_up(int space){
pedlerw 0:2d8d828ff276 966 for(int i=0;i<space;i++){
pedlerw 0:2d8d828ff276 967 xbee.printf("\r\n");
pedlerw 0:2d8d828ff276 968 }//End of for
pedlerw 0:2d8d828ff276 969 }//End of function