Diff: main.cpp

Revision:

1:c162c077430f

Parent:

0:bdd172e29b8b

--- a/main.cpp	Mon Dec 10 23:38:39 2018 +0000
+++ b/main.cpp	Wed Dec 12 05:24:22 2018 +0000
@@ -2,10 +2,12 @@
 #include "Servo.h"
 #include "rtos.h"
 
+
 Servo shoulder(p26); // pwm
 Servo elbow(p25); // pwm
 Servo wrist(p24); // pwm
 Servo hand(p23); // pwm
+DigitalIn pb(p8);
 
 I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
 Serial pc(USBTX, USBRX);    //Used to view the colors that are read in
@@ -20,18 +22,26 @@
 float newRedValue;
 float oldRedValue;
 
+int new_pb = 0;
+int old_pb = 0;
+bool state = 0;
+
 
  
 DigitalOut green(LED1);
 DigitalOut led4(LED4);
 DigitalOut led(p11);
+DigitalOut led3(LED3);
 
 void stroke();
 void dunk();
 void rotate();
 void readLight();
+void gohere();
 
 float shoulderPosition;
+float sh;
+//float shoulder;
 //
 Thread threadLight;
 Thread threadRotate;
@@ -44,62 +54,141 @@
 int main()
 {   
 
-threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds) 
-threadRotate.start(rotate);// to stop thread thread_name.terminate();
+    pb.mode(PullUp);
 
-
-
-
+    while(1){
+            
+                new_pb=pb;
+                
+                    if (state == 0){                             
+                            
+                            led4=0;
+                                                    
+                         
+                             
+                             if((new_pb == 0) && (old_pb==1)){
+                                    
+                                        state =1;
+                                            
+                                              }
+                                                                
+                                }
+                      else {
+                                 
+                                 led4=1;
+                           //      stroke();
+                                 threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds) 
+                                 threadRotate.start(rotate);// to stop thread thread_name.terminate();
+                                
+                                     
+                             if((new_pb == 0) && (old_pb==1)){
+                                    
+                                        state =0;
+                                            
+                                              }                                                               
+                                
+                                }          
+                                
+                    old_pb = new_pb;     
+                                
+            }
 
 
 
 }
 
+
 void rotate(){
-    wrist = 0.0f;
-    elbow = 0.30f;
     
+    // shoulder=0.37f;
+     wrist = 0.1f;
+     elbow = 0.10f;
+     
+   // elbow = 0.00f;
     //while(doRotate){
-
-    float count = 1.0f;
-   
-           while (count>=0.0f) {
-         
-             
-        float sCount = count/2;
-        
-        if(newRedValue <= oldRedValue - .04 ){
-        shoulderPosition = 0.5f + sCount;
-        
-        }
-        shoulder = 0.0f + sCount;;
-        count-= 0.01f;
+    
     
-       // wait(.05);
-        Thread::wait(50);
-
-    }
-   
-   
-    while (count<=1.0f) {
-       
-        float sCount = count/2;
-        
-        
-        shoulder = 0.0f + sCount;
-        count+= 0.01f;
-        Thread::wait(50);
-        
-        //wait(.05);
-
-    }
-    
+for (float i = 0.44f; i > 0.0f; i-=0.01f) {// Forward cycle
+                
+                led3=0;
+                shoulder=i;
+                wait_ms(100);
+               // pc.printf("shoulder %.2f  \n",i);
+                
+                                  }
+                                  
+                                  
+for (float j = 0; j < 0.34f; j+=0.01f) {  // Return cycle take reading
+                
+                led3=0;
+                shoulder=j;
+                wait_ms(100);
+                
+                if(newRedValue <= 0.615     ){//abs(newRedValue-oldRedValue)>=0.03){  //newRedValue <= oldRedValue - .02 ){
+ 
+                    shoulderPosition = shoulder;
+                    
+                 }
+               
+          //     pc.printf("shoulder %.2f \n",j);
+                
+                                  }
+                                  
+ for (float j = 0.34; j < 0.50f; j+=0.01f) {  // Return cycle 
+                
+                led3=1;
+                shoulder=j;
+                wait_ms(100);
+                
+        //        pc.printf("shoulder %.2f \n",j);
+                
+                                  }                                 
+     
 
     
+//    float count = 1.0f;
+//   
+//         while (count>=0.0f) {
+//         
+//                  led3=1;
+//                  float sCount = count/2;
+//        
+//                  shoulder = 0.0f + sCount;
+//                  sh = 0.0f + sCount;
+//        
+//        
+//                  count-= 0.01f;
+//    
+//                 // wait(.05);
+//                  Thread::wait(50);
+//
+//    }
+//   
+//   
+//    while (count<=1.0f) {
+//       
+//       led3=0;
+//        float sCount = count/2;
+//        
+//        
+//        shoulder = 0.0f + sCount;
+//               
+//        if(newRedValue <= oldRedValue - .04 ){
+//      //  shoulderPosition = 0.5f + sCount;
+//           shoulderPosition = shoulder;
+//        }
+//        
+//        count+= 0.01f;
+//        Thread::wait(50);
+//        
+//        //wait(.05);
+//
+//    }
     
     threadLight.terminate();
+
     
-     dunk();
+    dunk();
     
     threadRotate.terminate();
    
@@ -109,9 +198,12 @@
     
 }
 
+
 void stroke(){
     
-    shoulder = shoulderPosition;
+   shoulder = shoulderPosition-.10;
+   
+    Thread::wait(2000);
     
     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
         float wCount =  i/1.5;
@@ -125,6 +217,7 @@
 
 
     }
+    
     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
         float wCount =  i/1.5;
         float eCount = i/2;
@@ -136,11 +229,12 @@
         wait(.05);
 
     } 
+    
 }
 
 void dunk(){
         
-        shoulder = 0.3f;
+        shoulder = 0.50f;
         
         float wristIN;
         float elbowIN;
@@ -248,12 +342,18 @@
         newRedValue = red_value;
         green_value = green_value/clear_value;
         blue_value = blue_value/clear_value;
-        pc.printf("Clear (%.2f), Red (%.2f), old red (%.2f), new red (%.2f)\n, Shoulder position %.2f \n", clear_value, red_value, oldRedValue, newRedValue, shoulderPosition);
+        //pc.printf("Red (%.2f), old red (%.2f), new red (%.2f), shoul(%.2f)\n, Shoulder position %.2f \n", red_value, oldRedValue, newRedValue,sh ,shoulderPosition);
         //The above code displays the red, green, and blue values read in by the color sensor.
         //wait(0.5);
-         Thread::wait(300);
+       
+         
+         pc.printf("new Red Value %.2f  old Red value %.2f shoulder  %.2f \n",newRedValue,oldRedValue,shoulderPosition);
+       //    pc.printf("%.2f\n",newRedValue);
          
-         oldRedValue = newRedValue;
+         
+         Thread::wait(300);
+        oldRedValue = newRedValue;
+         
     
     
 }