Diff: main.cpp

Revision:

0:7e54f0e2be67

Child:

1:4ab886b72870

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 16 20:32:02 2018 +0000
@@ -0,0 +1,143 @@
+#include "mbed.h"
+
+DigitalOut FLdirection(PTB18); 
+DigitalOut FRdirection(PTA4);
+DigitalOut magDirection(PTB19);
+PwmOut stepFL(PTD3); 
+PwmOut stepFR(PTA5);
+PwmOut magArm(PTA12);
+InterruptIn killAll(PTC3); 
+DigitalOut enableH(PTC11);
+DigitalOut highH(PTC10);
+
+//Notes
+/*
+    -36 prewriten functions for the drop off decision
+    -Possibly use another RGB sensor as a line follower
+    -Decides function based on color
+    -findPath
+        -starting at bottomLeft
+        -travel up one leg
+        -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 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; //dist per second
+
+
+
+
+int main()
+{
+    //Wait for a button press
+    //Can use an interupt
+    
+    //Start a timer
+    //Will need to be a variable timer based on round number
+    
+    //Start Sequence - Get to the lines to start reading tokens
+    /*
+        -Travel 2 feet forward minus the radDistance plus the posDistance plus the armDistance
+        -Turn 90 degrees to the left with the left wheel stationary
+        -Back up the radDistance
+    
+    */
+
+     highH = 1; //This is always high for the H-Bridge
+     enableH = 0; //Making sure the H-Bridge starts low and off
+    
+    while(true)
+    
+     {
+        killAll.rise(&kill); 
+        grabToken(); 
+        move(1,FORWARD);
+        wait(2); 
+        dropToken(); 
+        wait(2);
+        move(1, BACKWARD); 
+        
+     }
+}
+//Distance is in meters
+void move(int dist, bool direction)
+{
+        FLdirection = direction;
+        FRdirection = !direction; 
+        
+        stepFL.period(1/FREQUENCY); 
+        stepFR.period(1/FREQUENCY);
+        stepFL.write(0.5); 
+        stepFR.write(0.5);
+        //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);
+       
+}
+void grabToken()
+{
+        enableH = 1;
+        wait(1);
+        magDirection = 1; 
+        magArm.period(0.002);
+        magArm.write(0.5);
+        wait(0.65);
+        magArm.period(0);
+        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);    
+}
+void rot90() 
+{
+       
+}
+void rot180()
+{
+     
+}
+void kill() 
+{
+    exit(0);    
+}