Revision 1:a586a937ad84, committed 2018-12-13

Comitter:

monwonga3

Date:

Thu Dec 13 06:12:05 2018 +0000

Parent:

0:bdd172e29b8b

Commit message:

Nail Polish Applicator mbed code.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PinDetect.lib	Thu Dec 13 06:12:05 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- a/main.cpp	Mon Dec 10 23:38:39 2018 +0000
+++ b/main.cpp	Thu Dec 13 06:12:05 2018 +0000
@@ -1,53 +1,57 @@
 #include "mbed.h"
 #include "Servo.h"
 #include "rtos.h"
+#include "PinDetect.h"
 
 Servo shoulder(p26); // pwm
 Servo elbow(p25); // pwm
 Servo wrist(p24); // pwm
 Servo hand(p23); // pwm
 
-I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
-Serial pc(USBTX, USBRX);    //Used to view the colors that are read in
- 
-int sensor_addr = 41 << 1;
+PinDetect button(p8);
+bool buttonHit = false;
 
-float clear_value;
-float red_value;
-float blue_value;
-float green_value;
-
-float newRedValue;
-float oldRedValue;
-
-
- 
-DigitalOut green(LED1);
-DigitalOut led4(LED4);
-DigitalOut led(p11);
+float currentShoulderPosition;
+float currentElbowPosition;
+float currentWristPosition;
 
 void stroke();
 void dunk();
-void rotate();
-void readLight();
 
-float shoulderPosition;
-//
-Thread threadLight;
-Thread threadRotate;
-
-void blinky() {
-        led4 = !led4;
-        //wait(0.2);
-    }
+void button_hit_callback (void) {
+    buttonHit = !buttonHit;
+}
 
 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();
+    currentShoulderPosition = 0.0f;
+    currentElbowPosition = 0.0f;
+    currentWristPosition = 0.0f;
+    
+    shoulder = currentShoulderPosition;
+    elbow = currentElbowPosition;
+    wrist = currentWristPosition;
+    
+    // Use internal pullup for pushbutton
+    button.mode(PullUp);
+    // Delay for initial pullup to take effect
+    wait(.001);
+    // Setup Interrupt callback function for a pb hit
+    button.attach_deasserted(&button_hit_callback);
+    // Start sampling pb input using interrupts
+    button.setSampleFrequency();
 
-
+        
+        
+while(1){
+             
+                while(buttonHit){
+                dunk();
+                stroke();
+                wait(.1);
+                }
+}
 
 
 
@@ -55,120 +59,69 @@
 
 }
 
-void rotate(){
-    wrist = 0.0f;
-    elbow = 0.30f;
+
+void stroke(){
     
-    //while(doRotate){
-
-    float count = 1.0f;
-   
-           while (count>=0.0f) {
-         
-             
-        float sCount = count/2;
-        
-        if(newRedValue <= oldRedValue - .04 ){
-        shoulderPosition = 0.5f + sCount;
-        
+    while(currentShoulderPosition >0.0f){
+        currentShoulderPosition -= .001f;
+        shoulder = currentShoulderPosition;
+        wait(.01);
         }
-        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);
-
-    }
     
 
     
-    
-    threadLight.terminate();
-    
-     dunk();
-    
-    threadRotate.terminate();
-   
-
-   //} 
-    
-    
-}
+    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
+         currentWristPosition =  i/1.7f;
+         currentElbowPosition = 0.19f + i/2;
 
-void stroke(){
-    
-    shoulder = shoulderPosition;
-    
-    for (float i = 0.0f; i < 0.5f; i+=0.01f) {
-        float wCount =  i/1.5;
-        float eCount = i/2;
-        //hand = 1.5f ;
-        wrist = 0.0f + wCount;
-        elbow =  0.19f + eCount;//wCount; //0.47f;
+        wrist = currentWristPosition;// increments to .29
+        elbow = currentElbowPosition;//increments to .47
         wait(.05);
-        //printf("wCount is %f \n", wCount);
-//        printf("eCount is %f \n", eCount);
 
 
     }
     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
-        float wCount =  i/1.5;
-        float eCount = i/2;
-        //hand = 1.5f ;
-        wrist = 0.33f - wCount;
-        elbow =  0.44f - eCount;//wCount; //0.47f;
-        //printf("wCount is %f \n", wCount);
-//        printf("eCount is %f \n", eCount);
+         currentWristPosition -=  i/1.7;
+         currentElbowPosition  -= i/2;
+        
+        wrist = currentWristPosition;//increments to 0.0f
+        elbow = currentElbowPosition;//increments to .19f
+        
         wait(.05);
-
     } 
 }
 
 void dunk(){
         
-        shoulder = 0.3f;
+        while(currentShoulderPosition <0.3f){
+        currentShoulderPosition += .001f;
+        shoulder = currentShoulderPosition;
+        wait(.01);
+        }
         
-        float wristIN;
-        float elbowIN;
+        while(currentElbowPosition <0.3f){
+        currentElbowPosition += .001f;
+        elbow = currentElbowPosition;
+        wait(.01);
+        }
+
+ 
    
     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
-        float wCount =  i/1.5;
-        float eCount = i/2;
-        //hand = 1.5f ;
-        wristIN = 0.2f - wCount;//equals.33f at end
-        //elbowIN =  0.5f + eCount;//equals .44f at the end
-        //wait(.5);
-        wrist = wristIN;
-        elbow = 0.3f;
-        //
-//     printf("wCount is %f \n", wCount);
-//       printf("eCount is %f \n", eCount);
+        
+        currentWristPosition = 0.2f - i/1.5;//increments to -.2f
+
+        wrist = currentWristPosition;
+
+
      wait(.05);
 
     }
     for (float i = 0.0f; i < 0.5f; i+=0.01f) {
-        float wCount =  i/1.5;
-        float eCount = i/2;
-        //hand = 1.5f ;
-        //wait(.5);
-        wrist = wristIN + wCount;
-        //elbow = wristIN - eCount;//wCount; //0.47f;
-//        printf("wCount is %f \n", wCount);
-//        printf("eCount is %f \n", eCount);
+ 
+        currentWristPosition = 0.2f + i/1.5;//increments to .47f
+        wrist = currentWristPosition;
+
     wait(.05);
 
     } 
@@ -178,87 +131,3 @@
 }
 
 
-void readLight () {
-    
-    
-    pc.baud(9600);
-    green = 1; // off
-    
-    // Connect to the Color sensor and verify
-    
-    i2c.frequency(200000);
-    
-    char id_regval[1] = {146};
-    char data[1] = {0};
-    i2c.write(sensor_addr,id_regval,1, true);
-    i2c.read(sensor_addr,data,1,false);
-    
-    if (data[0]==68) {
-        green = 0;
-        wait (2); 
-        green = 1;
-        } else {
-        green = 1; 
-    }
-    
-    // Initialize color sensor
-    
-    char timing_register[2] = {129,0};
-    i2c.write(sensor_addr,timing_register,2,false);
-    
-    char control_register[2] = {143,0};
-    i2c.write(sensor_addr,control_register,2,false);
-    
-    char enable_register[2] = {128,3};
-    i2c.write(sensor_addr,enable_register,2,false);
-    
-    // Read data from color sensor (Clear/Red/Green/Blue)
-    led = 1;
-  while (true) { 
-        char clear_reg[1] = {148};
-        char clear_data[2] = {0,0};
-        i2c.write(sensor_addr,clear_reg,1, true);
-        i2c.read(sensor_addr,clear_data,2, false);
-        
-         clear_value = ((int)clear_data[1] << 8) | clear_data[0];
-        
-        char red_reg[1] = {150};
-        char red_data[2] = {0,0};
-        i2c.write(sensor_addr,red_reg,1, true);
-        i2c.read(sensor_addr,red_data,2, false);
-        
-         red_value = ((int)red_data[1] << 8) | red_data[0];
-        
-        char green_reg[1] = {152};
-        char green_data[2] = {0,0};
-        i2c.write(sensor_addr,green_reg,1, true);
-        i2c.read(sensor_addr,green_data,2, false);
-        
-         green_value = ((int)green_data[1] << 8) | green_data[0];
-        
-        char blue_reg[1] = {154};
-        char blue_data[2] = {0,0};
-        i2c.write(sensor_addr,blue_reg,1, true);
-        i2c.read(sensor_addr,blue_data,2, false);
-        
-         blue_value = ((int)blue_data[1] << 8) | blue_data[0];
-        
-        // print sensor readings
-        red_value = red_value / clear_value;
-        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);
-        //The above code displays the red, green, and blue values read in by the color sensor.
-        //wait(0.5);
-         Thread::wait(300);
-         
-         oldRedValue = newRedValue;
-    
-    
-}
-    
-    
-    
-}
-