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:
- 3:1d681d86f1dd
- Parent:
- 2:e46a0432d96b
- Child:
- 4:29aafde2dcbc
--- a/main.cpp Tue Apr 16 19:56:07 2013 +0000
+++ b/main.cpp Wed Apr 17 22:56:54 2013 +0000
@@ -26,6 +26,9 @@
#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
//Declar global variables//
float left_min[track_length] = {0};
@@ -49,12 +52,19 @@
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 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
+//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);
@@ -73,35 +83,24 @@
int move_linear(int l); //l for location//
int error_check();
void display_mode();
-void move_all_leftBobby(int num);
+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();
///////////////////////////////////////////////////////////////////////////////
-/////////////////////////////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() {
+int main()
+{
//Declarations & Initializations//
int desired_location = 0;
int user_instruction = -1;
-
+
//Initialize Linear Coils//
linear0 = 1;
linear1 = 1;
@@ -109,7 +108,15 @@
linear3 = 1;
linear4 = 1;
linear5 = 1;
- linear6 = 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;
@@ -119,70 +126,74 @@
///////////////////////////New User Interface//////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
- while(1){
- //-Prompt user with 2 options-
- switch(user_instruction){
- case 1:
+ while(1) {
+ //-Prompt user with 2 options-
+ switch(user_instruction) {
+ case 0:
+ user_instruction = -1;
+ break;
+ case 1:
//Calibrate Motor//
- calibrate_linear();
+ calibrate_linear();
//Go to operate motor//
user_instruction = 4; //Go to operation menu//
- break;
+ break;
case 2:
//Use old calibration file and begin operation
//Read file to fill in max and min//
- read_file();
+ read_file();
user_instruction = 4; //Go to operation menu//
- break;
+ break;
case 3:
- display_mode();
+ display_mode();
user_instruction = -1; //Go to main menu//
- break;
+ break;
case 4:
//Operate Linear Part//
- for(int i=0;i<30;i++){
+ for(int i=0; i<30; i++) {
xbee.printf("\r\n");
- }//End of for
+ }//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);
+ 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\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;
+ 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;
+ 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
@@ -196,40 +207,41 @@
///////////////////////////////////////////////////////////////////////////////
//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){
+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
+ 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
+ //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
@@ -238,40 +250,41 @@
///////////////////////////////////////////////////////////////////////////////
//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){
+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
+ 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
+ //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
@@ -279,20 +292,21 @@
////////////////////////////get_Lsensor_min////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//returns the min distance value of left sensor when called//
-float get_Lsensor_min(){
+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
+ 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
@@ -300,20 +314,21 @@
//////////////////////////////get_Lsensor_max//////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//returns the max distance value of the left sensor when called//
-float get_Lsensor_max(){
+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
+ 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
@@ -321,40 +336,42 @@
/////////////////////////////get_Rsensor_min///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Function returns the min value of the right sensor when called//
-float get_Rsensor_min(){
+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
+ 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(){
+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
+ 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
@@ -362,154 +379,156 @@
///////////////////////////////move_one_right//////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Function takes current position and moves one to the right
-void move_one_right(int position){
+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//
+ //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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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
+ 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){
+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;
+ //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 = 1;
- linear3 = 1;
- linear2 = 0; //Fire Coil 3//
- linear1 = 1;
- linear0 = 1;
+ 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 = 1;
- linear2 = 1;
- linear1 = 0; //Fire Coil 2//
- linear0 = 1;
+ 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 = 1;
- linear1 = 1;
- linear0 = 0; //Fire Coil 1//
+ case 3:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 0; //Fire Coil 3//
+ linear1 = 1;
+ linear0 = 1;
break;
- case 4:
- linear6 = 0; //Fire Coil 7//
- linear5 = 1;
- linear4 = 1;
- linear3 = 1;
- linear2 = 1;
- linear1 = 1;
- linear0 = 1;
+ 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 = 0; //Fire Coil 6//
- linear4 = 1;
- linear3 = 1;
- linear2 = 1;
- linear1 = 1;
- linear0 = 1;
+ case 5:
+ linear6 = 1;
+ linear5 = 1;
+ linear4 = 1;
+ linear3 = 1;
+ linear2 = 1;
+ linear1 = 1;
+ linear0 = 0; //Fire Coil 1//
break;
- case 6:
- linear6 = 1;
- linear5 = 1;
- linear4 = 0; //Fire Coil 5//
- linear3 = 1;
- linear2 = 1;
- linear1 = 1;
- linear0 = 1;
+ 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
+ default:
+ wait(MIN);
+ }//End of switch
}//End of function
///////////////////////////////////////////////////////////////////////////////
@@ -518,63 +537,64 @@
//Function reads current position on track and returns the mod 7//
//Function uses calibration data to determin current position//
-int get_position(){
+int get_position()
+{
//Declrations and Initializaitons//
int position = -1;
int count = 0;
- while(position == -1){
+ 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);
+ //xbee.printf("\r\nright_sensor:%f",right_sensor);
//Use closest sensor//
- if(left_sensor > right_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]){
+ 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]){
+ 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]){
+ 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){
+
+ 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]){
+ 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]){
+ 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]){
+ 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//
+ }//End of else if//
count++;
- if(count > five){
- return(-2);
+ if(count > five) {
+ return(-2);
}//End of if//
}//End of while(position....
return position;
@@ -598,107 +618,109 @@
.
#EOF
*/
-void calibrate_linear(){
+void calibrate_linear()
+{
//1.Move all the way to the LEFT side of the track//
- move_all_left(six);
+ 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;
+ 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
-
+ 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);
+ 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(){
+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);
+ 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
///////////////////////////////////////////////////////////////////////////////
@@ -706,69 +728,70 @@
///////////////////////////////////////////////////////////////////////////////
//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 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//
+ 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();
+ 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//
+ 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;
+ 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();
+ 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);
+ 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//
///////////////////////////////////////////////////////////////////////////////
@@ -776,22 +799,23 @@
///////////////////////////////////////////////////////////////////////////////
//Function does scanf error checking//
//It clears the keyboard buffer incase anything other than numbers a pressed//
-int error_check(){
+int error_check()
+{
char not_number;
int number;
- int flag;
- while ((flag = xbee.scanf("%d", &number)) != EOF){
- if (flag != 1){
- not_number = xbee.getc();
+ 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);
+ 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...
+ }//End of if
+ else {
+ printf("\r\nRETURN = %d",number);
+ return(number);
+ }//End of else//
+ }//End of while((flag...
return(number);
}//End of function//
@@ -799,48 +823,173 @@
////////////////////////////////display_mode///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Function set up to run display mode options//
-void display_mode(){
+void display_mode()
+{
int option = -1;
int exit = 0;
- while(exit != one){
- switch (option){
+ //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:
- //Wind sprints//
- for(int j=0;j<large_num;j++){
- for(int i=0;i<12;i++){
+ //LINEAR AUTO PROGRAM//
+ for(int j=0; j<linear_auto; 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;
+ option = -1;
+ break;
+
case 2:
- //Sucide sprints//
- for(int j=0;j<large_num;j++){
- for(int i=0;i<12;i++){
+ //LINEAR OSCILLATION//
+ for(int j=0; j<linear_oscill; 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;
+ }//End of for//
+ option = -1;
+ break;
+
case 3:
- //Option 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_cw(six, rot_fast);
+ rotate_all_ccw(six, rot_fast);
+ }//End of for//
+ option = -1;
+ break;
+ case 6:
+ fast_rotary_cw();
+ 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(29);
+ 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(29);
+ 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(29);
+ 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(29);
+ 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(29);
+ rotary_auto = error_check();
+ break;
+ }//End of switch(setup)
+ option = -1;
+ break;
- break;
- case 4:
- //Return to main menu//
- exit = one;
- //break;
+ case 0:
+ //Return to the main menu//
+ exit = one;
+ break;
default:
- scroll_up(30);
+ option = -1;
+ 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//
+ xbee.printf("\r\nTo run LINEAR AUTO PROGRAM press 1 and <Enter>.");
+ xbee.printf("\r\nTo run LINEAR OSCILLATION press 2 and <Enter>.");
+ xbee.printf("\r\nTo run COMBINED OPERATION press 3 and <Enter>.");
+ xbee.printf("\r\nTo run HIGH SPEED ROTARY press 4 and <Enter>.");
+ xbee.printf("\r\nTo run ROTARY AUTO PROGRAM press 5 and <Enter>.");
+ xbee.printf("\r\nTo run SMALL FORCER SPEED RUN press 6 and <Enter>.");
+ xbee.printf("\r\nFor SET UP press 9 and <Enter>.");
+ xbee.printf("\r\nTo return to the previous menu press 0 and <Enter>.");
+ scroll_up(14);
+ option = error_check();
+ break;
+ }//End of switch//
}//End of while//
return;
}//End of function//
@@ -850,56 +999,57 @@
///////////////////////////////////////////////////////////////////////////////
//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){
+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
+ 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
-
+ //LEFT//
+ for(int i=0; i<set; i++) {
linear4 =! linear4; //Coil 5
wait(fast);
linear4 =! linear4;
linear5 =! linear5; //Coil 6
wait(fast);
- linear5 =! linear5;
+ linear5 =! linear5;
linear6 =! linear6; //Coil 7
wait(fast);
- linear6 =! linear6;
+ linear6 =! linear6;
linear0 =! linear0; //Coil 1
wait(fast);
- linear0 =! linear0;
+ linear0 =! linear0;
linear1 =! linear1; //Coil 2
wait(fast);
- linear1 =! linear1;
+ 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
@@ -909,54 +1059,55 @@
///////////////////////////////////////////////////////////////////////////////
//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){
+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
+ 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
-
+ //RIGHT//
+ for(int i=0; i<set; i++) {
linear0 =! linear0; //Coil 1
wait(fast);
linear0 =! linear0;
linear6 =! linear6; //Coil 7
wait(fast);
- linear6 =! linear6;
+ linear6 =! linear6;
linear5 =! linear5; //Coil 6
wait(fast);
- linear5 =! linear5;
+ linear5 =! linear5;
linear4 =! linear4; //Coil 5
wait(fast);
- linear4 =! linear4;
-
+ 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
@@ -965,8 +1116,231 @@
///////////////////////////////////////////////////////////////////////////////
//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++){
+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
+ }//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)
+{
+ 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)
+{
+ 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//
\ No newline at end of file