main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "rtos.h"
00004 
00005 
00006 Servo shoulder(p26); // pwm
00007 Servo elbow(p25); // pwm
00008 Servo wrist(p24); // pwm
00009 Servo hand(p23); // pwm
00010 DigitalIn pb(p8);
00011 
00012 I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
00013 Serial pc(USBTX, USBRX);    //Used to view the colors that are read in
00014  
00015 int sensor_addr = 41 << 1;
00016 
00017 float clear_value;
00018 float red_value;
00019 float blue_value;
00020 float green_value;
00021 
00022 float newRedValue;
00023 float oldRedValue;
00024 
00025 int new_pb = 0;
00026 int old_pb = 0;
00027 bool state = 0;
00028 
00029 
00030  
00031 DigitalOut green(LED1);
00032 DigitalOut led4(LED4);
00033 DigitalOut led(p11);
00034 DigitalOut led3(LED3);
00035 
00036 void stroke();
00037 void dunk();
00038 void rotate();
00039 void readLight();
00040 void gohere();
00041 
00042 float shoulderPosition;
00043 float sh;
00044 //float shoulder;
00045 //
00046 Thread threadLight;
00047 Thread threadRotate;
00048 
00049 void blinky() {
00050         led4 = !led4;
00051         //wait(0.2);
00052     }
00053 
00054 int main()
00055 {   
00056 
00057     pb.mode(PullUp);
00058 
00059     while(1){
00060             
00061                 new_pb=pb;
00062                 
00063                     if (state == 0){                             
00064                             
00065                             led4=0;
00066                                                     
00067                          
00068                              
00069                              if((new_pb == 0) && (old_pb==1)){
00070                                     
00071                                         state =1;
00072                                             
00073                                               }
00074                                                                 
00075                                 }
00076                       else {
00077                                  
00078                                  led4=1;
00079                            //      stroke();
00080                                  threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds) 
00081                                  threadRotate.start(rotate);// to stop thread thread_name.terminate();
00082                                 
00083                                      
00084                              if((new_pb == 0) && (old_pb==1)){
00085                                     
00086                                         state =0;
00087                                             
00088                                               }                                                               
00089                                 
00090                                 }          
00091                                 
00092                     old_pb = new_pb;     
00093                                 
00094             }
00095 
00096 
00097 
00098 }
00099 
00100 
00101 void rotate(){
00102     
00103     // shoulder=0.37f;
00104      wrist = 0.1f;
00105      elbow = 0.10f;
00106      
00107    // elbow = 0.00f;
00108     //while(doRotate){
00109     
00110     
00111 for (float i = 0.44f; i > 0.0f; i-=0.01f) {// Forward cycle
00112                 
00113                 led3=0;
00114                 shoulder=i;
00115                 wait_ms(100);
00116                // pc.printf("shoulder %.2f  \n",i);
00117                 
00118                                   }
00119                                   
00120                                   
00121 for (float j = 0; j < 0.34f; j+=0.01f) {  // Return cycle take reading
00122                 
00123                 led3=0;
00124                 shoulder=j;
00125                 wait_ms(100);
00126                 
00127                 if(newRedValue <= 0.615     ){//abs(newRedValue-oldRedValue)>=0.03){  //newRedValue <= oldRedValue - .02 ){
00128  
00129                     shoulderPosition = shoulder;
00130                     
00131                  }
00132                
00133           //     pc.printf("shoulder %.2f \n",j);
00134                 
00135                                   }
00136                                   
00137  for (float j = 0.34; j < 0.50f; j+=0.01f) {  // Return cycle 
00138                 
00139                 led3=1;
00140                 shoulder=j;
00141                 wait_ms(100);
00142                 
00143         //        pc.printf("shoulder %.2f \n",j);
00144                 
00145                                   }                                 
00146      
00147 
00148     
00149 //    float count = 1.0f;
00150 //   
00151 //         while (count>=0.0f) {
00152 //         
00153 //                  led3=1;
00154 //                  float sCount = count/2;
00155 //        
00156 //                  shoulder = 0.0f + sCount;
00157 //                  sh = 0.0f + sCount;
00158 //        
00159 //        
00160 //                  count-= 0.01f;
00161 //    
00162 //                 // wait(.05);
00163 //                  Thread::wait(50);
00164 //
00165 //    }
00166 //   
00167 //   
00168 //    while (count<=1.0f) {
00169 //       
00170 //       led3=0;
00171 //        float sCount = count/2;
00172 //        
00173 //        
00174 //        shoulder = 0.0f + sCount;
00175 //               
00176 //        if(newRedValue <= oldRedValue - .04 ){
00177 //      //  shoulderPosition = 0.5f + sCount;
00178 //           shoulderPosition = shoulder;
00179 //        }
00180 //        
00181 //        count+= 0.01f;
00182 //        Thread::wait(50);
00183 //        
00184 //        //wait(.05);
00185 //
00186 //    }
00187     
00188     threadLight.terminate();
00189 
00190     
00191     dunk();
00192     
00193     threadRotate.terminate();
00194    
00195 
00196    //} 
00197     
00198     
00199 }
00200 
00201 
00202 void stroke(){
00203     
00204    shoulder = shoulderPosition-.10;
00205    
00206     Thread::wait(2000);
00207     
00208     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00209         float wCount =  i/1.5;
00210         float eCount = i/2;
00211         //hand = 1.5f ;
00212         wrist = 0.0f + wCount;
00213         elbow =  0.19f + eCount;//wCount; //0.47f;
00214         wait(.05);
00215         //printf("wCount is %f \n", wCount);
00216 //        printf("eCount is %f \n", eCount);
00217 
00218 
00219     }
00220     
00221     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00222         float wCount =  i/1.5;
00223         float eCount = i/2;
00224         //hand = 1.5f ;
00225         wrist = 0.33f - wCount;
00226         elbow =  0.44f - eCount;//wCount; //0.47f;
00227         //printf("wCount is %f \n", wCount);
00228 //        printf("eCount is %f \n", eCount);
00229         wait(.05);
00230 
00231     } 
00232     
00233 }
00234 
00235 void dunk(){
00236         
00237         shoulder = 0.50f;
00238         
00239         float wristIN;
00240         float elbowIN;
00241    
00242     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00243         float wCount =  i/1.5;
00244         float eCount = i/2;
00245         //hand = 1.5f ;
00246         wristIN = 0.2f - wCount;//equals.33f at end
00247         //elbowIN =  0.5f + eCount;//equals .44f at the end
00248         //wait(.5);
00249         wrist = wristIN;
00250         elbow = 0.3f;
00251         //
00252 //     printf("wCount is %f \n", wCount);
00253 //       printf("eCount is %f \n", eCount);
00254      wait(.05);
00255 
00256     }
00257     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
00258         float wCount =  i/1.5;
00259         float eCount = i/2;
00260         //hand = 1.5f ;
00261         //wait(.5);
00262         wrist = wristIN + wCount;
00263         //elbow = wristIN - eCount;//wCount; //0.47f;
00264 //        printf("wCount is %f \n", wCount);
00265 //        printf("eCount is %f \n", eCount);
00266     wait(.05);
00267 
00268     } 
00269     
00270     stroke();
00271     
00272 }
00273 
00274 
00275 void readLight () {
00276     
00277     
00278     pc.baud(9600);
00279     green = 1; // off
00280     
00281     // Connect to the Color sensor and verify
00282     
00283     i2c.frequency(200000);
00284     
00285     char id_regval[1] = {146};
00286     char data[1] = {0};
00287     i2c.write(sensor_addr,id_regval,1, true);
00288     i2c.read(sensor_addr,data,1,false);
00289     
00290     if (data[0]==68) {
00291         green = 0;
00292         wait (2); 
00293         green = 1;
00294         } else {
00295         green = 1; 
00296     }
00297     
00298     // Initialize color sensor
00299     
00300     char timing_register[2] = {129,0};
00301     i2c.write(sensor_addr,timing_register,2,false);
00302     
00303     char control_register[2] = {143,0};
00304     i2c.write(sensor_addr,control_register,2,false);
00305     
00306     char enable_register[2] = {128,3};
00307     i2c.write(sensor_addr,enable_register,2,false);
00308     
00309     // Read data from color sensor (Clear/Red/Green/Blue)
00310     led = 1;
00311   while (true) { 
00312         char clear_reg[1] = {148};
00313         char clear_data[2] = {0,0};
00314         i2c.write(sensor_addr,clear_reg,1, true);
00315         i2c.read(sensor_addr,clear_data,2, false);
00316         
00317          clear_value = ((int)clear_data[1] << 8) | clear_data[0];
00318         
00319         char red_reg[1] = {150};
00320         char red_data[2] = {0,0};
00321         i2c.write(sensor_addr,red_reg,1, true);
00322         i2c.read(sensor_addr,red_data,2, false);
00323         
00324          red_value = ((int)red_data[1] << 8) | red_data[0];
00325         
00326         char green_reg[1] = {152};
00327         char green_data[2] = {0,0};
00328         i2c.write(sensor_addr,green_reg,1, true);
00329         i2c.read(sensor_addr,green_data,2, false);
00330         
00331          green_value = ((int)green_data[1] << 8) | green_data[0];
00332         
00333         char blue_reg[1] = {154};
00334         char blue_data[2] = {0,0};
00335         i2c.write(sensor_addr,blue_reg,1, true);
00336         i2c.read(sensor_addr,blue_data,2, false);
00337         
00338          blue_value = ((int)blue_data[1] << 8) | blue_data[0];
00339         
00340         // print sensor readings
00341         red_value = red_value / clear_value;
00342         newRedValue = red_value;
00343         green_value = green_value/clear_value;
00344         blue_value = blue_value/clear_value;
00345         //pc.printf("Red (%.2f), old red (%.2f), new red (%.2f), shoul(%.2f)\n, Shoulder position %.2f \n", red_value, oldRedValue, newRedValue,sh ,shoulderPosition);
00346         //The above code displays the red, green, and blue values read in by the color sensor.
00347         //wait(0.5);
00348        
00349          
00350          pc.printf("new Red Value %.2f  old Red value %.2f shoulder  %.2f \n",newRedValue,oldRedValue,shoulderPosition);
00351        //    pc.printf("%.2f\n",newRedValue);
00352          
00353          
00354          Thread::wait(300);
00355         oldRedValue = newRedValue;
00356          
00357     
00358     
00359 }
00360     
00361     
00362     
00363 }
00364