Abubeker Abdella / Mbed 2 deprecated mech_hand_demo

4180

Dependencies:   mbed Servo mbed-rtos X_NUCLEO_53L0A1

main.cpp@0:bdd172e29b8b, 2018-12-10 (annotated)

Committer:
monwonga3
Date:
Mon Dec 10 23:38:39 2018 +0000
Revision:
0:bdd172e29b8b
Child:
1:c162c077430f
mech hand demo code so far

Who changed what in which revision?

UserRevisionLine numberNew contents of line
monwonga3 0:bdd172e29b8b 1 #include "mbed.h"
monwonga3 0:bdd172e29b8b 2 #include "Servo.h"
monwonga3 0:bdd172e29b8b 3 #include "rtos.h"
monwonga3 0:bdd172e29b8b 4
monwonga3 0:bdd172e29b8b 5 Servo shoulder(p26); // pwm
monwonga3 0:bdd172e29b8b 6 Servo elbow(p25); // pwm
monwonga3 0:bdd172e29b8b 7 Servo wrist(p24); // pwm
monwonga3 0:bdd172e29b8b 8 Servo hand(p23); // pwm
monwonga3 0:bdd172e29b8b 9
monwonga3 0:bdd172e29b8b 10 I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
monwonga3 0:bdd172e29b8b 11 Serial pc(USBTX, USBRX); //Used to view the colors that are read in
monwonga3 0:bdd172e29b8b 12
monwonga3 0:bdd172e29b8b 13 int sensor_addr = 41 << 1;
monwonga3 0:bdd172e29b8b 14
monwonga3 0:bdd172e29b8b 15 float clear_value;
monwonga3 0:bdd172e29b8b 16 float red_value;
monwonga3 0:bdd172e29b8b 17 float blue_value;
monwonga3 0:bdd172e29b8b 18 float green_value;
monwonga3 0:bdd172e29b8b 19
monwonga3 0:bdd172e29b8b 20 float newRedValue;
monwonga3 0:bdd172e29b8b 21 float oldRedValue;
monwonga3 0:bdd172e29b8b 22
monwonga3 0:bdd172e29b8b 23
monwonga3 0:bdd172e29b8b 24
monwonga3 0:bdd172e29b8b 25 DigitalOut green(LED1);
monwonga3 0:bdd172e29b8b 26 DigitalOut led4(LED4);
monwonga3 0:bdd172e29b8b 27 DigitalOut led(p11);
monwonga3 0:bdd172e29b8b 28
monwonga3 0:bdd172e29b8b 29 void stroke();
monwonga3 0:bdd172e29b8b 30 void dunk();
monwonga3 0:bdd172e29b8b 31 void rotate();
monwonga3 0:bdd172e29b8b 32 void readLight();
monwonga3 0:bdd172e29b8b 33
monwonga3 0:bdd172e29b8b 34 float shoulderPosition;
monwonga3 0:bdd172e29b8b 35 //
monwonga3 0:bdd172e29b8b 36 Thread threadLight;
monwonga3 0:bdd172e29b8b 37 Thread threadRotate;
monwonga3 0:bdd172e29b8b 38
monwonga3 0:bdd172e29b8b 39 void blinky() {
monwonga3 0:bdd172e29b8b 40 led4 = !led4;
monwonga3 0:bdd172e29b8b 41 //wait(0.2);
monwonga3 0:bdd172e29b8b 42 }
monwonga3 0:bdd172e29b8b 43
monwonga3 0:bdd172e29b8b 44 int main()
monwonga3 0:bdd172e29b8b 45 {
monwonga3 0:bdd172e29b8b 46
monwonga3 0:bdd172e29b8b 47 threadLight.start(readLight); // the address of the function to be attached (flip) and the interval (2 seconds)
monwonga3 0:bdd172e29b8b 48 threadRotate.start(rotate);// to stop thread thread_name.terminate();
monwonga3 0:bdd172e29b8b 49
monwonga3 0:bdd172e29b8b 50
monwonga3 0:bdd172e29b8b 51
monwonga3 0:bdd172e29b8b 52
monwonga3 0:bdd172e29b8b 53
monwonga3 0:bdd172e29b8b 54
monwonga3 0:bdd172e29b8b 55
monwonga3 0:bdd172e29b8b 56 }
monwonga3 0:bdd172e29b8b 57
monwonga3 0:bdd172e29b8b 58 void rotate(){
monwonga3 0:bdd172e29b8b 59 wrist = 0.0f;
monwonga3 0:bdd172e29b8b 60 elbow = 0.30f;
monwonga3 0:bdd172e29b8b 61
monwonga3 0:bdd172e29b8b 62 //while(doRotate){
monwonga3 0:bdd172e29b8b 63
monwonga3 0:bdd172e29b8b 64 float count = 1.0f;
monwonga3 0:bdd172e29b8b 65
monwonga3 0:bdd172e29b8b 66 while (count>=0.0f) {
monwonga3 0:bdd172e29b8b 67
monwonga3 0:bdd172e29b8b 68
monwonga3 0:bdd172e29b8b 69 float sCount = count/2;
monwonga3 0:bdd172e29b8b 70
monwonga3 0:bdd172e29b8b 71 if(newRedValue <= oldRedValue - .04 ){
monwonga3 0:bdd172e29b8b 72 shoulderPosition = 0.5f + sCount;
monwonga3 0:bdd172e29b8b 73
monwonga3 0:bdd172e29b8b 74 }
monwonga3 0:bdd172e29b8b 75 shoulder = 0.0f + sCount;;
monwonga3 0:bdd172e29b8b 76 count-= 0.01f;
monwonga3 0:bdd172e29b8b 77
monwonga3 0:bdd172e29b8b 78 // wait(.05);
monwonga3 0:bdd172e29b8b 79 Thread::wait(50);
monwonga3 0:bdd172e29b8b 80
monwonga3 0:bdd172e29b8b 81 }
monwonga3 0:bdd172e29b8b 82
monwonga3 0:bdd172e29b8b 83
monwonga3 0:bdd172e29b8b 84 while (count<=1.0f) {
monwonga3 0:bdd172e29b8b 85
monwonga3 0:bdd172e29b8b 86 float sCount = count/2;
monwonga3 0:bdd172e29b8b 87
monwonga3 0:bdd172e29b8b 88
monwonga3 0:bdd172e29b8b 89 shoulder = 0.0f + sCount;
monwonga3 0:bdd172e29b8b 90 count+= 0.01f;
monwonga3 0:bdd172e29b8b 91 Thread::wait(50);
monwonga3 0:bdd172e29b8b 92
monwonga3 0:bdd172e29b8b 93 //wait(.05);
monwonga3 0:bdd172e29b8b 94
monwonga3 0:bdd172e29b8b 95 }
monwonga3 0:bdd172e29b8b 96
monwonga3 0:bdd172e29b8b 97
monwonga3 0:bdd172e29b8b 98
monwonga3 0:bdd172e29b8b 99
monwonga3 0:bdd172e29b8b 100 threadLight.terminate();
monwonga3 0:bdd172e29b8b 101
monwonga3 0:bdd172e29b8b 102 dunk();
monwonga3 0:bdd172e29b8b 103
monwonga3 0:bdd172e29b8b 104 threadRotate.terminate();
monwonga3 0:bdd172e29b8b 105
monwonga3 0:bdd172e29b8b 106
monwonga3 0:bdd172e29b8b 107 //}
monwonga3 0:bdd172e29b8b 108
monwonga3 0:bdd172e29b8b 109
monwonga3 0:bdd172e29b8b 110 }
monwonga3 0:bdd172e29b8b 111
monwonga3 0:bdd172e29b8b 112 void stroke(){
monwonga3 0:bdd172e29b8b 113
monwonga3 0:bdd172e29b8b 114 shoulder = shoulderPosition;
monwonga3 0:bdd172e29b8b 115
monwonga3 0:bdd172e29b8b 116 for (float i = 0.0f; i < 0.5f; i+=0.01f) {
monwonga3 0:bdd172e29b8b 117 float wCount = i/1.5;
monwonga3 0:bdd172e29b8b 118 float eCount = i/2;
monwonga3 0:bdd172e29b8b 119 //hand = 1.5f ;
monwonga3 0:bdd172e29b8b 120 wrist = 0.0f + wCount;
monwonga3 0:bdd172e29b8b 121 elbow = 0.19f + eCount;//wCount; //0.47f;
monwonga3 0:bdd172e29b8b 122 wait(.05);
monwonga3 0:bdd172e29b8b 123 //printf("wCount is %f \n", wCount);
monwonga3 0:bdd172e29b8b 124 // printf("eCount is %f \n", eCount);
monwonga3 0:bdd172e29b8b 125
monwonga3 0:bdd172e29b8b 126
monwonga3 0:bdd172e29b8b 127 }
monwonga3 0:bdd172e29b8b 128 for (float i = 0.0f; i < 0.5f; i+=0.01f) {
monwonga3 0:bdd172e29b8b 129 float wCount = i/1.5;
monwonga3 0:bdd172e29b8b 130 float eCount = i/2;
monwonga3 0:bdd172e29b8b 131 //hand = 1.5f ;
monwonga3 0:bdd172e29b8b 132 wrist = 0.33f - wCount;
monwonga3 0:bdd172e29b8b 133 elbow = 0.44f - eCount;//wCount; //0.47f;
monwonga3 0:bdd172e29b8b 134 //printf("wCount is %f \n", wCount);
monwonga3 0:bdd172e29b8b 135 // printf("eCount is %f \n", eCount);
monwonga3 0:bdd172e29b8b 136 wait(.05);
monwonga3 0:bdd172e29b8b 137
monwonga3 0:bdd172e29b8b 138 }
monwonga3 0:bdd172e29b8b 139 }
monwonga3 0:bdd172e29b8b 140
monwonga3 0:bdd172e29b8b 141 void dunk(){
monwonga3 0:bdd172e29b8b 142
monwonga3 0:bdd172e29b8b 143 shoulder = 0.3f;
monwonga3 0:bdd172e29b8b 144
monwonga3 0:bdd172e29b8b 145 float wristIN;
monwonga3 0:bdd172e29b8b 146 float elbowIN;
monwonga3 0:bdd172e29b8b 147
monwonga3 0:bdd172e29b8b 148 for (float i = 0.0f; i < 0.5f; i+=0.01f) {
monwonga3 0:bdd172e29b8b 149 float wCount = i/1.5;
monwonga3 0:bdd172e29b8b 150 float eCount = i/2;
monwonga3 0:bdd172e29b8b 151 //hand = 1.5f ;
monwonga3 0:bdd172e29b8b 152 wristIN = 0.2f - wCount;//equals.33f at end
monwonga3 0:bdd172e29b8b 153 //elbowIN = 0.5f + eCount;//equals .44f at the end
monwonga3 0:bdd172e29b8b 154 //wait(.5);
monwonga3 0:bdd172e29b8b 155 wrist = wristIN;
monwonga3 0:bdd172e29b8b 156 elbow = 0.3f;
monwonga3 0:bdd172e29b8b 157 //
monwonga3 0:bdd172e29b8b 158 // printf("wCount is %f \n", wCount);
monwonga3 0:bdd172e29b8b 159 // printf("eCount is %f \n", eCount);
monwonga3 0:bdd172e29b8b 160 wait(.05);
monwonga3 0:bdd172e29b8b 161
monwonga3 0:bdd172e29b8b 162 }
monwonga3 0:bdd172e29b8b 163 for (float i = 0.0f; i < 0.5f; i+=0.01f) {
monwonga3 0:bdd172e29b8b 164 float wCount = i/1.5;
monwonga3 0:bdd172e29b8b 165 float eCount = i/2;
monwonga3 0:bdd172e29b8b 166 //hand = 1.5f ;
monwonga3 0:bdd172e29b8b 167 //wait(.5);
monwonga3 0:bdd172e29b8b 168 wrist = wristIN + wCount;
monwonga3 0:bdd172e29b8b 169 //elbow = wristIN - eCount;//wCount; //0.47f;
monwonga3 0:bdd172e29b8b 170 // printf("wCount is %f \n", wCount);
monwonga3 0:bdd172e29b8b 171 // printf("eCount is %f \n", eCount);
monwonga3 0:bdd172e29b8b 172 wait(.05);
monwonga3 0:bdd172e29b8b 173
monwonga3 0:bdd172e29b8b 174 }
monwonga3 0:bdd172e29b8b 175
monwonga3 0:bdd172e29b8b 176 stroke();
monwonga3 0:bdd172e29b8b 177
monwonga3 0:bdd172e29b8b 178 }
monwonga3 0:bdd172e29b8b 179
monwonga3 0:bdd172e29b8b 180
monwonga3 0:bdd172e29b8b 181 void readLight () {
monwonga3 0:bdd172e29b8b 182
monwonga3 0:bdd172e29b8b 183
monwonga3 0:bdd172e29b8b 184 pc.baud(9600);
monwonga3 0:bdd172e29b8b 185 green = 1; // off
monwonga3 0:bdd172e29b8b 186
monwonga3 0:bdd172e29b8b 187 // Connect to the Color sensor and verify
monwonga3 0:bdd172e29b8b 188
monwonga3 0:bdd172e29b8b 189 i2c.frequency(200000);
monwonga3 0:bdd172e29b8b 190
monwonga3 0:bdd172e29b8b 191 char id_regval[1] = {146};
monwonga3 0:bdd172e29b8b 192 char data[1] = {0};
monwonga3 0:bdd172e29b8b 193 i2c.write(sensor_addr,id_regval,1, true);
monwonga3 0:bdd172e29b8b 194 i2c.read(sensor_addr,data,1,false);
monwonga3 0:bdd172e29b8b 195
monwonga3 0:bdd172e29b8b 196 if (data[0]==68) {
monwonga3 0:bdd172e29b8b 197 green = 0;
monwonga3 0:bdd172e29b8b 198 wait (2);
monwonga3 0:bdd172e29b8b 199 green = 1;
monwonga3 0:bdd172e29b8b 200 } else {
monwonga3 0:bdd172e29b8b 201 green = 1;
monwonga3 0:bdd172e29b8b 202 }
monwonga3 0:bdd172e29b8b 203
monwonga3 0:bdd172e29b8b 204 // Initialize color sensor
monwonga3 0:bdd172e29b8b 205
monwonga3 0:bdd172e29b8b 206 char timing_register[2] = {129,0};
monwonga3 0:bdd172e29b8b 207 i2c.write(sensor_addr,timing_register,2,false);
monwonga3 0:bdd172e29b8b 208
monwonga3 0:bdd172e29b8b 209 char control_register[2] = {143,0};
monwonga3 0:bdd172e29b8b 210 i2c.write(sensor_addr,control_register,2,false);
monwonga3 0:bdd172e29b8b 211
monwonga3 0:bdd172e29b8b 212 char enable_register[2] = {128,3};
monwonga3 0:bdd172e29b8b 213 i2c.write(sensor_addr,enable_register,2,false);
monwonga3 0:bdd172e29b8b 214
monwonga3 0:bdd172e29b8b 215 // Read data from color sensor (Clear/Red/Green/Blue)
monwonga3 0:bdd172e29b8b 216 led = 1;
monwonga3 0:bdd172e29b8b 217 while (true) {
monwonga3 0:bdd172e29b8b 218 char clear_reg[1] = {148};
monwonga3 0:bdd172e29b8b 219 char clear_data[2] = {0,0};
monwonga3 0:bdd172e29b8b 220 i2c.write(sensor_addr,clear_reg,1, true);
monwonga3 0:bdd172e29b8b 221 i2c.read(sensor_addr,clear_data,2, false);
monwonga3 0:bdd172e29b8b 222
monwonga3 0:bdd172e29b8b 223 clear_value = ((int)clear_data[1] << 8) | clear_data[0];
monwonga3 0:bdd172e29b8b 224
monwonga3 0:bdd172e29b8b 225 char red_reg[1] = {150};
monwonga3 0:bdd172e29b8b 226 char red_data[2] = {0,0};
monwonga3 0:bdd172e29b8b 227 i2c.write(sensor_addr,red_reg,1, true);
monwonga3 0:bdd172e29b8b 228 i2c.read(sensor_addr,red_data,2, false);
monwonga3 0:bdd172e29b8b 229
monwonga3 0:bdd172e29b8b 230 red_value = ((int)red_data[1] << 8) | red_data[0];
monwonga3 0:bdd172e29b8b 231
monwonga3 0:bdd172e29b8b 232 char green_reg[1] = {152};
monwonga3 0:bdd172e29b8b 233 char green_data[2] = {0,0};
monwonga3 0:bdd172e29b8b 234 i2c.write(sensor_addr,green_reg,1, true);
monwonga3 0:bdd172e29b8b 235 i2c.read(sensor_addr,green_data,2, false);
monwonga3 0:bdd172e29b8b 236
monwonga3 0:bdd172e29b8b 237 green_value = ((int)green_data[1] << 8) | green_data[0];
monwonga3 0:bdd172e29b8b 238
monwonga3 0:bdd172e29b8b 239 char blue_reg[1] = {154};
monwonga3 0:bdd172e29b8b 240 char blue_data[2] = {0,0};
monwonga3 0:bdd172e29b8b 241 i2c.write(sensor_addr,blue_reg,1, true);
monwonga3 0:bdd172e29b8b 242 i2c.read(sensor_addr,blue_data,2, false);
monwonga3 0:bdd172e29b8b 243
monwonga3 0:bdd172e29b8b 244 blue_value = ((int)blue_data[1] << 8) | blue_data[0];
monwonga3 0:bdd172e29b8b 245
monwonga3 0:bdd172e29b8b 246 // print sensor readings
monwonga3 0:bdd172e29b8b 247 red_value = red_value / clear_value;
monwonga3 0:bdd172e29b8b 248 newRedValue = red_value;
monwonga3 0:bdd172e29b8b 249 green_value = green_value/clear_value;
monwonga3 0:bdd172e29b8b 250 blue_value = blue_value/clear_value;
monwonga3 0:bdd172e29b8b 251 pc.printf("Clear (%.2f), Red (%.2f), old red (%.2f), new red (%.2f)\n, Shoulder position %.2f \n", clear_value, red_value, oldRedValue, newRedValue, shoulderPosition);
monwonga3 0:bdd172e29b8b 252 //The above code displays the red, green, and blue values read in by the color sensor.
monwonga3 0:bdd172e29b8b 253 //wait(0.5);
monwonga3 0:bdd172e29b8b 254 Thread::wait(300);
monwonga3 0:bdd172e29b8b 255
monwonga3 0:bdd172e29b8b 256 oldRedValue = newRedValue;
monwonga3 0:bdd172e29b8b 257
monwonga3 0:bdd172e29b8b 258
monwonga3 0:bdd172e29b8b 259 }
monwonga3 0:bdd172e29b8b 260
monwonga3 0:bdd172e29b8b 261
monwonga3 0:bdd172e29b8b 262
monwonga3 0:bdd172e29b8b 263 }
monwonga3 0:bdd172e29b8b 264