replacement code for Playground's ControlSensorsViewController.m

gistfile1.m

//
//  ControlSensorsViewController.m
//  playground
//
//  Created by Kevin Liang on 11/18/14.
//  Copyright (c) 2014 Wonder Workshop. All rights reserved.
//
//  Modified by Martin Bruner 11/24/14
//  Added state machine code for sound following
//
#import "ControlSensorsViewController.h"

@interface ControlSensorsViewController ()

@property (nonatomic, strong) NSTimer *refreshDataTimer;

- (void) refreshSensorData:(NSTimer *)timer;
- (void)ProcessSensorData:(WWSensorSet *)sensorData;
- (void)sendMoveCommandToDash:(double)lin withAngle:(double)angle;
- (void)earColorRed;
- (void)earColorGreen;
- (void)earColorBlue;

@end

#define RADIANS_TO_DEGREES(radians) ((radians) * (180.0 / M_PI))

@implementation ControlSensorsViewController

// used to see frame by frame
// audio data
#define DEBUG_NLOGS false

// Frame interval
float delay = .03;

// I use this to disable moving so I can
// run tests with Dash on my desk
bool enableMove = true;

// The different states for the state machine
enum stateDash { listenForSound, moveToSound, moveAwayFromObstacle};

// We will start out listening for a sound
enum stateDash currentState = listenForSound;

// This is my counter for the number of frames to
// move Dash
int timerInState=0;

// This is my max counter value
const int maxTimeInState = 33;

// I use this for ramping up and
// slowing down, preserving any
// angle.
double endingLinearVelocity;
double savedAngle;

// Method earColorBlue.  Turns ears Blue
- (void)earColorBlue{
    
    WWCommandSet *cmd = [WWCommandSet new];
    [cmd setLeftEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MIN green:WW_LIGHT_BRIGHTNESS_MIN blue:WW_LIGHT_BRIGHTNESS_MAX]];
    [cmd setRightEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MIN green:WW_LIGHT_BRIGHTNESS_MIN blue:WW_LIGHT_BRIGHTNESS_MAX]];
    
    [self sendCommandSetToRobots:cmd];
}

// Method earColorRed.  Turns ears red
- (void)earColorRed{
    
    WWCommandSet *cmd = [WWCommandSet new];
    [cmd setLeftEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MAX green:WW_LIGHT_BRIGHTNESS_MIN blue:WW_LIGHT_BRIGHTNESS_MIN]];
    [cmd setRightEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MAX green:WW_LIGHT_BRIGHTNESS_MIN blue:WW_LIGHT_BRIGHTNESS_MIN]];
    
    [self sendCommandSetToRobots:cmd];
}

// Method earColorGreen.  Turns ears Green
- (void)earColorGreen{
    
    WWCommandSet *cmd = [WWCommandSet new];
    [cmd setLeftEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MIN green:WW_LIGHT_BRIGHTNESS_MAX blue:WW_LIGHT_BRIGHTNESS_MIN]];
    [cmd setRightEarLight:[[WWCommandLightRGB alloc] initWithRed:WW_LIGHT_BRIGHTNESS_MIN green:WW_LIGHT_BRIGHTNESS_MAX blue:WW_LIGHT_BRIGHTNESS_MIN]];
    
    [self sendCommandSetToRobots:cmd];
}

// This method moves Dash. I wanted to encapsulate this method so
// I could provide disabling and logging.
-(void)sendMoveCommandToDash:(double)lin withAngle:(double)angle{
    if(enableMove){
        
        NSLog( @"Move command lin:'%@' angle:'%@' ", [[NSNumber numberWithFloat:lin] stringValue],[[NSNumber numberWithFloat:angle] stringValue]);
        WWCommandSet *cmdToSend = [WWCommandSet new];
        WWCommandBodyLinearAngular *linAng = [[WWCommandBodyLinearAngular alloc] initWithLinear:lin
                                                                                        angular:angle];
        [cmdToSend setBodyLinearAngular:linAng];
        
        [self sendCommandSetToRobots:cmdToSend];
    }
}

// This method is a state machine that uses the sensorData to move Dash around based upon
// detected sound and obstacles.
- (void)ProcessSensorData:(WWSensorSet *)sensorData{
    
    // Pull out sensor data
    WWSensorMicrophone *SMic = (WWSensorMicrophone *)[sensorData sensorForIndex:WW_SENSOR_MICROPHONE];
    WWSensorDistance *SDistFLF = (WWSensorDistance *)[sensorData sensorForIndex:WW_SENSOR_DISTANCE_FRONT_RIGHT_FACING];
    WWSensorDistance *SDistFRF = (WWSensorDistance *)[sensorData sensorForIndex:WW_SENSOR_DISTANCE_FRONT_LEFT_FACING];
    
    
    // see if we are being blocked.  This preempts any other operation.
    // My son might be wrestling with Dash...
    if((SDistFLF.reflectance> 15.0)||(SDistFRF.reflectance > 15.0))
    {
        // Turn ears blue
        [self earColorBlue];
        
        
        // Get dash to slowly move away.
        double lin = -10;
        double ang =0;
        
        [self sendMoveCommandToDash:lin withAngle:ang];
        
        currentState = moveAwayFromObstacle;
        NSLog(@"Change state moveAwayFromObstacle");
        timerInState = 0;
        return;
    }
    switch(currentState){
            double lin=0;
            double ang=0;
            
            // If we get to this case, then we were pulling away from an obstacle, but we are now
            // far enough away.
        case moveAwayFromObstacle:
            
            NSLog(@"Change state listenForSound");
            currentState = listenForSound;
            timerInState = 0;
            
            [self sendMoveCommandToDash:0 withAngle:0];
            
            break;
            
            // here we stay until we find a valid sound and direction
        case listenForSound:
            
            // Right now Dash reports a triangulationAngle of zero if he can't calculate a good angle.
            // So I ensure that I ignore triangulationAngle==0 (hey how often will a real 0 happen?)
            if((SMic.triangulationAngle != 0)&&(SMic.amplitude > 0.0002))
            {
                
                // We heard a good sound so color ears green
                [self earColorGreen];
                
                // if within the "visual angle" of Dash, move towards sound.
                if((SMic.triangulationAngle < M_PI/4 )&&(SMic.triangulationAngle > -M_PI/4)){
                    lin = 10; // start in a slower speed
                }
                
                // This variable is used for slow ramp ups and down
                endingLinearVelocity = lin;
                
                // Right now I have found that detected triangulationAngle/2
                // works pretty well for a one second movement
                ang = SMic.triangulationAngle*0.75;
                
                // Preserve rotation angle for ramp ups and downs
                savedAngle = ang;
                timerInState = 0;
                
                // Change state
                currentState = moveToSound;
                NSLog(@"Change state moveToSound");
                
                // Move Dash (start linear motion slow if exists)
                [self sendMoveCommandToDash:lin withAngle:ang];
            }else{
                
                // We didn't hear a good sound, so color the ears red
                [self earColorRed];
            }
            break;
            
            // If we are moving, move for only 33 frames (about 1 second).
            // I probably should break this state up into smaller states,
            // I am attempting to create a smoother start and stop for
            // the linear motion....
        case moveToSound:
            
            // This keeps track of how far we are into the movement
            timerInState++;
            
            // This is a situation where we just started.
            // if we are also doing linear motion, bump it up to full
            // speed
            if((timerInState == 1)&&(endingLinearVelocity==10)){
                lin=20;
                ang = savedAngle;
                [self sendMoveCommandToDash:lin withAngle:ang];
            }
            
            // Here we are almost finished with the movment.
            // If we are doing linear motion then first slow down
            if ((timerInState==maxTimeInState)&&(endingLinearVelocity==10)) {
                lin=10;
                ang = savedAngle;
                [self sendMoveCommandToDash:lin withAngle:ang];
            }
            
            // Fully stop Dash and listen for next sound
            if (timerInState>maxTimeInState) {
                lin=0;
                ang = 0;
                [self sendMoveCommandToDash:lin withAngle:ang];
                currentState = listenForSound;
                NSLog(@"Change state listenForSound");
            }
            
            break;
            
            
    }
    
}

- (void)viewDidLoad {
    [super viewDidLoad];
    // Do any additional setup after loading the view.
    
    self.dataTableView.rowHeight = 200;
}

- (void) viewWillAppear:(BOOL)animated {
    [super viewWillAppear:animated];
    
    self.refreshDataTimer = [NSTimer scheduledTimerWithTimeInterval:delay  target:self selector:@selector(refreshSensorData:) userInfo:nil repeats:YES];
}

- (void) viewWillDisappear:(BOOL)animated {
    [super viewWillDisappear:animated];
    
    [self.refreshDataTimer invalidate];
}

- (void) refreshSensorData:(NSTimer *)timer {
    [self.dataTableView reloadData];
}

# pragma mark - table view
- (NSInteger) numberOfSectionsInTableView:(UITableView *)tableView {
    return 1;
}

- (NSInteger) tableView:(UITableView *)tableView numberOfRowsInSection:(NSInteger)section {
    return self.connectedRobots.count;
}

- (UITableViewCell *) tableView:(UITableView *)tableView cellForRowAtIndexPath:(NSIndexPath *)indexPath {
    UITableViewCell *cell = [tableView dequeueReusableCellWithIdentifier:@"DataCell" forIndexPath:indexPath];
    
    WWRobot *robot = (WWRobot *)self.connectedRobots[indexPath.row];
    WWSensorSet *sensorData = robot.history.currentState;
    
    NSMutableString *detail = [NSMutableString stringWithCapacity:2000];
    [detail appendFormat:@"Name: %@\n", robot.name];
    
    //buttons
    WWSensorButton *SButtonMain = (WWSensorButton *)[sensorData sensorForIndex:WW_SENSOR_BUTTON_MAIN];
    WWSensorButton *SButtonOne = (WWSensorButton *)[sensorData sensorForIndex:WW_SENSOR_BUTTON_1];
    WWSensorButton *SButtonTwo = (WWSensorButton *)[sensorData sensorForIndex:WW_SENSOR_BUTTON_2];
    WWSensorButton *SButtonThree = (WWSensorButton *)[sensorData sensorForIndex:WW_SENSOR_BUTTON_3];
    [detail appendFormat:@"Btn Main: %@, ", SButtonMain.isPressed? @"true": @"false"];
    [detail appendFormat:@"Btn 1: %@, ", SButtonOne.isPressed? @"true": @"false"];
    [detail appendFormat:@"Btn 2: %@, ", SButtonTwo.isPressed? @"true": @"false"];
    [detail appendFormat:@"Btn 3: %@", SButtonThree.isPressed? @"true": @"false"];
    [detail appendFormat:@"\n"];
    
    //accelerometer
    WWSensorAccelerometer *SAcc = (WWSensorAccelerometer *)[sensorData sensorForIndex:WW_SENSOR_ACCELEROMETER];
    [detail appendString:@"Accel: "];
    [detail appendFormat:@"X: %3.2f, ", SAcc ? SAcc.x : NAN];
    [detail appendFormat:@"Y: %3.2f, ", SAcc ? SAcc.y : NAN];
    [detail appendFormat:@"Z: %3.2f", SAcc ? SAcc.z : NAN];
    [detail appendFormat:@"\n"];
    
    // Microphone sensor
    WWSensorMicrophone *SMic = (WWSensorMicrophone *)[sensorData sensorForIndex:WW_SENSOR_MICROPHONE];
    [detail appendFormat:@"Microphone: "];
    [detail appendFormat:@"Amplitude: %03f, ", SMic ? SMic.amplitude : NAN];
    [detail appendFormat:@"Angle: %3.2f degrees", RADIANS_TO_DEGREES(SMic ? SMic.triangulationAngle : NAN)];
    [detail appendFormat:@"\n"];
    
#if DEBUG_NLOGS == true
    if (SMic) {
        
        NSString *message = [[NSNumber numberWithFloat:SMic.amplitude] stringValue];
        
        NSLog( @"SMic.amplitude:          '%@'", message );
        
        message = [[NSNumber numberWithFloat:SMic.triangulationAngle] stringValue];
        
        NSLog( @"SMic.triangulationAngle: '%@'", message );
        
        
        message = [[NSNumber numberWithFloat:SAcc.x] stringValue];
        
        NSLog( @"SAcc.x:                  '%@'", message );
    }else{
        NSString *message = @"No Data";
        
        NSLog( @"Sorry: '%@'", message );
        
        
    }
#endif
    
    
    if (robot.robotType == WW_ROBOT_DASH) {
        //distance sensors
        WWSensorDistance *SDistFLF = (WWSensorDistance *)[sensorData sensorForIndex:WW_SENSOR_DISTANCE_FRONT_RIGHT_FACING];
        WWSensorDistance *SDistFRF = (WWSensorDistance *)[sensorData sensorForIndex:WW_SENSOR_DISTANCE_FRONT_LEFT_FACING];
        WWSensorDistance *SDistRRF = (WWSensorDistance *)[sensorData sensorForIndex:WW_SENSOR_DISTANCE_BACK];
        [detail appendFormat:@"Distance: "];
        [detail appendFormat:@"Left-Facing: %2.2f, ", SDistFLF ? SDistFLF.reflectance : NAN];
        [detail appendFormat:@"Dist Right-Facing: %2.2f, ", SDistFRF ? SDistFRF.reflectance : NAN];
        [detail appendFormat:@"Dist Tail: %2.2f", SDistRRF ? SDistRRF.reflectance : NAN];
        [detail appendFormat:@"\n"];
        
        //gyro
        WWSensorGyroscope *SGyro = (WWSensorGyroscope *)[sensorData sensorForIndex:WW_SENSOR_GYROSCOPE];
        [detail appendString:@"Gyro: "];
        [detail appendFormat:@"yaw: %3.2f, ", SGyro ? SGyro.yaw : NAN];
        [detail appendFormat:@"pitch: %3.2f, ", SGyro ? SGyro.pitch : NAN];
        [detail appendFormat:@"roll: %3.2f", SGyro ? SGyro.roll : NAN];
        [detail appendFormat:@"\n"];
        
        //head
        WWSensorHeadPosition *SMotorServoHeadPan = (WWSensorHeadPosition *)[sensorData sensorForIndex:WW_SENSOR_HEAD_POSITION_PAN];
        WWSensorHeadPosition *SMotorServoHeadTilt = (WWSensorHeadPosition *)[sensorData sensorForIndex:WW_SENSOR_HEAD_POSITION_TILT];
        [detail appendString:@"Head: "];
        [detail appendFormat:@"Pan: %3.2f degrees, ", RADIANS_TO_DEGREES(SMotorServoHeadPan ? SMotorServoHeadPan.radians : NAN)];
        [detail appendFormat:@"Tilt: %3.2f degrees", RADIANS_TO_DEGREES(SMotorServoHeadTilt ? SMotorServoHeadTilt.radians : NAN)];
        [detail appendFormat:@"\n"];
        
        //encoder
        WWSensorEncoder *SEncoderLW = (WWSensorEncoder *)[sensorData sensorForIndex:WW_SENSOR_ENCODER_LEFT_WHEEL];
        WWSensorEncoder *SEncoderRW = (WWSensorEncoder *)[sensorData sensorForIndex:WW_SENSOR_ENCODER_RIGHT_WHEEL];
        [detail appendString:@"Encoder: "];
        [detail appendFormat:@"Left: %4.2f cm, ", SEncoderLW? SEncoderLW.distance: NAN];
        [detail appendFormat:@"Right: %4.2f cm", SEncoderLW? SEncoderRW.distance: NAN];
        [detail appendFormat:@"\n"];
        
        //****************************************************************************
        // Here is my hook for the sound following state machine.
        //****************************************************************************
        [self ProcessSensorData:sensorData];
    }
    cell.textLabel.text = detail;
    //[cell.textLabel sizeToFit];
    return cell;
}


- (void)didReceiveMemoryWarning {
    [super didReceiveMemoryWarning];
    // Dispose of any resources that can be recreated.
}

@end