4180
Dependencies: mbed Servo mbed-rtos X_NUCLEO_53L0A1
main.cpp
- Committer:
- AbuAbdella
- Date:
- 2018-12-12
- Revision:
- 1:c162c077430f
- Parent:
- 0:bdd172e29b8b
File content as of revision 1:c162c077430f:
#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 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 int sensor_addr = 41 << 1; float clear_value; float red_value; float blue_value; float green_value; 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; void blinky() { led4 = !led4; //wait(0.2); } int main() { 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(){ // shoulder=0.37f; wrist = 0.1f; elbow = 0.10f; // elbow = 0.00f; //while(doRotate){ 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(); threadRotate.terminate(); //} } void stroke(){ shoulder = shoulderPosition-.10; Thread::wait(2000); 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.50f; 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("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); pc.printf("new Red Value %.2f old Red value %.2f shoulder %.2f \n",newRedValue,oldRedValue,shoulderPosition); // pc.printf("%.2f\n",newRedValue); Thread::wait(300); oldRedValue = newRedValue; } }