William Pedler
2013 VCU Senior Design Expo Linear Rotary Motor Control Program. This is the actual program used by the team at the Expo.
main.cpp
- Committer:
- pedlerw
- Date:
- 2013-04-25
- Revision:
- 4:29aafde2dcbc
- Parent:
- 3:1d681d86f1dd
File content as of revision 4:29aafde2dcbc:
//SeniorDesign_ExpoPro//
//Written by William Pedler 4/15/2013//
//This is the program the 2013 LRM team is using at the Senior Design Show Case//
//This program uses two xbee s1 for serial interface//
//Includes//
#include "mbed.h"
using namespace std;
//Define//
#define DEBUG
#define MAX 1.0
#define MIN 0.0
#define large_num 500 //This times 4 controls how long the sensors read for each position//
#define one 1
#define two 2
#define three 3.0
#define four 4
#define five 5
#define six 6
#define seven 7
#define eight 8
#define nine 9
#define track_length 42
#define fast 0.125
#define medium 1.0
#define slow 2.0
#define sensor_set 0.005
#define rot_fast 0.080
#define rot_med 0.100
#define rot_slow 0.120
//Declar global variables//
float left_min[track_length] = {0};
float left_max[track_length] = {0};
float right_min[track_length] = {0};
float right_max[track_length] = {0};
//Labeling the Local file system//
LocalFileSystem local("local"); // Create the local filesystem under the name "local"
//Labeling onboard LED's//
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
//Label linear coils//
DigitalOut linear0(p5); //coil 1
DigitalOut linear1(p6); //coil 2
DigitalOut linear2(p7); //coil 3
DigitalOut linear3(p8); //coil 4
DigitalOut linear4(p9); //coil 5
DigitalOut linear5(p10); //coil 6
DigitalOut linear6(p11); //coil 7
//Reduce noise by declaring all unused Analog pins as DigitalOut//
DigitalOut left15(p15);
DigitalOut left16(p16);
DigitalOut left17(p17);
DigitalOut left18(p18);
AnalogIn distanceR(p19); //Vout yellow GND black Vss red
AnalogIn distanceL(p20); //Vout yellow GND black Vss red
//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();
///////////////////////////////////////////////////////////////////////////////
//////////////////////////////////MAIN/////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
int main()
{
//Declarations & Initializations//
int desired_location = 0;
int user_instruction = -1;
//Initialize Linear Coils//
linear0 = 1;
linear1 = 1;
linear2 = 1;
linear3 = 1;
linear4 = 1;
linear5 = 1;
linear6 = 1;
//Initialize 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 2 options-
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:
//Operate Linear Part//
for(int i=0; i<30; i++) {
xbee.printf("\r\n");
}//End of for
scroll_up(30);
xbee.printf("\r\n\n****Operation Menu****");
xbee.printf("\r\nEnter a position number from 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;
default:
//Get valid user_input//
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);
}//End of function
///////////////////////////////////////////////////////////////////////////////
////////////////////////////read_file//////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//read_file reads the text files containing calibration data
void read_file()
{
char line[80];
int i = 0;
FILE *fr1;
fr1 = fopen ("/local/leftmin.txt", "rt");
while(fgets(line, 80, fr1) != NULL) {
sscanf (line, "%f", &left_min[i]);
i++;
}//End of while
fclose(fr1);
i = 0;
FILE *fr2;
fr2 = fopen ("/local/leftmax.txt", "rt");
while(fgets(line, 80, fr2) != NULL) {
sscanf (line, "%f", &left_max[i]);
i++;
}//End of while
fclose(fr2);
i = 0;
FILE *fr3;
fr3 = fopen ("/local/rightmin.txt", "rt");
while(fgets(line, 80, fr3) != NULL) {
sscanf (line, "%f", &right_min[i]);
i++;
}//End of while
fclose(fr3);
i = 0;
FILE *fr4;
fr4 = fopen ("/local/rightmax.txt", "rt");
while(fgets(line, 80, fr4) != NULL) {
sscanf (line, "%f", &right_max[i]);
i++;
}//End of while
fclose(fr4);
}//End of function
///////////////////////////////////////////////////////////////////////////////
//////////////////////////////move_linear//////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//move_linear accepts a desired locaiton and moves the forcer accordingly//
//Returns 4 on normal exit... prompting the user for a new position//
int move_linear(int desired_location)
{
int current_location = get_position();
int difference = current_location - desired_location;
int count = 0;
xbee.printf("\r\nInside move_linear function");
xbee.printf("\r\nCurrent_location %d", current_location);
if(desired_location == -1) {
return -1;
}//End of if
while(current_location != desired_location) {
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