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?
User | Revision | Line number | New 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 |