Diff: main.cpp

Revision:

0:4e9d7f54220e

Child:

1:839192bf53f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 09 03:13:51 2013 +0000
@@ -0,0 +1,469 @@
+//AutoCalibrate//
+//Written by William Pedler 4/8/2013//
+//This program is used to automatically calibrate and move the forcer linearly//
+
+//Includes//
+#include "mbed.h"
+
+//Define//
+#define MAXI 1.0
+#define MINI 0.0
+#define BIG 100        //This times 4 controls how long the sensors read for each position//
+#define three 3.0
+#define seven 7
+#define length 42
+#define SPEED 0.150
+#define slow 0.5
+
+//Declar global variables//
+float left_min[length] = {0};
+float left_max[length] = {0};
+float right_min[length] = {0};
+float right_max[length] = {0};
+
+//Labeling the Local file system//
+LocalFileSystem local("local");               // Create the local filesystem under the name "local"
+//Labeling onboard LED's//
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+//Label linear coils//
+DigitalOut linear0(p5);      //coil 1
+DigitalOut linear1(p6);      //coil 2
+DigitalOut linear2(p7);      //coil 3
+DigitalOut linear3(p8);      //coil 4
+DigitalOut linear4(p9);      //coil 5
+DigitalOut linear5(p10);     //coil 6
+DigitalOut linear6(p11);     //coil 7
+//Reduce noise by declaring all unused Analog pins as DigitalOut//
+DigitalOut left15(p15);          
+DigitalOut left16(p16);           
+DigitalOut left17(p17);
+DigitalOut left18(p18);
+AnalogIn distanceR(p19);        //Vout yellow GND black Vss red
+AnalogIn distanceL(p20);        //Vout yellow GND black Vss red
+//Set USB//
+//Serial pc(USBTX, USBRX);
+
+//Functions//
+void move_all_left();
+float get_Lsensor_min();
+float get_Lsensor_max();
+float get_Rsensor_min();
+float get_Rsensor_max();
+void move_one_right(int p);
+void move_one_left(int p);
+int get_position();
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////Main Program//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+/*//////////////////////Pseudo code for Main://////////////////////////////////
+-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
+-Operate Motor
+1. Get current location
+2. Get desired location
+3. Move to location
+4. Repeat
+*//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////MAIN/////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+int main() {
+    //Declarations & Initializations//
+    int current_location = 0;
+    int desired_location = 0;
+    int difference = 0;
+    
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+
+    //Speed Control//
+    wait(three);
+ 
+    //1.Move all the way to the LEFT side of the track//
+        move_all_left();
+    //2.Get Sensor Data//
+        for(int i=0;i<length;i++){
+            //3.Store in various arrays        
+            left_min[i] = get_Lsensor_min();
+            left_max[i] = get_Lsensor_max();
+            right_min[i] = get_Rsensor_min();
+            right_max[i] = get_Rsensor_max();
+            
+            //4.Move to the right one//
+            move_one_right(i);
+            wait(slow);
+        }//End of for
+        
+    //5. Export 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<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);
+        
+        //Set an LED high so we know calibration is complete//
+        led1 = 1;
+    
+ //////////////////////////////////////////////////////////////////////////////
+ ////////////////////////////Operate Motor/////////////////////////////////////
+ //////////////////////////////////////////////////////////////////////////////
+       
+    //Now use calibration data to operate motor//
+    while(1){
+    //1.Find current location//
+    current_location = get_position();
+    //2.Take in desired position//
+        //I'll code this later... for now go to position 13//
+    desired_location = 13;
+    //3.Move to desired position//
+        difference = current_location - desired_location;
+        while(difference > 0){
+            move_one_left(current_location);
+            current_location = get_position();
+            difference = current_location - desired_location;
+        }//End of while//
+        while(difference < 0){
+            move_one_right(current_location);
+            current_location = get_position();
+            difference = current_location - desired_location;
+        }//End of while//
+    //4.Repeat//
+    }//End of while//    
+}//End of main
+
+///////////////////////////////////////////////////////////////////////////////
+/////////////////////////////FUNCITONS/////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////
+////////////////////////////move_all_left//////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+//moves the forcer all the way to the left of the track//
+void move_all_left(){
+//Declartions and initialization//
+float lng = (SPEED);
+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<6;i++){
+        linear0 =! linear0;   //Coil 1
+        wait(lng);
+        linear0 =! linear0;
+        linear1 =! linear1;   //Coil 2
+        wait(lng);
+        linear1 =! linear1;        
+        linear2 =! linear2;   //Coil 3
+        wait(lng);
+        linear2 =! linear2;     
+        linear3 =! linear3;   //Coil 4
+        wait(lng);
+        linear3 =! linear3;       
+        linear4 =! linear4;   //Coil 5
+        wait(lng);
+        linear4 =! linear4;        
+        linear5 =! linear5;   //Coil 6
+        wait(lng);
+        linear5 =! linear5;      
+        linear6 =! linear6;   //Coil 7
+        wait(lng);
+        linear6 =! linear6;       
+        }//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 = MAXI;
+    float read;
+        for(int i=0;i<BIG;i++){
+            wait(0.005);
+            read = distanceL.read();
+            if(read < min){
+                min = read;
+            }//End of if
+            else{
+                wait(MINI);
+            }//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 = MINI;
+    float read;
+        for(int i=0;i<BIG;i++){
+            wait(0.005);
+            read = distanceL.read();
+            if(read > max){
+                max = read;
+            }//End of if
+            else{
+                wait(MINI);
+            }//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 = MAXI;
+    float read;
+        for(int i=0;i<BIG;i++){
+            wait(0.005);
+            read = distanceR.read();
+            if(read < min){
+                min = read;
+            }//End of if
+            else{
+                wait(MINI);
+            }//End of else
+        }//End of for
+    return(min);
+}//End of function
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////get_Rsensor_max/////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+float get_Rsensor_max(){
+    //Declarations and initializations//
+    float max = MINI;
+    float read;
+        for(int i=0;i<BIG;i++){
+            wait(0.005);
+            read = distanceR.read();
+            if(read > max){
+                max = read;
+            }//End of if
+            else{
+                wait(MINI);
+            }//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 = position % seven;
+        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(MINI);
+        }//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;
+        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 4//
+                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(MINI);
+        }//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 = 0;
+    float left_sensor = distanceL.read();
+    float right_sensor = distanceR.read();
+        //Use closest sensor//
+        if(left_sensor > right_sensor){
+            //Look at Lsensor data//
+            for(int i=0;i<length;i++){
+                if(left_sensor < left_max[i] && left_sensor > left_min[i]){
+                    position = i % seven;
+                }//End of if//
+            }//End of for//
+        }//End of if//
+        
+        else if(right_sensor >= left_sensor){
+            //Look at Rsensor data//
+            for(int i=0;i<length;i++){
+                if(right_sensor < right_max[i] && right_sensor > right_min[i]){
+                    position = i % seven;
+                }//End of if//
+            }//End of for//
+        }//End of else if//
+    return position;
+}//End of function
\ No newline at end of file