4180
Dependencies: mbed Servo mbed-rtos X_NUCLEO_53L0A1
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; + + +} + + + +} +