main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "motordriver.h"
00004 #define PI 3.14159
00005 
00006 //IMU used for monitoring orientation
00007 // - wired using i2c in cookbook example here
00008 //   https://developer.mbed.org/components/LSM9DS1-IMU/
00009 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00010 float initial_direction;
00011 
00012 //wheel motors
00013 Motor left(p21, p20, p23, 1);
00014 Motor right(p26, p25, p24, 1);
00015 
00016 //arm motor
00017 Motor arm(p22, p7, p6,1);
00018 
00019 //Color sensor, used to assist robot positioning
00020 // - wired using i2c according to
00021 //   https://developer.mbed.org/users/raj1995/notebook/adafruit-tcs34725-rgb-color-sensor/
00022 I2C color_sensor(p28, p27); //pins for I2C communication (SDA, SCL)
00023 int color_addr = 41 << 1;
00024 int color_thresh = 4500;//threshold for green before it triggers
00025 bool on_color = false;
00026 DigitalOut green(LED1);
00027 
00028 //pc serial connection, for debugging purposes
00029 Serial pc(USBTX, USBRX);
00030 
00031 //initialize and calibrate IMU
00032 
00033 /*void initIMU(){
00034     IMU.begin();
00035     if (!IMU.begin()) {
00036         pc.printf("Failed to communicate with LSM9DS1.\n");
00037     }
00038     IMU.calibrate(1);
00039     IMU.calibrateMag(0);   
00040     
00041    initial_direction = getDirection();
00042 }*/
00043 
00044 //verify color sensor and initialize
00045 void initColorSensor(){
00046     pc.baud(9600);
00047     
00048     // Connect to the Color sensor and verify
00049     color_sensor.frequency(200000);
00050     
00051     char id_regval[1] = {146};
00052     char data[1] = {0};
00053     color_sensor.write(color_addr,id_regval,1, true);
00054     color_sensor.read(color_addr,data,1,false);
00055 
00056     // Initialize color sensor
00057     
00058     char timing_register[2] = {129,0};
00059     color_sensor.write(color_addr,timing_register,2,false);
00060     
00061     char control_register[2] = {143,0};
00062     color_sensor.write(color_addr,control_register,2,false);
00063     
00064     char enable_register[2] = {128,3};
00065     color_sensor.write(color_addr,enable_register,2,false);   
00066 }
00067 
00068 void loadArm(){
00069     arm.speed(-.3);
00070     wait(2.7);
00071     arm.speed(0);        
00072 }
00073 
00074 //initialization for all components
00075 void init(){
00076     //initIMU();    
00077     initColorSensor();
00078     loadArm();
00079 }
00080 
00081 // Sends a request to the server and waits for a path response
00082 char* getPathFromServer(){
00083     //use wifi module here
00084     //https://developer.mbed.org/users/electromotivated/notebook/wifi-pid-redbot-robot-webserver/    
00085     char* path = "";
00086     return path;
00087 }
00088 
00089 //gets the orientation from imu
00090 float getDirection(){
00091     float sum = 0;
00092     int target_samples = 100;
00093     int num_samples = 0;
00094     while(num_samples++ < target_samples){
00095         
00096         while(!IMU.magAvailable(X_AXIS));
00097             IMU.readMag();
00098         float my = IMU.calcMag(IMU.my);
00099         float mx = IMU.calcMag(IMU.mx);
00100         float dir;
00101         if (my == 0.0)
00102             dir = (mx < 0.0) ? 180.0 : 0.0;
00103         else
00104             dir = atan2(mx, my)*360.0/(2.0*PI);
00105         sum += dir;
00106     }
00107     
00108     return sum / target_samples;
00109 }
00110 
00111 float getRelativeDirection(){
00112     return getDirection() - initial_direction;    
00113 }
00114 
00115 //returns true when color is entered (subsequently returns false until color is exited and entered again)
00116 bool colorEntered(){
00117     char red_reg[1] = {150};
00118     char red_data[2] = {0,0};
00119     color_sensor.write(color_addr,red_reg,1, true);
00120     color_sensor.read(color_addr,red_data,2, false);
00121     
00122     int red_value = ((int)red_data[1] << 8) | red_data[0];
00123 
00124     bool is_red = red_value > color_thresh;
00125     bool color_triggered = !on_color && is_red;
00126     on_color = is_red;
00127     //pc.printf("%d\r\n",red_value);
00128     return color_triggered;
00129 }
00130 
00131 //technologies for navigation may include
00132 // - Line following https://www.sparkfun.com/products/11769
00133 // - color sensor (indicates when end of movement is reached)
00134 void moveForward(){}
00135 void rotateLeft(){
00136     left.speed(0.3);
00137     right.speed(0.3);
00138     while(!colorEntered()) wait(.01);
00139 }
00140 void rotateRight(){}
00141 
00142 //arem technology is yet to be determined, will probably involve adjusting a motor
00143 void armUp(){
00144     arm.speed(.3);
00145     wait(2.5);
00146     arm.speed(0);            
00147 }
00148 void armDown(){
00149     arm.speed(-.3);
00150     wait(2.5);
00151     arm.speed(0);                
00152 }
00153 
00154 // Loops through path commands and executes each one sequentially
00155 void executePath(char* path){
00156     int index = 0;
00157     while(path[index] != '\0'){
00158         switch(path[index]){
00159             case 'F': moveForward(); break;    
00160             case 'L': rotateLeft(); break;
00161             case 'R': rotateRight(); break;
00162             case 'U': armUp(); break;
00163             case 'D': armDown(); break;
00164         }    
00165     }
00166 }
00167 
00168 //main code
00169 //- repeatedly listens for paths and then executes them
00170 int main() {
00171     init();
00172     //rotateLeft();
00173     wait(5);
00174     while(true){
00175         armUp();
00176         wait(3);
00177         armDown();
00178         wait(3);
00179         /*while(!colorEntered()) wait(.1);
00180         green = 1;
00181         wait(5);
00182         green = 0;
00183         rotateLeft();
00184         left.speed(0);
00185         right.speed(0);
00186         wait(1);*/
00187         /*green = colorEntered();
00188         pc.printf("%f\r\n",getDirection());
00189         pc.printf("%f\r\n",getRelativeDirection());
00190         char* path = getPathFromServer(); */
00191         //executePath(path);
00192         //wait(.1);
00193     }
00194 }