Diff: main.cpp

Revision:

4:9b1c6b9dae1c

Parent:

3:d3264a6f7a62

Child:

5:17a8d8395a50

diff -r d3264a6f7a62 -r 9b1c6b9dae1c main.cpp
--- a/main.cpp	Sat Mar 24 18:19:37 2018 +0000
+++ b/main.cpp	Sat Mar 31 18:47:48 2018 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+
+//   PIN DECLARATIONS
 DigitalOut FLdirection(PTB18); 
 DigitalOut FRdirection(PTA4);
 DigitalOut magDirection(PTB19);
@@ -10,8 +12,32 @@
 DigitalIn Start(PTC12); 
 DigitalOut enableH(PTC11);
 DigitalOut highH(PTC10);
+DigitalOut enableL(PTC11);
+DigitalOut highL(PTC7);
+I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
+Serial pc(USBTX, USBRX);
+DigitalOut LED(PTC4);
+DigitalOut green(LED_GREEN);
 
-//Notes
+
+//   PROTOTYPE FUNCTION DECLARATIONS
+void move(float dist, bool direction); 
+void grabToken();//Picks up the token for reading
+void dropToken();//Drops the token off
+void kill(); 
+void turnRight(float, bool);
+void turnLeft(float, bool);
+void rot180(); //Turns the robot around
+
+//    GLOBAL VARIABLES
+const int FORWARD = 0;
+const int BACKWARD = 1;
+const float stepSize = 0.001212; //in meters
+const float FREQUENCY = 500; //steps per second
+int sensor_addr = 41 << 1;
+
+
+//    NOTES
 /*
     -36 prewriten functions for the drop off decision
     -Possibly use another RGB sensor as a line follower
@@ -22,43 +48,18 @@
         -turn right if rgb
         -turn left if cmy
         -turn right, left, or stay based on color choice
-*/
-/*
-    int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
+
     void findPath(); //Figures out the path to take to take the disk to its drop off position
     void returnHome(); //Returns to the home white square
     void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
-    void rot90(); //Rotates the robot 90 degrees to the left
-    void rot180(); //Turns the robot around
-    void readToken(); //Gets the color data from the RGB sensor
-*/
 
 //Variables
-/*
+
     -boxSizes, 2x2, 3x3, 4x4, etc
     -legSize, 1 foot, 1.5 feet, 2 feet, etc.
     -direction choices for findPath
-
-
 */
 
-void move(int dist, bool direction); 
-void grabToken();//Picks up the token for reading
-void dropToken();//Drops the token off
-void kill(); 
-void turnRight(int, bool);
-void turnLeft(int, bool);
-
-//void rot90(); //Rotates the robot 90 degrees to the left
-//void rot180(); //Turns the robot around
-
-    const int FORWARD = 0;
-    const int BACKWARD = 1;
-    const float stepSize = 0.001212; //in meters
-    const float FREQUENCY = 500; //steps per second
-
-
-
 
 int main()
 {
@@ -76,49 +77,86 @@
     
     */
 
-     highH = 1; //This is always high for the H-Bridge
      enableH = 0; //Making sure the H-Bridge starts low and off
-     while(true) 
+     highH = 0; //This starts high for the H-Bridge
+     highL = 1; //This starts low for the H-Bridge
+     while(true) //The start button
      {
          if (Start == 0)
          break;
      }
+     
+     killAll.rise(&kill); //The kill interupt
+     
+     pc.baud(115200);
+     green = 1; // off
+    
+     // Connect to the Color sensor and verify whether we connected to the correct sensor. 
+    
+    i2c.frequency(200000);
+    
+    //   RGB Sensor Settings
+    char id_regval[1] = {146};
+    char data[1] = {0};
+    i2c.write(sensor_addr,id_regval,1, true);
+    i2c.read(sensor_addr,data,1,false);
+    if (data[0]==68) {
+        green = 0;
+        wait (2); 
+        green = 1;
+        } else {
+        green = 1; 
+    }
+    
+    //    Initialize color sensor
+    char timing_register[2] = {129,0};
+    i2c.write(sensor_addr,timing_register,2,false);
+    
+    char control_register[2] = {143,0};
+    i2c.write(sensor_addr,control_register,2,false);
+    
+    char enable_register[2] = {128,3};
+    i2c.write(sensor_addr,enable_register,2,false);
+    
+    
     
     while(true)
-    
-     {
-        killAll.rise(&kill); 
+     { 
         grabToken(); 
         move(1,FORWARD);
-        turnLeft(1, FORWARD);
+        turnLeft(0.35343, FORWARD);
+        wait(0.5);
+        turnLeft(0.35343, FORWARD);
         wait(2); 
         dropToken(); 
         wait(2);
-        move(1, BACKWARD); 
-        
      }
 }
+
 //Distance is in meters
-void move(int dist, bool direction)
+void move(float dist, bool direction)
 {
         FLdirection = direction;
         FRdirection = !direction; 
         
-        stepFL.period(1/FREQUENCY); 
+        stepFL.period(1.0/FREQUENCY); 
         stepFR.period(1/FREQUENCY);
-        stepFL.write(0.5); 
-        stepFR.write(0.5);
+        stepFL.write(0.5f); 
+        stepFR.write(0.5f);
         //dist/stepSize is the number of steps
         //1/FREQUENCY is the time per step
-        wait((dist/stepSize)*(1/FREQUENCY));
-        stepFL.period(0);
-        stepFR.period(0);
-        stepFL.write(0);
-        stepFR.write(0);
+        wait(4*(dist/stepSize)*(1/FREQUENCY));
+        stepFL.period(0.0f);
+        stepFR.period(0.0f);
+        stepFL.write(0.0f);
+        stepFR.write(0.0f);
        
 }
+
 void grabToken()
 {
+        highL = 0;
+        highH = 1;
         enableH = 1;
         wait(1);
         magDirection = 1; 
@@ -129,17 +167,22 @@
         magArm.write(0);
     
 }
+
 void dropToken()
 {  
         magDirection = 0; 
         magArm.period(0.002); 
         magArm.write(0.5); 
         wait(0.65); 
-        enableH = 0; 
         magArm.period(0); 
-        magArm.write(0);    
+        magArm.write(0);
+        highL = 1;
+        highH = 0;
+        wait(2);
+        enableH = 0;    
 }
-void turnRight(int dist, bool direction)
+
+void turnRight(float dist, bool direction)
 { 
     //Get rid of all FR occurences which will turn right motor off
     FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
@@ -150,11 +193,11 @@
     
     //dist/stepSize is the number of steps
     //1/FREQUENCY is the time per step
-    wait((dist/stepSize)*(1/FREQUENCY));
+    wait(4*(dist/stepSize)*(1/FREQUENCY));
     stepFL.period(0);
     stepFL.write(0);
 }
-void turnLeft(int dist, bool direction)
+void turnLeft(float dist, bool direction)
 { 
     //Get rid of all FL occurences which will turn left motor off
     FRdirection = !direction;    //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
@@ -165,15 +208,93 @@
     
     //dist/stepSize is the number of steps
     //1/FREQUENCY is the time per step
-    wait((dist/stepSize)*(1/FREQUENCY));
+    wait(4*(dist/stepSize)*(1/FREQUENCY));
     stepFR.period(0);
     stepFR.write(0);
 }
 void rot180()
 {
-     
+    //Get rid of all FR occurences which will turn right motor off
+    FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
+                                
+    
+    stepFL.period(1/FREQUENCY); 
+    stepFL.write(0.5); 
+    
+    //dist/stepSize is the number of steps
+    //1/FREQUENCY is the time per step
+    wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
+    stepFL.period(0);
+    stepFL.write(0);
 }
 void kill() 
 {
     exit(0);    
 }
+
+int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
+        
+    while (true) { 
+        wait(1);
+        char clear_reg[1] = {148};
+        char clear_data[2] = {0,0};
+        i2c.write(sensor_addr,clear_reg,1, true);
+        i2c.read(sensor_addr,clear_data,2, false);
+        
+        int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
+        
+        char red_reg[1] = {150};
+        char red_data[2] = {0,0};
+        i2c.write(sensor_addr,red_reg,1, true);
+        i2c.read(sensor_addr,red_data,2, false);
+        
+        int red_value = ((int)red_data[1] << 8) | red_data[0];
+        
+        char green_reg[1] = {152};
+        char green_data[2] = {0,0};
+        i2c.write(sensor_addr,green_reg,1, true);
+        i2c.read(sensor_addr,green_data,2, false);
+        
+        int green_value = ((int)green_data[1] << 8) | green_data[0];
+        
+        char blue_reg[1] = {154};
+        char blue_data[2] = {0,0};
+        i2c.write(sensor_addr,blue_reg,1, true);
+        i2c.read(sensor_addr,blue_data,2, false);
+        
+        int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
+        
+        
+        
+        //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error
+        if(blue_value<10000 && red_value>10000){
+            return(1);
+        }
+        else if(green_value>18000 && blue_value<30000){
+            return(2);   
+        }
+        else if(red_value<10000 && blue_value>15000){
+            return(3);   
+        }
+        else if(blue_value>30000 && red_value<20000){
+            return(4);   
+        }
+        else if(red_value>25000 && green_value<15000){
+            return(5);   
+        }
+        else if(red_value>50000){
+            return(6);   
+        }
+        else if(red_value<10000 && blue_value<10000){
+            return(7);   
+        }
+        else if(red_value==0){
+            return(8);
+        }
+        
+        // print sensor readings
+        
+        //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
+        //wait(0.5);
+    }
+}
\ No newline at end of file