Diff: main.cpp

Revision:

0:bdd172e29b8b

Child:

1:c162c077430f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 10 23:38:39 2018 +0000
@@ -0,0 +1,264 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.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;
+
+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);
+
+void stroke();
+void dunk();
+void rotate();
+void readLight();
+
+float shoulderPosition;
+//
+Thread threadLight;
+Thread threadRotate;
+
+void blinky() {
+        led4 = !led4;
+        //wait(0.2);
+    }
+
+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();
+
+
+
+
+
+
+
+}
+
+void rotate(){
+    wrist = 0.0f;
+    elbow = 0.30f;
+    
+    //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);
+
+    }
+    
+
+    
+    
+    threadLight.terminate();
+    
+     dunk();
+    
+    threadRotate.terminate();
+   
+
+   //} 
+    
+    
+}
+
+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;
+        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);
+        wait(.05);
+
+    } 
+}
+
+void dunk(){
+        
+        shoulder = 0.3f;
+        
+        float wristIN;
+        float elbowIN;
+   
+    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);
+     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);
+    wait(.05);
+
+    } 
+    
+    stroke();
+    
+}
+
+
+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;
+    
+    
+}
+    
+    
+    
+}
+