4180
Dependencies: mbed Servo mbed-rtos X_NUCLEO_53L0A1
main.cpp
- Committer:
- monwonga3
- Date:
- 2018-12-10
- Revision:
- 0:bdd172e29b8b
- Child:
- 1:c162c077430f
File content as of revision 0:bdd172e29b8b:
#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; } }