MBED_DEMOS / Mbed 2 deprecated df-2013-minihack-slingshot

DreamForce 2013 Mini-Hack Challenge Project

Dependencies:   ADXL345 USBDevice filter mbed

Fork of df-minihack-slingshot by Doug Anson

main.cpp@9:3af90289b117, 2013-11-19 (annotated)

Committer:
ansond
Date:
Tue Nov 19 01:21:46 2013 +0000
Revision:
9:3af90289b117
Parent:
8:1a26c7f5b2d5 
updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ansond 0:a2c33a8eded1 1 /* mbed USB Slingshot,
ansond 0:a2c33a8eded1 2 *
ansond 0:a2c33a8eded1 3 * Copyright (c) 2010-2011 mbed.org, MIT License
ansond 0:a2c33a8eded1 4 *
ansond 4:c82d5978d626 5 * smokrani, sford, danson, sgrove
ansond 0:a2c33a8eded1 6 *
ansond 0:a2c33a8eded1 7 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
ansond 0:a2c33a8eded1 8 * and associated documentation files (the "Software"), to deal in the Software without
ansond 0:a2c33a8eded1 9 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
ansond 0:a2c33a8eded1 10 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
ansond 0:a2c33a8eded1 11 * Software is furnished to do so, subject to the following conditions:
ansond 0:a2c33a8eded1 12 *
ansond 0:a2c33a8eded1 13 * The above copyright notice and this permission notice shall be included in all copies or
ansond 0:a2c33a8eded1 14 * substantial portions of the Software.
ansond 0:a2c33a8eded1 15 *
ansond 0:a2c33a8eded1 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
ansond 0:a2c33a8eded1 17 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
ansond 0:a2c33a8eded1 18 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
ansond 0:a2c33a8eded1 19 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ansond 0:a2c33a8eded1 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
ansond 0:a2c33a8eded1 21 */
ansond 0:a2c33a8eded1 22
ansond 1:d9d593d4ea39 23 //
ansond 1:d9d593d4ea39 24 // DreamForce 2013 Challenge:
ansond 1:d9d593d4ea39 25 //
ansond 2:e1c07ecec050 26 // Goal: modify the code below to adjust the sling body angle (theta) to take into account
ansond 1:d9d593d4ea39 27 // the relative angle between the sling body and the sling band.
ansond 1:d9d593d4ea39 28 //
ansond 1:d9d593d4ea39 29 //
ansond 1:d9d593d4ea39 30 // Mini-hack challenge: Your mission, should you choose to accept it, is to complete the function
ansond 8:1a26c7f5b2d5 31 // "potentiometer_value_to_degrees()" below (around line 113) to return a reasonable
ansond 1:d9d593d4ea39 32 // estimate of the sling band angle relative to the sling body
ansond 1:d9d593d4ea39 33 //
ansond 1:d9d593d4ea39 34 //
ansond 1:d9d593d4ea39 35
ansond 3:08207d3fcf3e 36 // Sling Tunables Start - !!! be careful changing these !!!
ansond 1:d9d593d4ea39 37
ansond 1:d9d593d4ea39 38 // stretch start threshold
ansond 1:d9d593d4ea39 39 float stretch_start_threshold = 0.4;
ansond 1:d9d593d4ea39 40
ansond 1:d9d593d4ea39 41 // fire threshold
ansond 1:d9d593d4ea39 42 float fire_threshold = 0.15;
ansond 1:d9d593d4ea39 43
ansond 1:d9d593d4ea39 44 // fire timing threshold
ansond 1:d9d593d4ea39 45 float fire_timing_threshold = 3.0;
ansond 1:d9d593d4ea39 46
ansond 7:991749350315 47 // scaling for mouse movement - may need to be maniuplated depending on screen resolution to get a full deflection
ansond 7:991749350315 48 int mouse_scale = 250;
ansond 1:d9d593d4ea39 49
ansond 1:d9d593d4ea39 50 // Sling Tunables End
ansond 1:d9d593d4ea39 51
ansond 1:d9d593d4ea39 52 // definition of PI
ansond 1:d9d593d4ea39 53 #define M_PI 3.14159
ansond 1:d9d593d4ea39 54
ansond 1:d9d593d4ea39 55 // Includes
ansond 0:a2c33a8eded1 56 #include "mbed.h"
ansond 0:a2c33a8eded1 57 #include "USBMouse.h"
ansond 0:a2c33a8eded1 58 #include "ADXL345.h"
ansond 0:a2c33a8eded1 59
ansond 0:a2c33a8eded1 60 // Physical interfaces
ansond 0:a2c33a8eded1 61 USBMouse mouse;
ansond 0:a2c33a8eded1 62 ADXL345 accelerometer(p5, p6, p7, p8);
ansond 9:3af90289b117 63 AnalogIn stretch_sensor(p16);
ansond 0:a2c33a8eded1 64 BusOut leds(LED1, LED2, LED3, LED4);
ansond 0:a2c33a8eded1 65
ansond 3:08207d3fcf3e 66 // Potentiometer
ansond 1:d9d593d4ea39 67 AnalogIn pot_1(p19);
ansond 1:d9d593d4ea39 68
ansond 1:d9d593d4ea39 69 // keep track of mouse position
ansond 1:d9d593d4ea39 70 int current_x = 0;
ansond 1:d9d593d4ea39 71 int current_y = 0;
ansond 1:d9d593d4ea39 72
ansond 1:d9d593d4ea39 73 // Potentiometer filters
ansond 1:d9d593d4ea39 74 #include "filter.h"
ansond 1:d9d593d4ea39 75 medianFilter prefilter(13);
ansond 1:d9d593d4ea39 76 medianFilter postfilter(7);
ansond 1:d9d593d4ea39 77
ansond 1:d9d593d4ea39 78 // return radians for a given degree
ansond 1:d9d593d4ea39 79 float degrees_to_radians(float degrees) {
ansond 1:d9d593d4ea39 80 float radians = ((M_PI*degrees)/180.0);
ansond 1:d9d593d4ea39 81 return radians;
ansond 1:d9d593d4ea39 82 }
ansond 1:d9d593d4ea39 83
ansond 1:d9d593d4ea39 84 // return degrees for a given radian
ansond 1:d9d593d4ea39 85 float radians_to_degrees(float radians) {
ansond 1:d9d593d4ea39 86 float degrees = ((180*radians)/M_PI);
ansond 1:d9d593d4ea39 87 return degrees;
ansond 1:d9d593d4ea39 88 }
ansond 1:d9d593d4ea39 89
ansond 1:d9d593d4ea39 90 // get_potentiometer_value() reads the potentiometer, filters its value and remaps it to [0, 100.0]
ansond 1:d9d593d4ea39 91 float get_potentiometer_value(AnalogIn pot) {
ansond 1:d9d593d4ea39 92 float f = pot;
ansond 3:08207d3fcf3e 93 f = prefilter.process(f); // pre-filter
ansond 3:08207d3fcf3e 94 f = (f * 100); // remap: [ 0, 100]
ansond 3:08207d3fcf3e 95 return postfilter.process(f); // post-filter after remap
ansond 1:d9d593d4ea39 96 }
ansond 1:d9d593d4ea39 97
ansond 0:a2c33a8eded1 98 //
ansond 0:a2c33a8eded1 99 // DreamForce 2013 Challenge:
ansond 1:d9d593d4ea39 100 // potentiometer_value_to_degrees() takes the potentiometer value (val_pot) and
ansond 1:d9d593d4ea39 101 // maps it to an angle between [min_sling_angle, max_sling_angle] as defined in the tunables section
ansond 1:d9d593d4ea39 102 //
ansond 1:d9d593d4ea39 103 // NOTE: This function is INCOMPLETE. To complete it you should:
ansond 1:d9d593d4ea39 104 // 1). Uncomment the debug statement, run the program, and look at raw potentiometer values
ansond 1:d9d593d4ea39 105 // 2). Determine the min and max potentiometer values you wish to scale to degrees
ansond 1:d9d593d4ea39 106 // 3). Determine the 90 degree potentiometer value ("median_pot") that denotes the sling band at 90 to the sling body
ansond 7:991749350315 107 // 4). Guestimate the range of angles you wish to map the potentiometer values (i.e. -x degrees to +y degrees)
ansond 7:991749350315 108 // 5). Fill in min_deg and max_deg below with those angle ranges
ansond 7:991749350315 109 // 6). Compile up and give it a try
ansond 7:991749350315 110 // 7). Additional hint: you may need to modify mouse_scale above to get a full deflection when you stretch the string back..
ansond 1:d9d593d4ea39 111 //
ansond 1:d9d593d4ea39 112 float potentiometer_value_to_degrees(float val_pot) {
ansond 1:d9d593d4ea39 113 float deg = 0.0;
ansond 1:d9d593d4ea39 114 float accuracy = 0.1;
ansond 1:d9d593d4ea39 115
ansond 1:d9d593d4ea39 116 // DEBUG - may need this to calibrate pot values below
ansond 1:d9d593d4ea39 117 //std::printf("Raw pot value=%.1f\r\n",val_pot);
ansond 1:d9d593d4ea39 118
ansond 9:3af90289b117 119 // Potentiometer range: typically about [36.8, 80.6] with 56.0 being "median_pot"
ansond 3:08207d3fcf3e 120 float median_pot = 0.0;
ansond 3:08207d3fcf3e 121 float min_pot = 0.0;
ansond 3:08207d3fcf3e 122 float max_pot = 0.0;
ansond 1:d9d593d4ea39 123 float incr_pot = (max_pot*10) - (min_pot*10); // how many .1 increments we have in the interval [min, max]
ansond 1:d9d593d4ea39 124
ansond 7:991749350315 125 // Mapped degree range: INCOMPLETE: you need to guesstimate the approx angle range i.e. [-x, +y] degrees so convert to
ansond 7:991749350315 126 float min_deg = 0.0;
ansond 7:991749350315 127 float max_deg = 0.0;
ansond 1:d9d593d4ea39 128 float incr_deg = (max_deg*10) - (min_deg*10); // how many .1 increments we have in the interval [min, max]
ansond 0:a2c33a8eded1 129
ansond 1:d9d593d4ea39 130 // see if we are centered or not
ansond 1:d9d593d4ea39 131 float centered_pot = fabs(val_pot - median_pot);
ansond 1:d9d593d4ea39 132
ansond 1:d9d593d4ea39 133 // if we are off 90 degrees (i.e. sling body and sling band are not at 90 degrees) - calculate the relative angle
ansond 1:d9d593d4ea39 134 if (centered_pot > accuracy) {
ansond 1:d9d593d4ea39 135 // map to degree range
ansond 1:d9d593d4ea39 136 float conversion = (incr_deg/incr_pot);
ansond 1:d9d593d4ea39 137 deg = min_deg + (conversion*(val_pot - min_pot));
ansond 1:d9d593d4ea39 138 }
ansond 6:446635b12af2 139
ansond 1:d9d593d4ea39 140 // return the calculated degrees
ansond 1:d9d593d4ea39 141 return deg;
ansond 1:d9d593d4ea39 142 }
ansond 1:d9d593d4ea39 143
ansond 1:d9d593d4ea39 144 // adjust the final angle (theta) taking into account the relative angle between the sling body and the sling band.
ansond 1:d9d593d4ea39 145 float adjust_for_sling_angle(float slingshot_body_angle) {
ansond 3:08207d3fcf3e 146 // get the sling angle through approximation with the potentiometer
ansond 3:08207d3fcf3e 147 float sling_angle_degrees = potentiometer_value_to_degrees(get_potentiometer_value(pot_1));
ansond 1:d9d593d4ea39 148
ansond 1:d9d593d4ea39 149 // the sling angle is in degrees - so lets convert the body angle to degrees as well
ansond 1:d9d593d4ea39 150 float modified_angle_degrees = radians_to_degrees(slingshot_body_angle);
ansond 1:d9d593d4ea39 151
ansond 1:d9d593d4ea39 152 // we simply add the sling angle to adjust it
ansond 1:d9d593d4ea39 153 modified_angle_degrees += sling_angle_degrees;
ansond 1:d9d593d4ea39 154
ansond 1:d9d593d4ea39 155 // make sure that we are always between 0 and 359 degrees
ansond 1:d9d593d4ea39 156 while (modified_angle_degrees > 360.0) modified_angle_degrees = modified_angle_degrees - 360;
ansond 1:d9d593d4ea39 157 while (modified_angle_degrees < 0.0) modified_angle_degrees = modified_angle_degrees + 360;
ansond 1:d9d593d4ea39 158
ansond 1:d9d593d4ea39 159 // convert the modified angle back to radians
ansond 1:d9d593d4ea39 160 float modified_angle_radians = degrees_to_radians(modified_angle_degrees);
ansond 1:d9d593d4ea39 161
ansond 1:d9d593d4ea39 162 // DEBUG
ansond 1:d9d593d4ea39 163 //std::printf("adjust_for_sling_angle: body_angle=%.1f sling_angle=%.1f modified_angle=%.1f\r\n",radians_to_degrees(slingshot_body_angle),sling_angle_degrees,modified_angle_degrees);
ansond 1:d9d593d4ea39 164
ansond 1:d9d593d4ea39 165 // return the modified angle
ansond 1:d9d593d4ea39 166 return modified_angle_radians;
ansond 0:a2c33a8eded1 167 }
ansond 0:a2c33a8eded1 168
ansond 0:a2c33a8eded1 169 // Return slingshot angle in radians, up > 0 > down
ansond 0:a2c33a8eded1 170 float get_angle() {
ansond 0:a2c33a8eded1 171 int readings[3];
ansond 0:a2c33a8eded1 172 accelerometer.getOutput(readings);
ansond 0:a2c33a8eded1 173 float x = (int16_t)readings[0];
ansond 0:a2c33a8eded1 174 float z = (int16_t)readings[2];
ansond 0:a2c33a8eded1 175 return atan(z / x);
ansond 0:a2c33a8eded1 176 }
ansond 0:a2c33a8eded1 177
ansond 0:a2c33a8eded1 178 // Return normalised stretch value based on bounds of all readings seen
ansond 0:a2c33a8eded1 179 float get_stretch() {
ansond 0:a2c33a8eded1 180 static float min_strength = 0.7;
ansond 0:a2c33a8eded1 181 static float max_strength = 0.7;
ansond 0:a2c33a8eded1 182 float current_strength = stretch_sensor.read();
ansond 0:a2c33a8eded1 183 if(current_strength > max_strength) { max_strength = current_strength; }
ansond 0:a2c33a8eded1 184 if(current_strength < min_strength) { min_strength = current_strength; }
ansond 0:a2c33a8eded1 185 float stretch = (current_strength - min_strength) / (max_strength - min_strength);
ansond 0:a2c33a8eded1 186 return 1.0 - stretch;
ansond 0:a2c33a8eded1 187 }
ansond 0:a2c33a8eded1 188
ansond 0:a2c33a8eded1 189 // move mouse to a location relative to the start point, stepping as needed
ansond 0:a2c33a8eded1 190 void move_mouse(int x, int y) {
ansond 0:a2c33a8eded1 191 const int STEP = 10;
ansond 0:a2c33a8eded1 192
ansond 0:a2c33a8eded1 193 int move_x = x - current_x;
ansond 0:a2c33a8eded1 194 int move_y = y - current_y;
ansond 0:a2c33a8eded1 195
ansond 0:a2c33a8eded1 196 // Move the mouse, in steps of max step size to ensure it is picked up by OS
ansond 0:a2c33a8eded1 197 while(move_x > STEP) { mouse.move(STEP, 0); move_x -= STEP; }
ansond 0:a2c33a8eded1 198 while(move_x < -STEP) { mouse.move(-STEP, 0); move_x += STEP; }
ansond 0:a2c33a8eded1 199 while(move_y > STEP) { mouse.move(0, STEP); move_y -= STEP; }
ansond 0:a2c33a8eded1 200 while(move_y < -STEP) { mouse.move(0, -STEP); move_y += STEP; }
ansond 1:d9d593d4ea39 201 mouse.move(move_x, move_y);
ansond 0:a2c33a8eded1 202
ansond 0:a2c33a8eded1 203 current_x = x;
ansond 0:a2c33a8eded1 204 current_y = y;
ansond 0:a2c33a8eded1 205 }
ansond 1:d9d593d4ea39 206
ansond 1:d9d593d4ea39 207 // reset the mouse position
ansond 1:d9d593d4ea39 208 void reset_mouse() {
ansond 1:d9d593d4ea39 209 current_x = 0;
ansond 1:d9d593d4ea39 210 current_y = 0;
ansond 1:d9d593d4ea39 211 mouse.move(0,0);
ansond 1:d9d593d4ea39 212 }
ansond 0:a2c33a8eded1 213
ansond 0:a2c33a8eded1 214 template <class T>
ansond 0:a2c33a8eded1 215 T filter(T* array, int len, T value) {
ansond 0:a2c33a8eded1 216 T mean = 0.0;
ansond 0:a2c33a8eded1 217 for(int i = 0; i<len - 1; i++) {
ansond 0:a2c33a8eded1 218 mean += array[i + 1];
ansond 0:a2c33a8eded1 219 array[i] = array[i + 1];
ansond 0:a2c33a8eded1 220 }
ansond 0:a2c33a8eded1 221 mean += value;
ansond 0:a2c33a8eded1 222 array[len - 1] = value;
ansond 0:a2c33a8eded1 223 return mean / (T)len;
ansond 0:a2c33a8eded1 224 }
ansond 0:a2c33a8eded1 225
ansond 0:a2c33a8eded1 226 typedef enum {
ansond 0:a2c33a8eded1 227 WAITING = 2,
ansond 0:a2c33a8eded1 228 AIMING = 4,
ansond 0:a2c33a8eded1 229 FIRING = 8
ansond 0:a2c33a8eded1 230 } state_t;
ansond 0:a2c33a8eded1 231
ansond 0:a2c33a8eded1 232 int main() {
ansond 1:d9d593d4ea39 233 bool loop_forever = true;
ansond 0:a2c33a8eded1 234 leds = 1;
ansond 1:d9d593d4ea39 235
ansond 1:d9d593d4ea39 236 // init mouse tracking
ansond 1:d9d593d4ea39 237 reset_mouse();
ansond 0:a2c33a8eded1 238
ansond 0:a2c33a8eded1 239 // setup accelerometer
ansond 0:a2c33a8eded1 240 accelerometer.setPowerControl(0x00);
ansond 0:a2c33a8eded1 241 accelerometer.setDataFormatControl(0x0B);
ansond 0:a2c33a8eded1 242 accelerometer.setDataRate(ADXL345_3200HZ);
ansond 0:a2c33a8eded1 243 accelerometer.setPowerControl(0x08);
ansond 0:a2c33a8eded1 244
ansond 0:a2c33a8eded1 245 state_t state = WAITING;
ansond 0:a2c33a8eded1 246 Timer timer;
ansond 0:a2c33a8eded1 247
ansond 0:a2c33a8eded1 248 float angles[8] = {0};
ansond 0:a2c33a8eded1 249 float stretches[8] = {0};
ansond 0:a2c33a8eded1 250
ansond 1:d9d593d4ea39 251 while(loop_forever) {
ansond 0:a2c33a8eded1 252 // get the slingshot parameters
ansond 0:a2c33a8eded1 253 float this_stretch = get_stretch();
ansond 0:a2c33a8eded1 254 float this_angle = get_angle();
ansond 0:a2c33a8eded1 255
ansond 0:a2c33a8eded1 256 // apply some filtering
ansond 0:a2c33a8eded1 257 float stretch = filter(stretches, 8, this_stretch);
ansond 0:a2c33a8eded1 258 float angle = filter(angles, 8, this_angle);
ansond 0:a2c33a8eded1 259
ansond 0:a2c33a8eded1 260 // DreamForce 2013 Challenge: Adjust the angle to account for the relative angle between the sling and the slingshot body
ansond 0:a2c33a8eded1 261 angle = adjust_for_sling_angle(angle);
ansond 9:3af90289b117 262
ansond 9:3af90289b117 263 // DEBUG
ansond 9:3af90289b117 264 //std::printf("stretch=%.1f angle=%.1f\r\n",stretch,angle);
ansond 0:a2c33a8eded1 265
ansond 0:a2c33a8eded1 266 leds = state;
ansond 1:d9d593d4ea39 267
ansond 0:a2c33a8eded1 268 // act based on the current state
ansond 0:a2c33a8eded1 269 switch (state) {
ansond 0:a2c33a8eded1 270 case WAITING:
ansond 1:d9d593d4ea39 271 if(stretch > stretch_start_threshold) { // significant stretch, considered starting
ansond 0:a2c33a8eded1 272 mouse.press(MOUSE_LEFT);
ansond 0:a2c33a8eded1 273 state = AIMING;
ansond 0:a2c33a8eded1 274 }
ansond 0:a2c33a8eded1 275 break;
ansond 0:a2c33a8eded1 276
ansond 0:a2c33a8eded1 277 case AIMING:
ansond 1:d9d593d4ea39 278 if(stretch - this_stretch > fire_threshold) { // rapid de-stretch, considered a fire
ansond 0:a2c33a8eded1 279 mouse.release(MOUSE_LEFT);
ansond 1:d9d593d4ea39 280 reset_mouse();
ansond 0:a2c33a8eded1 281 timer.start();
ansond 0:a2c33a8eded1 282 state = FIRING;
ansond 1:d9d593d4ea39 283 }
ansond 1:d9d593d4ea39 284 else if(stretch < stretch_start_threshold) { // de-stretch
ansond 1:d9d593d4ea39 285 reset_mouse();
ansond 1:d9d593d4ea39 286 timer.stop();
ansond 1:d9d593d4ea39 287 timer.reset();
ansond 1:d9d593d4ea39 288 state = WAITING;
ansond 0:a2c33a8eded1 289 } else {
ansond 1:d9d593d4ea39 290 int x = 0.0 - cos(angle) * stretch * mouse_scale;
ansond 1:d9d593d4ea39 291 int y = sin(angle) * stretch * mouse_scale;
ansond 0:a2c33a8eded1 292 move_mouse(x, y);
ansond 0:a2c33a8eded1 293 }
ansond 0:a2c33a8eded1 294 break;
ansond 0:a2c33a8eded1 295
ansond 0:a2c33a8eded1 296 case FIRING:
ansond 1:d9d593d4ea39 297 if(timer > fire_timing_threshold) {
ansond 0:a2c33a8eded1 298 timer.stop();
ansond 0:a2c33a8eded1 299 timer.reset();
ansond 1:d9d593d4ea39 300 reset_mouse();
ansond 0:a2c33a8eded1 301 state = WAITING;
ansond 0:a2c33a8eded1 302 }
ansond 0:a2c33a8eded1 303 break;
ansond 0:a2c33a8eded1 304 };
ansond 0:a2c33a8eded1 305
ansond 1:d9d593d4ea39 306 // wait for 100ms
ansond 1:d9d593d4ea39 307 wait_ms(100);
ansond 0:a2c33a8eded1 308 }
ansond 9:3af90289b117 309 }