William Pedler
Code used for the 2013 LRM Senior Design Team at VCU 2013 Senior Design Linear Rotary Motor
Diff: main.cpp
- Revision:
- 0:b1f00c299273
- Child:
- 1:27dcf7ff5b96
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 23 21:31:49 2013 +0000
@@ -0,0 +1,1911 @@
+//FinalFinal_SeniorDesign//
+//Written by William Pedler 4/23/2013//
+//This is the program the 2013 LRM team is using as the Final Program Written
+//for Senior Design. 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
+#define rot_fast 0.080
+#define rot_med 0.100
+#define rot_slow 0.120
+#define rot_pos 24
+#define rot_step 15
+
+//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};
+float rotary_threshold[rot_pos] = {0};
+float threash = 0;
+int rot_position = 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//
+AnalogIn rot_indication(p15);
+AnalogIn zero(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
+//Declar rotary coils Digital I/O//
+DigitalOut rotary0(p21);
+DigitalOut rotary1(p22);
+DigitalOut rotary2(p23);
+DigitalOut rotary3(p24);
+DigitalOut rotary4(p25);
+DigitalOut rotary5(p26);
+//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); //line for number lines to scroll//
+void cork_screw();
+void rotate_all_cw(int set, float speed);
+void rotate_all_ccw(int set, float speed);
+void rotary_speed();
+void fast_rotary_cw();
+void small_rotary_speed();
+void rot_one_cw();
+void rot_one_ccw();
+void calibrate_rot();
+int get_user_angle(int desired_angle);
+void go_to_angle(int angle);
+void rot_pos_inc();
+void rot_pos_dec();
+///////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////MAIN/////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+int main()
+{
+ //Declarations & Initializations//
+ int desired_location = 0;
+ int desired_angle = 0;
+ int user_instruction = -1;
+
+ //Initialize Linear Coils//
+ linear0 = 1;
+ linear1 = 1;
+ linear2 = 1;
+ linear3 = 1;
+ linear4 = 1;
+ linear5 = 1;
+ linear6 = 1;
+ //Initialize Rotary Coils//
+ //Declar rotary coils Digital I/O//
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ //Initialize onbord led//
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////New User Interface//////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+ while(1) {
+ //-Prompt user with main menu-
+ switch(user_instruction) {
+ case 0:
+ user_instruction = -1;
+ break;
+ 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:
+ //Operation Menu//
+ 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 2 to 38: ");
+ xbee.printf("\r\nTo get back to the Main Menu press 0 and <Enter>.");
+ scroll_up(25);
+ desired_location = error_check();
+ if(desired_location == 0) {
+ user_instruction = -1;
+ break;
+ }//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\nEnter a rotary angle from 0 to 360");
+ scroll_up(25);
+ desired_angle = error_check();
+ desired_angle = get_user_angle(desired_angle);
+ 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);
+ //Go to the desired angle//
+ xbee.printf("\r\nGoing to angle %d", desired_angle);
+ go_to_angle(desired_angle);
+ break;
+ default:
+ //MAIN MENU//
+ scroll_up(30);
+ xbee.printf("\r\n\n****Main Menu****");
+ xbee.printf("\r\nFor Calibration press 1 and \r\n<Enter>.");
+ xbee.printf("\r\nTo enter a desired location press 2 and \r\n<Enter>.");
+ xbee.printf("\r\nFor display mode press 3 and\r\n<Enter>.");
+ scroll_up(22);
+ 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 = 0; //Fire Coil 6//
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 1:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 0; //Fire Coil 5//
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 2:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 0; //Fire Coil 4//
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 3:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 0; //Fire Coil 3//
+ linear1 = 1;
+ linear0 = 1;
+ break;
+ case 4:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 0; //Fire Coil 2//
+ linear0 = 1;
+ break;
+ case 5:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 0; //Fire Coil 1//
+ break;
+ case 6:
+ linear6 = 0; //Fire Coil 7//
+ linear5 = 1;
+ linear4 = 1;
+ 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()
+{
+ //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);
+ calibrate_rot();
+ 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);
+
+ i = 0;
+ FILE *fr5;
+ fr5 = fopen ("/local/rotary.txt","rt");
+ while(fgets(line, 80,fr5) != NULL){
+ sscanf (line, "%f", &rotary_threshold[i]);
+ i++;
+ }//End of while
+
+ i = 0;
+ FILE *fr6;
+ fr6 = fopen ("/local/threash.txt","rt");
+ while(fgets(line, 80, fr6) != NULL){
+ sscanf (line, "%f", &threash);
+ i++;
+ }//End of while
+
+
+}//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) {
+ difference = current_location - desired_location;
+ xbee.printf("\r\nDifference %d",difference);
+ while(difference > 0) {
+ if(count > four) {
+ for(int i=0; i<difference; i++) {
+ move_one_left(current_location-one);
+ current_location--;
+ }//End of for
+ }//End of if//
+ //Move LEFT//
+ xbee.printf("\r\nMoving Left");
+ led2 = 1;
+ wait(medium);
+ for(int i=0; i<difference; 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 > four) {
+ for(int i=0; i<(abs(difference)); i++) {
+ move_one_right(current_location+one);
+ current_location++;
+ }//End of for//
+ }//End of if//
+ //Move RIGHT//
+ xbee.printf("\r\nMoving Right");
+ led3 = 1;
+ wait(medium);
+ for(int i=0; i<(abs(difference)); 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;
+ //char input = 1;
+ int linear_auto = 3;
+ int linear_oscill = 3;
+ int combined = 3;
+ int high_speed = 3;
+ int rotary_auto = 3;
+ int set_up = 0;
+
+ while(exit != one) {
+ switch (option) {
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////DISPLAY OPTIONS///////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+ case 1:
+ //LINEAR AUTO PROGRAM//
+ for(int j=0; j<linear_auto; j++) {
+ for(int i=0; i<12; i++) {
+ move_all_left(abs(five-i));
+ move_all_right(abs(five-i));
+ }//End of for//
+ }//End of for//
+ option = -1;
+ break;
+
+ case 2:
+ //LINEAR OSCILLATION//
+ for(int j=0; j<linear_oscill; j++) {
+ for(int i=0; i<12; i++) {
+ move_all_leftBobby(abs(five-i));
+ move_all_rightBobby(abs(five-i));
+ }//End of for//
+ }//End of for//
+ option = -1;
+ break;
+
+ case 3:
+ //COMBINED//
+ for(int i=0; i<combined; i++) {
+ //1.Linear//
+ move_all_right(five);
+ 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;
+ wait(0.10);
+ //2.Rotate//
+ rotate_all_cw(eight, rot_slow);
+ wait(0.10);
+ rotate_all_ccw(eight, rot_slow);
+ //3.Cork Screw//
+ cork_screw();
+ wait(1.00);
+ //move_all_right(six);
+ //wait(0.10);
+ //4.Repeat//
+ }//End of for loop
+ option = -1;
+ break;
+
+ case 4:
+ //HIGH SPEED ROTARY//
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ for(int i=0; i<high_speed; i++) {
+ rotary_speed();
+ }//End of for//
+ option = -1;
+ break;
+ case 5:
+ //ROTARY AUTO PROGRAM//
+ for(int i=0; i<rotary_auto; i++) {
+ rotate_all_ccw(six, rot_fast);
+ rotate_all_cw(six, rot_fast);
+ }//End of for//
+ option = -1;
+ break;
+ case 6:
+ fast_rotary_cw();
+ option = -1;
+ break;
+ case 7:
+ //SMALL FORCER ROTARY AUTO PROGRAM//
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ for(int i=0; i<high_speed; i++) {
+ small_rotary_speed();
+ }//End of for//
+ option = -1;
+ break;
+ case 9:
+ //SET UP CASE//
+ xbee.printf("\r\n****Display Mode Set UP****");
+ xbee.printf("\r\nChoose a program and press <Enter>.");
+ xbee.printf("\r\n1. Linear Auto Program");
+ xbee.printf("\r\n2. Linear Oscillation");
+ xbee.printf("\r\n3. Combined Operation");
+ xbee.printf("\r\n4. High Speed Rotary");
+ xbee.printf("\r\n5. Rotary Auto Program");
+ scroll_up(22);
+ set_up = error_check();
+ switch(set_up) {
+ case 1:
+ //Edit LINEAR AUTO PROGRAM//
+ xbee.printf("\r\nHow many times would you like to run Linear Auto Program?");
+ scroll_up(27);
+ linear_auto = error_check();
+ break;
+ case 2:
+ //Edit LINEAR OSCILLATION//
+ xbee.printf("\r\nHow many times would you like to run Linear Oscillation?");
+ scroll_up(27);
+ linear_oscill = error_check();
+ break;
+ case 3:
+ //Edit COMBINED OPERATION//
+ xbee.printf("\r\nHow many times would you like to run Combined Operation?");
+ scroll_up(27);
+ combined = error_check();
+ break;
+ case 4:
+ //Edit HIGH SPEED ROTARY//
+ xbee.printf("\r\nHow many times would you like to run High Speed Rotary?");
+ scroll_up(27);
+ high_speed = error_check();
+ break;
+ case 5:
+ //Edit ROTARY AUTO PROGRAM//
+ xbee.printf("\r\nHow many times would you like to run Rotary Auto Program?");
+ scroll_up(27);
+ rotary_auto = error_check();
+ break;
+ }//End of switch(setup)
+ option = -1;
+ break;
+
+ case 0:
+ //Return to the main menu//
+ exit = one;
+ break;
+ default:
+ option = -1;
+ scroll_up(30);
+ xbee.printf("\r\n\n****Display Mode****");
+ xbee.printf("\r\nTo run LINEAR AUTO PROGRAM press 1 and \r\n<Enter>.");
+ xbee.printf("\r\nTo run LINEAR OSCILLATION press 2 and \r\n<Enter>.");
+ xbee.printf("\r\nTo run COMBINED OPERATION press 3 and \r\n<Enter>.");
+ xbee.printf("\r\nTo run HIGH SPEED ROTARY press 4 and \r\n<Enter>.");
+ xbee.printf("\r\nTo run ROTARY AUTO PROGRAM press 5 and \r\n<Enter>.");
+ xbee.printf("\r\nTo run SMALL FORCER SPEED RUN press 6 and \r\n<Enter>.");
+ //xbee.printf("\r\nTo run SMALL FORCER AUTO ROTARY press 7 and \r\n<Enter>.");
+ xbee.printf("\r\nFor SET UP press 9 and \r\n<Enter>.");
+ xbee.printf("\r\nTo return to the previous menu press 0 and \r\n<Enter>.");
+ scroll_up(12);
+ 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;
+ //linear3 =! linear3; //Coil 4
+ //wait(fast);
+ //linear3 =! linear3;
+
+
+ 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
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////cork_screw()////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function moves linear and rotary at the same time... from right to left//////
+void cork_screw()
+{
+rotary0 = 1;
+rotary1 = 1;
+rotary2 = 1;
+rotary3 = 1;
+rotary4 = 1;
+rotary5 = 1;
+ int length = track_length;
+ //for(int i=0; i<track_length; i++) {
+ move_one_left(abs(length--));
+ rotary0 = !rotary0;
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary1 = !rotary1;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary2 = !rotary2;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary3 = !rotary3;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary4 = !rotary4;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary5 = !rotary5;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary0 = !rotary0;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary1 = !rotary1;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary2 = !rotary2;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary3 = !rotary3;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary4 = !rotary4;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary5 = !rotary5;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary0 = !rotary0;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary1 = !rotary1;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary2 = !rotary2;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary3 = !rotary3;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary4 = !rotary4;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary5 = !rotary5;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary0 = !rotary0;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary1 = !rotary1;
+ move_one_left(abs(length--));
+ wait(fast);
+ move_one_left(abs(length--));
+ wait(fast);
+ rotary2 = !rotary2;
+ move_one_left(abs(length--));
+ wait(fast);
+ /*move_one_left(abs(length--));
+ wait(fast);
+ rotary3 = !rotary3;
+ move_one_left(abs(length--)); // */
+ //}//End of for//
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////rotate_all_cw(int )///////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+void rotate_all_cw(int set, float speed)
+{
+rotary0 = 1;
+rotary1 = 1;
+rotary2 = 1;
+rotary3 = 1;
+rotary4 = 1;
+rotary5 = 1;
+ for(int i=0; i<set; i++) {
+ rotary0 = !rotary0;
+ wait(speed);
+ rotary1 = !rotary1;
+ wait(speed);
+ rotary2 = !rotary2;
+ wait(speed);
+ rotary3 = !rotary3;
+ wait(speed);
+ rotary4 = !rotary4;
+ wait(speed);
+ rotary5 = !rotary5;
+ wait(speed);
+ }//End of for
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////rotate_all_ccw(int s)///////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function rotate the forcer
+void rotate_all_ccw(int set, float speed)
+{
+rotary0 = 1;
+rotary1 = 1;
+rotary2 = 1;
+rotary3 = 1;
+rotary4 = 1;
+rotary5 = 1;
+ for(int i=0; i<set; i++) {
+ rotary5 = !rotary5;
+ wait(speed);
+ rotary4 = !rotary4;
+ wait(speed);
+ rotary3 = !rotary3;
+ wait(speed);
+ rotary2 = !rotary2;
+ wait(speed);
+ rotary1 = !rotary1;
+ wait(speed);
+ rotary0 = !rotary0;
+ wait(speed);
+ }//End of for
+}//End of function//
+
+///////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////rotary_speed();//////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//rotary_speed() rotates the forcer faster and faster//
+void rotary_speed()
+{
+ float time = 0.160;
+ float step = 0.010;
+ int a = 0;
+ int cycle = 16;
+ int change = 0;
+ int turns = 4;
+ int maxint = .08;
+
+ while(a<2) {
+ for(int i=cycle; i>0; i--) {
+ if(time == maxint) {
+ turns = 80;
+ } else if(time != maxint) {
+ turns = 4;
+ }
+ if(change == 0) {
+ if(i > cycle/2) {
+ rotate_all_cw(turns,time);
+ time = time - step;
+ }//End of if
+ if(i < ((cycle/2) + 1 ) && i > 0) {
+ rotate_all_cw(turns,time);
+ time = time + step;
+ }//End of if
+ } else if(change==1) {
+ if(i > cycle/2) {
+
+ rotate_all_ccw(turns,time);
+ time = time - step;
+ }//End of if
+ if(i < ((cycle/2) + 1 ) && i > 0) {
+ rotate_all_ccw(turns,time);
+ time = time + step;
+ }//End of if
+ }//End of else if//
+ }//End of for//
+ change = !change;
+ a++;
+ }//End of while
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////fast_rotary_cw()////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Special function for little forcer
+void fast_rotary_cw(){
+ rotate_all_cw(400, 0.01);
+}//End of Function//
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////////small_rotary_speed();//////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//small_rotary_speed() rotates the small forcer faster and faster//
+void small_rotary_speed()
+{
+ float time = 0.080;
+ float step = 0.005;
+ int a = 0;
+ int cycle = 32;
+ int change = 0;
+ int turns = 8;
+ int maxint = 0.010;
+
+ while(a<2) {
+ for(int i=cycle; i>0; i--) {
+ if(time == maxint) {
+ turns = 400;
+ } else if(time != maxint) {
+ turns = 8;
+ }
+ if(change == 0) {
+ if(i > cycle/2) {
+ rotate_all_cw(turns,time);
+ time = time - step;
+ }//End of if
+ if(i < ((cycle/2) + 1 ) && i > 0) {
+ rotate_all_cw(turns,time);
+ time = time + step;
+ }//End of if
+ } else if(change==1) {
+ if(i > cycle/2) {
+ rotate_all_ccw(turns,time);
+ time = time - step;
+ }//End of if
+ if(i < ((cycle/2) + 1 ) && i > 0) {
+ rotate_all_ccw(turns,time);
+ time = time + step;
+ }//End of if
+ }//End of else if//
+ }//End of for//
+ change = !change;
+ a++;
+ }//End of while
+}//End of function
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////rot_one_cw()//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function analyzes current state of digital I/O for rotary and increments cw//
+void rot_one_cw(){
+ if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 1 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ rot_pos_dec();
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////rot_one_ccw()//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function analyzes current state of rotary digital I/O and increments ccw//
+void rot_one_ccw(){
+ if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 1 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 0 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 0 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 0;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 0){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 0;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 1 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 0;
+ }
+ else if(rotary0 == 0 && rotary1 == 1 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 1 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 1 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 1 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ else if(rotary0 == 0 && rotary1 == 0 && rotary2 == 0 && rotary3 == 0 && rotary4 == 0 && rotary5 == 1){
+ rotary0 = 0;
+ rotary1 = 0;
+ rotary2 = 0;
+ rotary3 = 0;
+ rotary4 = 1;
+ rotary5 = 1;
+ }
+ rot_pos_inc();
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////calibrate_rot()/////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function rotates cw 24 times gathering return data from photo resistors//
+//Then it sets the threashold (global variable)//
+void calibrate_rot(){
+ float sum = 0;
+ rotary0 = 1;
+ rotary1 = 1;
+ rotary2 = 1;
+ rotary3 = 1;
+ rotary4 = 1;
+ rotary5 = 1;
+ for(int i=0;i<rot_pos;i++){
+ rotary_threshold[i] = rot_indication.read();
+ rot_one_cw();
+ }//End of for
+ //Print rotary_threshold[i] to file//
+ FILE *fp5= fopen("/local/rotary.txt", "w");
+ for(int i=0;i<rot_pos;i++) {
+ fprintf(fp5,"%f\r\n",rotary_threshold[i]);
+ }//End of for//
+ fclose(fp5);
+ //Calculate Threash//
+ for(int i=0;i<rot_pos;i++){
+ sum = sum + rotary_threshold[i];
+ }//End of for
+ //Print Threash to file//
+ threash = sum/rot_pos;
+ FILE *fp6= fopen("/local/threash.txt", "w");
+ fprintf(fp6,"%f\r\n",threash);
+ fclose(fp6);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////get_user_angle()//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//This function accepts an int and returns an int... the int value represents//
+//a desired angle...this returns the nearest angle within 15 degrees//
+int get_user_angle(int desired_angle){
+ int angle = -1;
+ if(desired_angle <= 0){
+ angle = 0;
+ }
+ else if(desired_angle > 0 && desired_angle < (0+7.5)){
+ angle = 0;
+ }
+ else if(desired_angle >= (0+7.5) && desired_angle <= 15){
+ angle = 15;
+ }
+ else if(desired_angle > 15 && desired_angle < (15+7.5)){
+ angle = 15;
+ }
+ else if(desired_angle >= (15+7.5) && desired_angle <= 30){
+ angle = 30;
+ }
+ else if(desired_angle > 30 && desired_angle < (30+7.5)){
+ angle = 30;
+ }
+ else if(desired_angle >= (30+7.5) && desired_angle <= 45){
+ angle = 45;
+ }
+ else if(desired_angle > 45 && desired_angle < (45+7.5)){
+ angle = 45;
+ }
+ else if(desired_angle >= (45+7.5) && desired_angle <= 60){
+ angle = 60;
+ }
+ else if(desired_angle > 60 && desired_angle < (60+7.5)){
+ angle = 60;
+ }
+ else if(desired_angle >= (60+7.5) && desired_angle <= 75){
+ angle = 75;
+ }
+ else if(desired_angle > 75 && desired_angle < (75+7.5)){
+ angle = 75;
+ }
+ else if(desired_angle >= (75+7.5) && desired_angle <= 90){
+ angle = 90;
+ }
+ else if(desired_angle > 90 && desired_angle < (90+7.5)){
+ angle = 90;
+ }
+ else if(desired_angle >= (90+7.5) && desired_angle <= 105){
+ angle = 105;
+ }
+ else if(desired_angle > 105 && desired_angle < (105+7.5)){
+ angle = 105;
+ }
+ else if(desired_angle >= (105+7.5) && desired_angle <= 120){
+ angle = 120;
+ }
+ else if(desired_angle > 120 && desired_angle < (120+7.5)){
+ angle = 120;
+ }
+ else if(desired_angle >= (120+7.5) && desired_angle <= 135){
+ angle = 135;
+ }
+ else if(desired_angle > 135 && desired_angle < (135+7.5)){
+ angle = 135;
+ }
+ else if(desired_angle >= (135+7.5) && desired_angle <= 150){
+ angle = 150;
+ }
+ else if(desired_angle > 150 && desired_angle < (150+7.5)){
+ angle = 150;
+ }
+ else if(desired_angle >= (150+7.5) && desired_angle <= 165){
+ angle = 165;
+ }
+ else if(desired_angle > 165 && desired_angle < (165+7.5)){
+ angle = 165;
+ }
+ else if(desired_angle >= (165+7.5) && desired_angle <= 180){
+ angle = 180;
+ }
+ else if(desired_angle > 180 && desired_angle < (180+7.5)){
+ angle = 180;
+ }
+ else if(desired_angle >= (180+7.5) && desired_angle <= 195){
+ angle = 195;
+ }
+ else if(desired_angle > 195 && desired_angle < (195+7.5)){
+ angle = 195;
+ }
+ else if(desired_angle >= (195+7.5) && desired_angle <= 210){
+ angle = 210;
+ }
+ else if(desired_angle > 210 && desired_angle < (210+7.5)){
+ angle = 210;
+ }
+ else if(desired_angle >= (210+7.5) && desired_angle <= 225){
+ angle = 225;
+ }
+ else if(desired_angle > 225 && desired_angle < (225+7.5)){
+ angle = 225;
+ }
+ else if(desired_angle >= (225+7.5) && desired_angle <= 240){
+ angle = 240;
+ }
+ else if(desired_angle > 240 && desired_angle < (240+7.5)){
+ angle = 240;
+ }
+ else if(desired_angle >= (240+7.5) && desired_angle <= 255){
+ angle = 255;
+ }
+ else if(desired_angle > 255 && desired_angle < (255+7.5)){
+ angle = 255;
+ }
+ else if(desired_angle >= (255+7.5) && desired_angle <= 270){
+ angle = 270;
+ }
+ else if(desired_angle > 270 && desired_angle < (270+7.5)){
+ angle = 270;
+ }
+ else if(desired_angle >= (270+7.5) && desired_angle <= 285){
+ angle = 285;
+ }
+ else if(desired_angle > 285 && desired_angle < (285+7.5)){
+ angle = 285;
+ }
+ else if(desired_angle >= (285+7.5) && desired_angle <= 300){
+ angle = 300;
+ }
+ else if(desired_angle > 300 && desired_angle < (300+7.5)){
+ angle = 300;
+ }
+ else if(desired_angle >= (300+7.5) && desired_angle <= 315){
+ angle = 315;
+ }
+ else if(desired_angle > 315 && desired_angle < (315+7.5)){
+ angle = 315;
+ }
+ else if(desired_angle >= (315+7.5) && desired_angle <= 330){
+ angle = 330;
+ }
+ else if(desired_angle > 330 && desired_angle < (330+7.5)){
+ angle = 330;
+ }
+ else if(desired_angle >= (330+7.5) && desired_angle <= 345){
+ angle = 345;
+ }
+ else if(desired_angle > 345 && desired_angle < (345+7.5)){
+ angle = 345;
+ }
+ else if(desired_angle >= (345+7.5) && desired_angle <= 360){
+ angle = 0;
+ }
+ else if(desired_angle > 360){
+ angle = 0;
+ }
+ else{
+ xbee.printf("\r\nAngle input was not understandable");
+ }
+ return(angle);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////go_to_angle()///////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//Function accepts an angle then rotates the forcer to said angle//
+void go_to_angle(int angle){
+ while(rot_position != angle){
+ if(rot_position - angle == 0){
+ return;
+ }//End of if
+ else if(rot_position - angle > 0){
+ rot_one_cw();
+ }//End of else if
+ else if(rot_position - angle < 0){
+ rot_one_ccw();
+ }//End of else if
+ }
+ return;
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////rot_pos_inc()///////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+void rot_pos_inc(){
+ if(zero.read() > threash){
+ rot_position = 0;
+ return;
+ }//End of if//
+ else if(zero.read() < threash){
+ rot_position = rot_position + rot_step;
+ }//End of else if//
+}//End of fucntion
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////rot_pos_dec()//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+void rot_pos_dec(){
+ if(zero.read() > threash){
+ rot_position = 0;
+ return;
+ }//End of if//
+ else if(zero.read() < threash){
+ rot_position = rot_position - rot_step;
+ }//End of else if//
+}//End of function
\ No newline at end of file