Diff: main.cpp

Revision:

5:507618748cb4

Parent:

4:104acd180bb1

Child:

6:2fa349da33bb

diff -r 104acd180bb1 -r 507618748cb4 main.cpp
--- a/main.cpp	Fri Dec 02 07:38:22 2016 +0000
+++ b/main.cpp	Mon Dec 05 22:45:09 2016 +0000
@@ -1,24 +1,31 @@
 #include "mbed.h"
 #include "LSM9DS1.h"
+#include "motordriver.h"
 #define PI 3.14159
 
 //IMU used for monitoring orientation
 // - wired using i2c in cookbook example here
 //   https://developer.mbed.org/components/LSM9DS1-IMU/
 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+float initial_direction;
+Motor left(p21, p22, p23, 1);
+Motor right(p26, p25, p24, 1);
+
 
 //Color sensor, used to assist robot positioning
 // - wired using i2c according to
 //   https://developer.mbed.org/users/raj1995/notebook/adafruit-tcs34725-rgb-color-sensor/
 I2C color_sensor(p28, p27); //pins for I2C communication (SDA, SCL)
 int color_addr = 41 << 1;
-int color_thresh = 10000;//threshold for green before it triggers
+int color_thresh = 4500;//threshold for green before it triggers
 bool on_color = false;
+DigitalOut green(LED1);
 
 //pc serial connection, for debugging purposes
 Serial pc(USBTX, USBRX);
 
 //initialize and calibrate IMU
+/*
 void initIMU(){
     IMU.begin();
     if (!IMU.begin()) {
@@ -26,8 +33,10 @@
     }
     IMU.calibrate(1);
     IMU.calibrateMag(0);   
+    
+   // initial_direction = getDirection();
 }
-
+*/
 //verify color sensor and initialize
 void initColorSensor(){
     pc.baud(9600);
@@ -54,7 +63,7 @@
 
 //initialization for all components
 void init(){
-    initIMU();    
+    //initIMU();    
     initColorSensor();
 }
 
@@ -72,6 +81,7 @@
     int target_samples = 100;
     int num_samples = 0;
     while(num_samples++ < target_samples){
+        
         while(!IMU.magAvailable(X_AXIS));
             IMU.readMag();
         float my = IMU.calcMag(IMU.my);
@@ -83,21 +93,27 @@
             dir = atan2(mx, my)*360.0/(2.0*PI);
         sum += dir;
     }
+    
     return sum / target_samples;
 }
 
+float getRelativeDirection(){
+    return getDirection() - initial_direction;    
+}
+
 //returns true when color is entered (subsequently returns false until color is exited and entered again)
 bool colorEntered(){
-    char green_reg[1] = {152};
-    char green_data[2] = {0,0};
-    color_sensor.write(color_addr,green_reg,1, true);
-    color_sensor.read(color_addr,green_data,2, false);
+    char red_reg[1] = {150};
+    char red_data[2] = {0,0};
+    color_sensor.write(color_addr,red_reg,1, true);
+    color_sensor.read(color_addr,red_data,2, false);
     
-    int green_value = ((int)green_data[1] << 8) | green_data[0];
+    int red_value = ((int)red_data[1] << 8) | red_data[0];
 
-    bool is_green = green_value > color_thresh;
-    bool color_triggered = !on_color && is_green;
-    on_color = is_green;
+    bool is_red = red_value > color_thresh;
+    bool color_triggered = !on_color && is_red;
+    on_color = is_red;
+    //pc.printf("%d\r\n",red_value);
     return color_triggered;
 }
 
@@ -105,7 +121,11 @@
 // - Line following https://www.sparkfun.com/products/11769
 // - color sensor (indicates when end of movement is reached)
 void moveForward(){}
-void rotateLeft(){}
+void rotateLeft(){
+    left.speed(0.3);
+    right.speed(0.3);
+    while(!colorEntered()) wait(.01);
+}
 void rotateRight(){}
 
 //arem technology is yet to be determined, will probably involve adjusting a motor
@@ -131,7 +151,19 @@
 int main() {
     init();
     while(true){
-        char* path = getPathFromServer(); 
-        executePath(path);
+        while(!colorEntered()) wait(.1);
+        green = 1;
+        wait(5);
+        green = 0;
+        rotateLeft();
+        left.speed(0);
+        right.speed(0);
+        wait(1);
+        /*green = colorEntered();
+        pc.printf("%f\r\n",getDirection());
+        pc.printf("%f\r\n",getRelativeDirection());
+        char* path = getPathFromServer(); */
+        //executePath(path);
+        //wait(.1);
     }
 }