Ticket #69: example.c

#include "config.h"
#include "stdlib.h"
#include "string.h"
#include <stdio.h>

#define LEFT_ENGINE   0
#define RIGHT_ENGINE  1
#define MIN_SPEED   10
#define MAX_SPEED   30
#define STEP      1

void Run( ) {
  
  int Left, Center, Right;

  Usb_SetActive( 1 );
  // Define robot name
  System_SetName( "R23");
  // Fix for initial negative stepping
  Stepper_SetPosition(  LEFT_ENGINE,    100000);
  Stepper_SetPosition(  RIGHT_ENGINE,   100000);
  while ( true ) {
    Right   = AnalogIn_GetValue(0);
    Center  = AnalogIn_GetValue(1);
    Left  = AnalogIn_GetValue(2);
    // FRONT 1 - 0 - 1
    if ( Left > 1000 && Center < 10 && Right > 1000 ) {
      Stepper_SetSpeed( LEFT_ENGINE,    MAX_SPEED );
      Stepper_Step(     LEFT_ENGINE,    STEP );
      Stepper_SetSpeed(   RIGHT_ENGINE,   MAX_SPEED );
      Stepper_Step(     RIGHT_ENGINE,   STEP );
    // LEFT 0 - 1 - 1
    // ***** Problem is in these next two if's, where I want to set different speeds for each steeper *****
    } else if ( Left < 10 && Center > 1000 && Right > 1000 ) {
      Stepper_SetSpeed( LEFT_ENGINE,    MIN_SPEED );
      Stepper_Step(     LEFT_ENGINE,    STEP );
      Stepper_SetSpeed( RIGHT_ENGINE,   MAX_SPEED );
      Stepper_Step(     RIGHT_ENGINE,   STEP );
    // RIGHT 1 - 1 - 0
    } else if ( Left > 1000 && Center > 1000 && Right < 10 ) {
      AppLed_SetState( 0, 0 ); AppLed_SetState( 1, 1 ); AppLed_SetState( 2, 1 );
      Stepper_SetSpeed( LEFT_ENGINE,    MAX_SPEED );
      Stepper_Step(     LEFT_ENGINE,    STEP );
      Stepper_SetSpeed( RIGHT_ENGINE,   MIN_SPEED );
      Stepper_Step(     RIGHT_ENGINE,   STEP );
    } else {
      Stepper_SetActive( 0, 0 );
      Stepper_SetActive( 1, 0 );
    }
  }
}