William Pedler
2013 VCU Senior Design Expo Linear Rotary Motor Control Program. This is the actual program used by the team at the Expo.
Diff: main.cpp
- Revision:
- 0:2d8d828ff276
- Child:
- 1:99f06e7e6906
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 16 19:12:05 2013 +0000
@@ -0,0 +1,969 @@
+//SeniorDesign_ExpoPro//
+//Written by William Pedler 4/15/2013//
+//This is the program the 2013 LRM team is using at the Senior Design Show Case//
+//This program uses two xbee s1 for serial interface//
+
+//Includes//
+#include "mbed.h"
+using namespace std;
+
+//Define//
+#define DEBUG
+#define MAX 1.0
+#define MIN 0.0
+#define large_num 500 //This times 4 controls how long the sensors read for each position//
+#define one 1
+#define two 2
+#define three 3.0
+#define four 4
+#define five 5
+#define six 6
+#define seven 7
+#define eight 8
+#define nine 9
+#define track_length 42
+#define fast 0.125
+#define medium 1.0
+#define slow 2.0
+#define sensor_set 0.005
+
+//Declar global variables//
+float left_min[track_length] = {0};
+float left_max[track_length] = {0};
+float right_min[track_length] = {0};
+float right_max[track_length] = {0};
+
+//Labeling the Local file system//
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+//Labeling onboard LED's//
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+//Label linear coils//
+DigitalOut linear0(p5); //coil 1
+DigitalOut linear1(p6); //coil 2
+DigitalOut linear2(p7); //coil 3
+DigitalOut linear3(p8); //coil 4
+DigitalOut linear4(p9); //coil 5
+DigitalOut linear5(p10); //coil 6
+DigitalOut linear6(p11); //coil 7
+//Reduce noise by declaring all unused Analog pins as DigitalOut//
+DigitalOut left15(p15);
+DigitalOut left16(p16);
+DigitalOut left17(p17);
+DigitalOut left18(p18);
+AnalogIn distanceR(p19); //Vout yellow GND black Vss red
+AnalogIn distanceL(p20); //Vout yellow GND black Vss red
+//Set serial com//
+Serial xbee(p13,p14);
+
+//Function Prototypes//
+void move_all_left(int num); //num for # of coil sets left//
+void move_all_right(int num); //num for # of coil sets right//
+float get_Lsensor_min();
+float get_Lsensor_max();
+float get_Rsensor_min();
+float get_Rsensor_max();
+void move_one_right(int p); //p for position//
+void move_one_left(int p); //p for position//
+int get_position();
+void calibrate_linear();
+void read_file();
+int move_linear(int l); //l for location//
+int error_check();
+void display_mode();
+void move_all_leftBobby(int num);
+void move_all_rightBobby(int num);
+void scroll_up(int line);
+
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////Main Program//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+/*//////////////////////Pseudo code for Main://////////////////////////////////
+-Prompt user with 2 options-
+1.Calibrate Motor
+ a.Call calibration function... write new data to local files
+2.Use old calibration file
+ a.Read in files
+ b.Prompt for desired location
+-Operate Motor
+1. Get current location
+2. Get desired location
+3. Move to location
+4. Repeat
+*//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////MAIN/////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+int main() {
+ //Declarations & Initializations//
+ int desired_location = 0;
+ int user_instruction = -1;
+
+ //Initialize Linear Coils//
+ linear0 = 1;
+ linear1 = 1;
+ linear2 = 1;
+ linear3 = 1;
+ linear4 = 1;
+ linear5 = 1;
+ linear6 = 1;
+ //Initialize onbord led//
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////New User Interface//////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+ while(1){
+ //-Prompt user with 2 options-
+ switch(user_instruction){
+ case 1:
+ //Calibrate Motor//
+ calibrate_linear();
+ //Go to operate motor//
+ user_instruction = 4; //Go to operation menu//
+ break;
+ case 2:
+ //Use old calibration file and begin operation
+ //Read file to fill in max and min//
+ read_file();
+ user_instruction = 4; //Go to operation menu//
+ break;
+ case 3:
+ display_mode();
+ user_instruction = -1; //Go to main menu//
+ break;
+ case 4:
+ //Operate Linear Part//
+ for(int i=0;i<30;i++){
+ xbee.printf("\r\n");
+ }//End of for
+ scroll_up(30);
+ xbee.printf("\r\n\n****Operation Menu****");
+ xbee.printf("\r\nEnter a position number from 0 to 40: ");
+ xbee.printf("\r\nTo get back to the Main Menu press -1 and <Enter>.");
+ scroll_up(25);
+ desired_location = error_check();
+ if(desired_location == -1){
+ user_instruction = move_linear(desired_location);
+ }//End of if
+ else if(desired_location > 40){
+ user_instruction = 4;
+ xbee.printf("\r\nUser input was out of bounds");
+ xbee.printf("\r\nPlease try again");
+ scroll_up(20);
+ wait(slow);
+ break;
+ }//End of else if
+ else if(desired_location < 0){
+ user_instruction = 4;
+ xbee.printf("\r\nUser input was out of bounds");
+ xbee.printf("\r\nPlease try again");
+ scroll_up(20);
+ wait(slow);
+ break;
+ }//End of else if
+ xbee.printf("\r\nGoing to position %d", desired_location);
+ //Pass desired_location to move_linear();
+ //On normal exit move_linear returns 4... causes repeat of operation menu//
+ user_instruction = move_linear(desired_location);
+ break;
+ default:
+ //Get valid user_input//
+ scroll_up(30);
+ xbee.printf("\r\n\n****Main Menu****");
+ xbee.printf("\r\nFor Calibration press 1 and <Enter>.");
+ xbee.printf("\r\nTo enter a desired location press 2 and <Enter>.");
+ xbee.printf("\r\nFor display mode press 3 and <Enter>.");
+ scroll_up(24);
+ user_instruction = error_check();
+ break;
+ }//End of switch
+ }//End of while(1)//
+}//End of main
+/*******************************************************************************
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////FUNCITONS/////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+*******************************************************************************/
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////move_all_left//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//This function accepts an interger value and moves the forcer to the left//
+//The interger number of coil sets//
+void move_all_left(int set){
+//Declartions and initialization//
+linear0 = 1; //Coil1 Mod0
+linear1 = 1; //Coil2 Mod1
+linear2 = 1; //Coil3 Mod2
+linear3 = 1; //Coil4 Mod3
+linear4 = 1; //Coil5 Mod4
+linear5 = 1; //Coil6 Mod5
+linear6 = 1; //Coil7 Mod6
+
+ //LEFT//
+ for(int i=0;i<set;i++){
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear1 =! linear1; //Coil 2
+ wait(fast);
+ linear1 =! linear1;
+ linear2 =! linear2; //Coil 3
+ wait(fast);
+ linear2 =! linear2;
+ linear3 =! linear3; //Coil 4
+ wait(fast);
+ linear3 =! linear3;
+ }//End of For loop
+ return;
+}//End of Funciton
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////move_all_right/////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//This function accepts an interger value and moves the forcer to the right//
+//The interger number of coil sets//
+void move_all_right(int set){
+//Declartions and initialization//
+linear0 = 1; //Coil1 Mod0
+linear1 = 1; //Coil2 Mod1
+linear2 = 1; //Coil3 Mod2
+linear3 = 1; //Coil4 Mod3
+linear4 = 1; //Coil5 Mod4
+linear5 = 1; //Coil6 Mod5
+linear6 = 1; //Coil7 Mod6
+
+ //RIGHT//
+ for(int i=0;i<set;i++){
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+ linear3 =! linear3; //Coil 4
+ wait(fast);
+ linear3 =! linear3;
+ linear2 =! linear2; //Coil 3
+ wait(fast);
+ linear2 =! linear2;
+ linear1 =! linear1; //Coil 2
+ wait(fast);
+ linear1 =! linear1;
+ }//End of For loop
+ return;
+}//End of Funciton
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////get_Lsensor_min////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//returns the min distance value of left sensor when called//
+float get_Lsensor_min(){
+ //Declarations and initializations//
+ float min = MAX;
+ float read;
+ for(int i=0;i<large_num;i++){
+ wait(sensor_set);
+ read = distanceL.read();
+ if(read < min){
+ min = read;
+ }//End of if
+ else{
+ wait(MIN);
+ }//End of else
+ }//End of for
+ return(min);
+}//End of Function
+
+///////////////////////////////////////////////////////////////////////////////
+//////////////////////////////get_Lsensor_max//////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//returns the max distance value of the left sensor when called//
+float get_Lsensor_max(){
+ //Declarations and initializations//
+ float max = MIN;
+ float read;
+ for(int i=0;i<large_num;i++){
+ wait(sensor_set);
+ read = distanceL.read();
+ if(read > max){
+ max = read;
+ }//End of if
+ else{
+ wait(MIN);
+ }//End of else
+ }//End of for
+ return(max);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////get_Rsensor_min///////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function returns the min value of the right sensor when called//
+float get_Rsensor_min(){
+ //Declarations and initializations//
+ float min = MAX;
+ float read;
+ for(int i=0;i<large_num;i++){
+ wait(sensor_set);
+ read = distanceR.read();
+ if(read < min){
+ min = read;
+ }//End of if
+ else{
+ wait(MIN);
+ }//End of else
+ }//End of for
+ return(min);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////get_Rsensor_max/////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+float get_Rsensor_max(){
+ //Declarations and initializations//
+ float max = MIN;
+ float read;
+ for(int i=0;i<large_num;i++){
+ wait(sensor_set);
+ read = distanceR.read();
+ if(read > max){
+ max = read;
+ }//End of if
+ else{
+ wait(MIN);
+ }//End of else
+ }//End of for
+ return(max);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////move_one_right//////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function takes current position and moves one to the right
+void move_one_right(int position){
+ //Declarations and initializations//
+ int current_coil = abs(position % seven);
+ //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
+ switch(current_coil){
+ case 0:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 0; //Fire Coil 1//
+ break;
+ case 1:
+ linear6 = 0; //Fire Coil 7//
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 2:
+ linear6 = 1;
+ linear5 = 0; //Fire Coil 6//
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 3:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 0; //Fire Coil 5//
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 4:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 0; //Fire Coil 4//
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 5:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 0; //Fire Coil 3//
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 6:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 0; //Fire Coil 2//
+ linear0 = 1;
+ break;
+ default:
+ wait(MIN);
+ }//End of switch
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////move_one_left//////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function takes current position and moves one to the left
+void move_one_left(int position){
+ //Declarations and initializations//
+ int current_coil = position % seven;
+ //xbee.printf("\r\nCurrent Coil:\t%d",current_coil);
+ switch(current_coil){
+ case 0:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 0; //Fire Coil 4//
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 1:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 0; //Fire Coil 3//
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 2:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 0; //Fire Coil 2//
+ linear0 = 1;
+ break;
+ case 3:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 0; //Fire Coil 1//
+ break;
+ case 4:
+ linear6 = 0; //Fire Coil 7//
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 5:
+ linear6 = 1;
+ linear5 = 0; //Fire Coil 6//
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 6:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 0; //Fire Coil 5//
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ default:
+ wait(MIN);
+ }//End of switch
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////get_position//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+//Function reads current position on track and returns the mod 7//
+//Function uses calibration data to determin current position//
+int get_position(){
+ //Declrations and Initializaitons//
+ int position = -1;
+ int count = 0;
+ while(position == -1){
+ wait(slow);
+ float left_sensor = distanceL.read();
+ float right_sensor = distanceR.read();
+ //xbee.printf("\r\nleft_sensor:%f",left_sensor);
+ //xbee.printf("\r\nright_sensor:%f",right_sensor);
+ //Use closest sensor//
+ if(left_sensor > right_sensor){
+ //xbee.printf("\r\nleft > right");
+ //Look at Lsensor data//
+ for(int i=0;i<track_length;i++){
+ if(left_sensor <= left_max[i] && left_sensor >= left_min[i]){
+ position = i;
+ xbee.printf("\r\nPosition L1 %d",position);
+ return position;
+ }//End of if//
+ else if(left_sensor < left_max[i] && left_sensor > left_max[i+one]){
+ position = i+one;
+ xbee.printf("\r\nPosition L2 %d",position);
+ return position;
+ }//End of else if//
+ else if(left_sensor < left_max[i] && left_sensor > left_min[i+one]){
+ position = i;
+ xbee.printf("\r\nPosition L3 %d",position);
+ return position;
+ }//End of else if//
+ }//End of for//
+ }//End of if//
+
+ else if(right_sensor >= left_sensor){
+ xbee.printf("\r\nright > left");
+ //Look at Rsensor data//
+ for(int i=0;i<track_length;i++){
+ if(right_sensor <= right_max[i] && right_sensor >= right_min[i]){
+ position = i;
+ xbee.printf("\r\nPosition R1 %d",position);
+ return position;
+ }//End of if//
+ else if(right_sensor > right_max[i] && right_sensor < right_max[i+one]){
+ position = i+one;
+ xbee.printf("\r\nPosition R2 %d",position);
+ return position;
+ }//End of else if//
+ else if(right_sensor > right_max[i] && right_sensor < right_min[i+one]){
+ position = i;
+ xbee.printf("\r\nPosition R3 %d",position);
+ return position;
+ }//End of else if//
+ }//End of for//
+ }//End of else if//
+ count++;
+ if(count > five){
+ return(-2);
+ }//End of if//
+ }//End of while(position....
+ return position;
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////////CALIBRATE//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+/*
+-Calibrate Motor
+1. Move all the way to the LEFT side of the track.
+2. Get max and mins from both distance sensors.
+3. Store the max and mins in various arrays.
+4. Move one position to the RIGHT and repeat for entire track.
+5. Once the entire track has been recorded export to a txt file on the mbed.
+Format for txt file:
+Position # :Left Low :Left High :Right Low :Right High
+0 :#### :#### :#### :####
+1 (3 \t) :#### (2 \t)...
+.
+.
+#EOF
+*/
+void calibrate_linear(){
+ //Speed Control//
+ wait(three);
+
+ //1.Move all the way to the LEFT side of the track//
+ move_all_left(six);
+ //2.Get Sensor Data//
+ for(int i=0;i<track_length;i++){
+ //3.Store in various arrays
+ wait(slow);
+ left_min[i] = get_Lsensor_min();
+ left_max[i] = get_Lsensor_max();
+ right_min[i] = get_Rsensor_min();
+ right_max[i] = get_Rsensor_max();
+ led4 = !led4;
+ //4.Move to the right one//
+ move_one_right(i);
+ }//End of for
+
+ //5. Export data to a txt file on the mbed//
+ FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing
+ fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n");
+ for(int i=0;i<track_length;i++){
+ 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]);
+ /*
+ //Example File Handeling Code//
+ FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
+ fprintf(fp, "Hello World!");
+ fclose(fp);
+ */
+ }//End of for
+ fclose(fp);
+ //Export each variable to its own file//
+ //Write leftmin.txt file//
+ FILE *fp1 = fopen("/local/leftmin.txt", "w");
+ for(int i=0;i<track_length;i++){
+ fprintf(fp1,"%f\r\n",left_min[i]);
+ }//End of for//
+ fclose(fp1);
+ //Write leftmax.txt file//
+ FILE *fp2 = fopen("/local/leftmax.txt", "w");
+ for(int i=0;i<track_length;i++){
+ fprintf(fp2,"%f\r\n",left_max[i]);
+ }//End of for//
+ fclose(fp2);
+ //Write rightmin.txt file//
+ FILE *fp3 = fopen("/local/rightmin.txt", "w");
+ for(int i=0;i<track_length;i++){
+ fprintf(fp3,"%f\r\n",right_min[i]);
+ }//End of for//
+ fclose(fp3);
+ //Write rightmax.txt file//
+ FILE *fp4 = fopen("/local/rightmax.txt", "w");
+ for(int i=0;i<track_length;i++){
+ fprintf(fp4,"%f\r\n",right_max[i]);
+ }//End of for//
+ fclose(fp4);
+
+ //Set an LED high so we know calibration is complete//
+ led1 = 1;
+ move_all_left(six);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////read_file//////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//read_file reads the text files containing calibration data
+void read_file(){
+ char line[80];
+ int i = 0;
+ FILE *fr1;
+ fr1 = fopen ("/local/leftmin.txt", "rt");
+ while(fgets(line, 80, fr1) != NULL){
+ sscanf (line, "%f", &left_min[i]);
+ i++;
+ }//End of while
+ fclose(fr1);
+
+ i = 0;
+ FILE *fr2;
+ fr2 = fopen ("/local/leftmax.txt", "rt");
+ while(fgets(line, 80, fr2) != NULL){
+ sscanf (line, "%f", &left_max[i]);
+ i++;
+ }//End of while
+ fclose(fr2);
+
+ i = 0;
+ FILE *fr3;
+ fr3 = fopen ("/local/rightmin.txt", "rt");
+ while(fgets(line, 80, fr3) != NULL){
+ sscanf (line, "%f", &right_min[i]);
+ i++;
+ }//End of while
+ fclose(fr3);
+
+ i = 0;
+ FILE *fr4;
+ fr4 = fopen ("/local/rightmax.txt", "rt");
+ while(fgets(line, 80, fr4) != NULL){
+ sscanf (line, "%f", &right_max[i]);
+ i++;
+ }//End of while
+ fclose(fr4);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+//////////////////////////////move_linear//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//move_linear accepts a desired locaiton and moves the forcer accordingly//
+//Returns 4 on normal exit... prompting the user for a new position//
+int move_linear(int desired_location){
+ int current_location = get_position();
+ int difference = current_location - desired_location;
+ int count = 0;
+ xbee.printf("\r\nInside move_linear function");
+ xbee.printf("\r\nCurrent_location %d", current_location);
+ if(desired_location == -1){
+ return -1;
+ }//End of if
+ while(current_location > desired_location || current_location < desired_location){
+ difference = current_location - desired_location;
+ xbee.printf("\r\nDifference %d",difference);
+ while(difference > 0){
+ if(count > five){
+ move_one_left(--current_location);
+ }//End of if//
+ //Move LEFT//
+ xbee.printf("\r\nMoving Left");
+ led2 = 1;
+ wait(medium);
+ for(int i=0; i<difference+two;i++){
+ move_one_left(current_location);
+ current_location--;
+ wait(fast);
+ }//End of for
+ current_location = get_position();
+ difference = current_location - desired_location;
+ led2 = 0;
+ count++;
+ if(current_location == -2){
+ return(four);
+ }//End of if//
+ }//End of while(greater than zero)//
+
+ while(difference < 0){
+ if(count > five){
+ move_one_right(++current_location);
+ }//End of if//
+ //Move RIGHT//
+ xbee.printf("\r\nMoving Right");
+ led3 = 1;
+ wait(medium);
+ for(int i=0; i<(abs(difference-three));i++){
+ move_one_right(current_location);
+ current_location++;
+ wait(fast);
+ }//End of for
+ current_location = get_position();
+ difference = current_location - desired_location;
+ led3 = 0;
+ count++;
+ if(current_location == -2){
+ return(four);
+ }//End of if//
+ }//End of while(difference less than 0)//
+ }//End of while(current not desired)//
+ return(four);
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////error_check////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function does scanf error checking//
+//It clears the keyboard buffer incase anything other than numbers a pressed//
+int error_check(){
+ char not_number;
+ int number;
+ int flag;
+ while ((flag = xbee.scanf("%d", &number)) != EOF){
+ if (flag != 1){
+ not_number = xbee.getc();
+#ifdef DEBUG
+xbee.printf("\r\ndegub:%c in input stream, discarding", not_number);
+#endif
+ }//End of if
+ else {
+ printf("\r\nRETURN = %d",number);
+ return(number);
+ }//End of else//
+ }//End of while((flag...
+ return(number);
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////////display_mode///////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function set up to run display mode options//
+void display_mode(){
+ int option = -1;
+ int exit = 0;
+ while(exit != one){
+ switch (option){
+ case 1:
+ //Wind sprints//
+ for(int j=0;j<large_num;j++){
+ for(int i=0;i<12;i++){
+ move_all_left(abs(six-i));
+ move_all_right(abs(six-i));
+ }//End of for//
+ }//End of for//
+ break;
+ case 2:
+ //Sucide sprints//
+ for(int j=0;j<large_num;j++){
+ for(int i=0;i<12;i++){
+ move_all_leftBobby(abs(six-i));
+ move_all_rightBobby(abs(six-i));
+ }//End of for//
+ }//End of for//
+ break;
+ case 3:
+ //Option 3//
+
+ break;
+ case 4:
+ //Return to main menu//
+ exit = one;
+ //break;
+ default:
+ scroll_up(30);
+ xbee.printf("\r\n\n****Display Mode****");
+ xbee.printf("\r\nTo run wind sprints press 1 and <Enter>.");
+ xbee.printf("\r\nTo run suicide sprints press 2 and <Enter>.");
+ xbee.printf("\r\nTo run option 3 press 3 and <Enter>.");
+ xbee.printf("\r\nTo return to the previous menu press 4 and <Enter>.");
+ scroll_up(25);
+ option = error_check();
+ break;
+ }//End of switch//
+ }//End of while//
+ return;
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////move_all_leftBobby/////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//This function accepts an interger value and moves the forcer to the left//
+//The interger number of coil sets//
+void move_all_leftBobby(int set){
+//Declartions and initialization//
+linear0 = 1; //Coil1 Mod0
+linear1 = 1; //Coil2 Mod1
+linear2 = 1; //Coil3 Mod2
+linear3 = 1; //Coil4 Mod3
+linear4 = 1; //Coil5 Mod4
+linear5 = 1; //Coil6 Mod5
+linear6 = 1; //Coil7 Mod6
+
+ //LEFT//
+ for(int i=0;i<set;i++){
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear1 =! linear1; //Coil 2
+ wait(fast);
+ linear1 =! linear1;
+ linear2 =! linear2; //Coil 3
+ wait(fast);
+ linear2 =! linear2;
+ linear3 =! linear3; //Coil 4
+ wait(fast);
+ linear3 =! linear3;
+ }//End of For loop
+
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear1 =! linear1; //Coil 2
+ wait(fast);
+ linear1 =! linear1;
+
+ return;
+}//End of Funciton
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////move_all_rightBobby////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//This function accepts an interger value and moves the forcer to the right//
+//The interger number of coil sets//
+void move_all_rightBobby(int set){
+//Declartions and initialization//
+linear0 = 1; //Coil1 Mod0
+linear1 = 1; //Coil2 Mod1
+linear2 = 1; //Coil3 Mod2
+linear3 = 1; //Coil4 Mod3
+linear4 = 1; //Coil5 Mod4
+linear5 = 1; //Coil6 Mod5
+linear6 = 1; //Coil7 Mod6
+
+ //RIGHT//
+ for(int i=0;i<set;i++){
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+ linear3 =! linear3; //Coil 4
+ wait(fast);
+ linear3 =! linear3;
+ linear2 =! linear2; //Coil 3
+ wait(fast);
+ linear2 =! linear2;
+ linear1 =! linear1; //Coil 2
+ wait(fast);
+ linear1 =! linear1;
+ }//End of For loop
+
+ linear0 =! linear0; //Coil 1
+ wait(fast);
+ linear0 =! linear0;
+ linear6 =! linear6; //Coil 7
+ wait(fast);
+ linear6 =! linear6;
+ linear5 =! linear5; //Coil 6
+ wait(fast);
+ linear5 =! linear5;
+ linear4 =! linear4; //Coil 5
+ wait(fast);
+ linear4 =! linear4;
+
+ return;
+}//End of Funciton
+
+///////////////////////////////////////////////////////////////////////////////
+//////////////////////////////scroll_up()//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function accepts an integer and scrolls the text on the serial terminal that
+//Number of spaces//
+void scroll_up(int space){
+ for(int i=0;i<space;i++){
+ xbee.printf("\r\n");
+ }//End of for
+}//End of function
\ No newline at end of file