main.cpp

00001 /* mbed USB Slingshot, 
00002  *
00003  * Copyright (c) 2010-2011 mbed.org, MIT License
00004  * 
00005  * smokrani, sford, danson, sgrove 
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00008  * and associated documentation files (the "Software"), to deal in the Software without
00009  * restriction, including without limitation the rights to use, copy, modify, merge, publish,
00010  * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
00011  * Software is furnished to do so, subject to the following conditions:
00012  * 
00013  *  The above copyright notice and this permission notice shall be included in all copies or
00014  * substantial portions of the Software.
00015  *
00016  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00017  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00018  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00019  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00021  */
00022  
00023 //
00024 // DreamForce 2013 Challenge: 
00025 //
00026 //      Goal: modify the code below to adjust the sling body angle (theta) to take into account 
00027 //            the relative angle between the sling body and the sling band.
00028 //
00029 //
00030 //      Mini-hack challenge: Your mission, should you choose to accept it, is to complete the function
00031 //                           "potentiometer_value_to_degrees()" below (around line 113) to return a reasonable 
00032 //                           estimate of the sling band angle relative to the sling body 
00033 // 
00034 //
00035 
00036 // Sling Tunables Start - !!! be careful changing these !!!
00037 
00038 // stretch start threshold
00039 float stretch_start_threshold = 0.4;
00040 
00041 // fire threshold
00042 float fire_threshold = 0.15;
00043 
00044 // fire timing threshold
00045 float fire_timing_threshold = 3.0;
00046 
00047 // scaling for mouse movement - may need to be maniuplated depending on screen resolution to get a full deflection
00048 int mouse_scale = 250;
00049 
00050 // Sling Tunables End
00051 
00052 // definition of PI
00053 #define M_PI    3.14159
00054 
00055 // Includes
00056 #include "mbed.h"
00057 #include "USBMouse.h"
00058 #include "ADXL345.h"
00059  
00060 // Physical interfaces
00061 USBMouse mouse;
00062 ADXL345 accelerometer(p5, p6, p7, p8);
00063 AnalogIn stretch_sensor(p16);
00064 BusOut leds(LED1, LED2, LED3, LED4);
00065 
00066 // Potentiometer
00067 AnalogIn pot_1(p19);
00068 
00069 // keep track of mouse position
00070 int current_x = 0;
00071 int current_y = 0;
00072 
00073 // Potentiometer filters
00074 #include "filter.h"
00075 medianFilter prefilter(13);
00076 medianFilter postfilter(7);
00077 
00078 //  return radians for a given degree
00079 float degrees_to_radians(float degrees) {
00080     float radians = ((M_PI*degrees)/180.0); 
00081     return radians;
00082 }
00083 
00084 //  return degrees for a given radian
00085 float radians_to_degrees(float radians) {
00086     float degrees = ((180*radians)/M_PI); 
00087     return degrees;
00088 }
00089 
00090 //  get_potentiometer_value() reads the potentiometer, filters its value and remaps it to [0, 100.0]
00091 float get_potentiometer_value(AnalogIn pot) {
00092     float f = pot;
00093     f = prefilter.process(f);       // pre-filter
00094     f = (f * 100);                  // remap: [ 0, 100]
00095     return postfilter.process(f);   // post-filter after remap
00096 }
00097 
00098 //
00099 // DreamForce 2013 Challenge: 
00100 //  potentiometer_value_to_degrees() takes the potentiometer value (val_pot) and
00101 //  maps it to an angle between [min_sling_angle, max_sling_angle] as defined in the tunables section
00102 //
00103 //  NOTE: This function is INCOMPLETE. To complete it you should:
00104 //        1). Uncomment the debug statement, run the program, and look at raw potentiometer values
00105 //        2). Determine the min and max potentiometer values you wish to scale to degrees
00106 //        3). Determine the 90 degree potentiometer value ("median_pot") that denotes the sling band at 90 to the sling body
00107 //        4). Guestimate the range of angles you wish to map the potentiometer values (i.e. -x degrees to +y degrees)
00108 //        5). Fill in min_deg and max_deg below with those angle ranges
00109 //        6). Compile up and give it a try
00110 //        7). Additional hint: you may need to modify mouse_scale above to get a full deflection when you stretch the string back.. 
00111 //
00112 float potentiometer_value_to_degrees(float val_pot) {
00113     float deg = 0.0;
00114     float accuracy = 0.1;
00115     
00116     // DEBUG - may need this to calibrate pot values below
00117     //std::printf("Raw pot value=%.1f\r\n",val_pot);
00118     
00119     // Potentiometer range: typically about [36.8, 80.6] with 56.0 being "median_pot"
00120     float median_pot = 0.0;
00121     float min_pot = 0.0;
00122     float max_pot = 0.0;
00123     float incr_pot = (max_pot*10) - (min_pot*10);       // how many .1 increments we have in the interval [min, max]
00124     
00125     // Mapped degree range: INCOMPLETE: you need to guesstimate the approx angle range i.e. [-x, +y] degrees so convert to
00126     float min_deg = 0.0;
00127     float max_deg = 0.0;
00128     float incr_deg = (max_deg*10) - (min_deg*10);       // how many .1 increments we have in the interval [min, max]
00129     
00130     // see if we are centered or not
00131     float centered_pot = fabs(val_pot - median_pot);
00132   
00133     // if we are off 90 degrees (i.e. sling body and sling band are not at 90 degrees) - calculate the relative angle
00134     if (centered_pot > accuracy) {
00135         // map to degree range
00136         float conversion = (incr_deg/incr_pot);
00137         deg = min_deg + (conversion*(val_pot - min_pot));                
00138     }
00139         
00140     // return the calculated degrees
00141     return deg;
00142 }
00143 
00144 // adjust the final angle (theta) taking into account the relative angle between the sling body and the sling band.
00145 float adjust_for_sling_angle(float slingshot_body_angle) {    
00146     // get the sling angle through approximation with the potentiometer
00147     float sling_angle_degrees = potentiometer_value_to_degrees(get_potentiometer_value(pot_1));
00148     
00149     // the sling angle is in degrees - so lets convert the body angle to degrees as well
00150     float modified_angle_degrees = radians_to_degrees(slingshot_body_angle);
00151     
00152     // we simply add the sling angle to adjust it
00153     modified_angle_degrees += sling_angle_degrees;
00154     
00155     // make sure that we are always between 0 and 359 degrees
00156     while (modified_angle_degrees > 360.0) modified_angle_degrees = modified_angle_degrees - 360;
00157     while (modified_angle_degrees < 0.0) modified_angle_degrees = modified_angle_degrees + 360;
00158     
00159     // convert the modified angle back to radians
00160     float modified_angle_radians = degrees_to_radians(modified_angle_degrees);
00161     
00162     // DEBUG
00163     //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);
00164             
00165     // return the modified angle
00166     return modified_angle_radians;
00167 }
00168 
00169 // Return slingshot angle in radians, up > 0 > down
00170 float get_angle() {
00171     int readings[3];
00172     accelerometer.getOutput(readings);
00173     float x = (int16_t)readings[0];
00174     float z = (int16_t)readings[2];
00175     return atan(z / x);    
00176 }
00177  
00178 // Return normalised stretch value based on bounds of all readings seen
00179 float get_stretch() {
00180     static float min_strength = 0.7;
00181     static float max_strength = 0.7;
00182     float current_strength = stretch_sensor.read();
00183     if(current_strength > max_strength) { max_strength = current_strength; }
00184     if(current_strength < min_strength) { min_strength = current_strength; }
00185     float stretch = (current_strength - min_strength) / (max_strength - min_strength);
00186     return 1.0 - stretch;
00187 }
00188  
00189 // move mouse to a location relative to the start point, stepping as needed
00190 void move_mouse(int x, int y) {
00191     const int STEP = 10;
00192     
00193     int move_x = x - current_x;
00194     int move_y = y - current_y; 
00195  
00196     // Move the mouse, in steps of max step size to ensure it is picked up by OS
00197     while(move_x > STEP) { mouse.move(STEP, 0); move_x -= STEP; }
00198     while(move_x < -STEP) { mouse.move(-STEP, 0); move_x += STEP; }
00199     while(move_y > STEP) { mouse.move(0, STEP); move_y -= STEP; }
00200     while(move_y < -STEP) { mouse.move(0, -STEP); move_y += STEP; }
00201     mouse.move(move_x, move_y);   
00202     
00203     current_x = x;
00204     current_y = y;
00205 }
00206 
00207 // reset the mouse position
00208 void reset_mouse() {
00209     current_x = 0;
00210     current_y = 0;
00211     mouse.move(0,0);
00212 }
00213  
00214 template <class T>
00215 T filter(T* array, int len, T value) {
00216     T mean = 0.0;
00217     for(int i = 0; i<len - 1; i++) {
00218         mean += array[i + 1];
00219         array[i] = array[i + 1];
00220     }
00221     mean += value;
00222     array[len - 1] = value;
00223     return mean / (T)len;
00224 }
00225  
00226 typedef enum {
00227     WAITING = 2,
00228     AIMING = 4,
00229     FIRING = 8
00230 } state_t;
00231  
00232 int main() {
00233     bool loop_forever = true;
00234     leds = 1;
00235     
00236     // init mouse tracking
00237     reset_mouse();
00238  
00239     // setup accelerometer
00240     accelerometer.setPowerControl(0x00);
00241     accelerometer.setDataFormatControl(0x0B);
00242     accelerometer.setDataRate(ADXL345_3200HZ);
00243     accelerometer.setPowerControl(0x08);
00244  
00245     state_t state = WAITING;    
00246     Timer timer;
00247  
00248     float angles[8] = {0};
00249     float stretches[8] = {0};
00250     
00251     while(loop_forever) {       
00252         // get the slingshot parameters
00253         float this_stretch = get_stretch();
00254         float this_angle = get_angle();
00255  
00256         // apply some filtering
00257         float stretch = filter(stretches, 8, this_stretch);
00258         float angle = filter(angles, 8, this_angle);
00259         
00260         // DreamForce 2013 Challenge: Adjust the angle to account for the relative angle between the sling and the slingshot body
00261         angle = adjust_for_sling_angle(angle);
00262         
00263         // DEBUG
00264         //std::printf("stretch=%.1f angle=%.1f\r\n",stretch,angle);
00265             
00266         leds = state;
00267                         
00268         // act based on the current state
00269         switch (state) {
00270             case WAITING:
00271                 if(stretch > stretch_start_threshold) {             // significant stretch, considered starting 
00272                     mouse.press(MOUSE_LEFT);
00273                     state = AIMING;
00274                 }
00275                 break;
00276  
00277             case AIMING:
00278                 if(stretch - this_stretch > fire_threshold) { // rapid de-stretch, considered a fire
00279                     mouse.release(MOUSE_LEFT);
00280                     reset_mouse();
00281                     timer.start();
00282                     state = FIRING;
00283                 } 
00284                 else if(stretch < stretch_start_threshold) { // de-stretch 
00285                     reset_mouse();
00286                     timer.stop();
00287                     timer.reset();
00288                     state = WAITING;
00289                 } else {
00290                     int x = 0.0 - cos(angle) * stretch * mouse_scale;
00291                     int y = sin(angle) * stretch * mouse_scale;
00292                     move_mouse(x, y);
00293                 }
00294                 break;
00295  
00296             case FIRING:
00297                 if(timer > fire_timing_threshold) {
00298                     timer.stop();
00299                     timer.reset();
00300                     reset_mouse();
00301                     state = WAITING;
00302                 }        
00303                 break;
00304         };
00305         
00306         // wait for 100ms
00307         wait_ms(100);
00308     }
00309 }