Abubeker Abdella
4180
Dependencies: mbed Servo mbed-rtos X_NUCLEO_53L0A1
Revision 0:bdd172e29b8b, committed 2018-12-10
- Comitter:
- monwonga3
- Date:
- Mon Dec 10 23:38:39 2018 +0000
- Child:
- 1:c162c077430f
- Commit message:
- mech hand demo code so far
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Dec 10 23:38:39 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
--- /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;
+
+
+}
+
+
+
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Mon Dec 10 23:38:39 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 10 23:38:39 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file