File content as of revision 9:3af90289b117:

/* mbed USB Slingshot, 
 *
 * Copyright (c) 2010-2011 mbed.org, MIT License
 * 
 * smokrani, sford, danson, sgrove 
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
 * and associated documentation files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 * 
 *  The above copyright notice and this permission notice shall be included in all copies or
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
//
// DreamForce 2013 Challenge: 
//
//      Goal: modify the code below to adjust the sling body angle (theta) to take into account 
//            the relative angle between the sling body and the sling band.
//
//
//      Mini-hack challenge: Your mission, should you choose to accept it, is to complete the function
//                           "potentiometer_value_to_degrees()" below (around line 113) to return a reasonable 
//                           estimate of the sling band angle relative to the sling body 
// 
//

// Sling Tunables Start - !!! be careful changing these !!!

// stretch start threshold
float stretch_start_threshold = 0.4;

// fire threshold
float fire_threshold = 0.15;

// fire timing threshold
float fire_timing_threshold = 3.0;

// scaling for mouse movement - may need to be maniuplated depending on screen resolution to get a full deflection
int mouse_scale = 250;

// Sling Tunables End

// definition of PI
#define M_PI    3.14159

// Includes
#include "mbed.h"
#include "USBMouse.h"
#include "ADXL345.h"
 
// Physical interfaces
USBMouse mouse;
ADXL345 accelerometer(p5, p6, p7, p8);
AnalogIn stretch_sensor(p16);
BusOut leds(LED1, LED2, LED3, LED4);

// Potentiometer
AnalogIn pot_1(p19);

// keep track of mouse position
int current_x = 0;
int current_y = 0;

// Potentiometer filters
#include "filter.h"
medianFilter prefilter(13);
medianFilter postfilter(7);

//  return radians for a given degree
float degrees_to_radians(float degrees) {
    float radians = ((M_PI*degrees)/180.0); 
    return radians;
}

//  return degrees for a given radian
float radians_to_degrees(float radians) {
    float degrees = ((180*radians)/M_PI); 
    return degrees;
}

//  get_potentiometer_value() reads the potentiometer, filters its value and remaps it to [0, 100.0]
float get_potentiometer_value(AnalogIn pot) {
    float f = pot;
    f = prefilter.process(f);       // pre-filter
    f = (f * 100);                  // remap: [ 0, 100]
    return postfilter.process(f);   // post-filter after remap
}

//
// DreamForce 2013 Challenge: 
//  potentiometer_value_to_degrees() takes the potentiometer value (val_pot) and
//  maps it to an angle between [min_sling_angle, max_sling_angle] as defined in the tunables section
//
//  NOTE: This function is INCOMPLETE. To complete it you should:
//        1). Uncomment the debug statement, run the program, and look at raw potentiometer values
//        2). Determine the min and max potentiometer values you wish to scale to degrees
//        3). Determine the 90 degree potentiometer value ("median_pot") that denotes the sling band at 90 to the sling body
//        4). Guestimate the range of angles you wish to map the potentiometer values (i.e. -x degrees to +y degrees)
//        5). Fill in min_deg and max_deg below with those angle ranges
//        6). Compile up and give it a try
//        7). Additional hint: you may need to modify mouse_scale above to get a full deflection when you stretch the string back.. 
//
float potentiometer_value_to_degrees(float val_pot) {
    float deg = 0.0;
    float accuracy = 0.1;
    
    // DEBUG - may need this to calibrate pot values below
    //std::printf("Raw pot value=%.1f\r\n",val_pot);
    
    // Potentiometer range: typically about [36.8, 80.6] with 56.0 being "median_pot"
    float median_pot = 0.0;
    float min_pot = 0.0;
    float max_pot = 0.0;
    float incr_pot = (max_pot*10) - (min_pot*10);       // how many .1 increments we have in the interval [min, max]
    
    // Mapped degree range: INCOMPLETE: you need to guesstimate the approx angle range i.e. [-x, +y] degrees so convert to
    float min_deg = 0.0;
    float max_deg = 0.0;
    float incr_deg = (max_deg*10) - (min_deg*10);       // how many .1 increments we have in the interval [min, max]
    
    // see if we are centered or not
    float centered_pot = fabs(val_pot - median_pot);
  
    // if we are off 90 degrees (i.e. sling body and sling band are not at 90 degrees) - calculate the relative angle
    if (centered_pot > accuracy) {
        // map to degree range
        float conversion = (incr_deg/incr_pot);
        deg = min_deg + (conversion*(val_pot - min_pot));                
    }
        
    // return the calculated degrees
    return deg;
}

// adjust the final angle (theta) taking into account the relative angle between the sling body and the sling band.
float adjust_for_sling_angle(float slingshot_body_angle) {    
    // get the sling angle through approximation with the potentiometer
    float sling_angle_degrees = potentiometer_value_to_degrees(get_potentiometer_value(pot_1));
    
    // the sling angle is in degrees - so lets convert the body angle to degrees as well
    float modified_angle_degrees = radians_to_degrees(slingshot_body_angle);
    
    // we simply add the sling angle to adjust it
    modified_angle_degrees += sling_angle_degrees;
    
    // make sure that we are always between 0 and 359 degrees
    while (modified_angle_degrees > 360.0) modified_angle_degrees = modified_angle_degrees - 360;
    while (modified_angle_degrees < 0.0) modified_angle_degrees = modified_angle_degrees + 360;
    
    // convert the modified angle back to radians
    float modified_angle_radians = degrees_to_radians(modified_angle_degrees);
    
    // DEBUG
    //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);
            
    // return the modified angle
    return modified_angle_radians;
}

// Return slingshot angle in radians, up > 0 > down
float get_angle() {
    int readings[3];
    accelerometer.getOutput(readings);
    float x = (int16_t)readings[0];
    float z = (int16_t)readings[2];
    return atan(z / x);    
}
 
// Return normalised stretch value based on bounds of all readings seen
float get_stretch() {
    static float min_strength = 0.7;
    static float max_strength = 0.7;
    float current_strength = stretch_sensor.read();
    if(current_strength > max_strength) { max_strength = current_strength; }
    if(current_strength < min_strength) { min_strength = current_strength; }
    float stretch = (current_strength - min_strength) / (max_strength - min_strength);
    return 1.0 - stretch;
}
 
// move mouse to a location relative to the start point, stepping as needed
void move_mouse(int x, int y) {
    const int STEP = 10;
    
    int move_x = x - current_x;
    int move_y = y - current_y; 
 
    // Move the mouse, in steps of max step size to ensure it is picked up by OS
    while(move_x > STEP) { mouse.move(STEP, 0); move_x -= STEP; }
    while(move_x < -STEP) { mouse.move(-STEP, 0); move_x += STEP; }
    while(move_y > STEP) { mouse.move(0, STEP); move_y -= STEP; }
    while(move_y < -STEP) { mouse.move(0, -STEP); move_y += STEP; }
    mouse.move(move_x, move_y);   
    
    current_x = x;
    current_y = y;
}

// reset the mouse position
void reset_mouse() {
    current_x = 0;
    current_y = 0;
    mouse.move(0,0);
}
 
template <class T>
T filter(T* array, int len, T value) {
    T mean = 0.0;
    for(int i = 0; i<len - 1; i++) {
        mean += array[i + 1];
        array[i] = array[i + 1];
    }
    mean += value;
    array[len - 1] = value;
    return mean / (T)len;
}
 
typedef enum {
    WAITING = 2,
    AIMING = 4,
    FIRING = 8
} state_t;
 
int main() {
    bool loop_forever = true;
    leds = 1;
    
    // init mouse tracking
    reset_mouse();
 
    // setup accelerometer
    accelerometer.setPowerControl(0x00);
    accelerometer.setDataFormatControl(0x0B);
    accelerometer.setDataRate(ADXL345_3200HZ);
    accelerometer.setPowerControl(0x08);
 
    state_t state = WAITING;    
    Timer timer;
 
    float angles[8] = {0};
    float stretches[8] = {0};
    
    while(loop_forever) {       
        // get the slingshot parameters
        float this_stretch = get_stretch();
        float this_angle = get_angle();
 
        // apply some filtering
        float stretch = filter(stretches, 8, this_stretch);
        float angle = filter(angles, 8, this_angle);
        
        // DreamForce 2013 Challenge: Adjust the angle to account for the relative angle between the sling and the slingshot body
        angle = adjust_for_sling_angle(angle);
        
        // DEBUG
        //std::printf("stretch=%.1f angle=%.1f\r\n",stretch,angle);
            
        leds = state;
                        
        // act based on the current state
        switch (state) {
            case WAITING:
                if(stretch > stretch_start_threshold) {             // significant stretch, considered starting 
                    mouse.press(MOUSE_LEFT);
                    state = AIMING;
                }
                break;
 
            case AIMING:
                if(stretch - this_stretch > fire_threshold) { // rapid de-stretch, considered a fire
                    mouse.release(MOUSE_LEFT);
                    reset_mouse();
                    timer.start();
                    state = FIRING;
                } 
                else if(stretch < stretch_start_threshold) { // de-stretch 
                    reset_mouse();
                    timer.stop();
                    timer.reset();
                    state = WAITING;
                } else {
                    int x = 0.0 - cos(angle) * stretch * mouse_scale;
                    int y = sin(angle) * stretch * mouse_scale;
                    move_mouse(x, y);
                }
                break;
 
            case FIRING:
                if(timer > fire_timing_threshold) {
                    timer.stop();
                    timer.reset();
                    reset_mouse();
                    state = WAITING;
                }        
                break;
        };
        
        // wait for 100ms
        wait_ms(100);
    }
}