Error in code light on, trouble finding error

[Bryan Herbst]

Hey guys, finally got code to compile (I really hate MPLAB now), and got it to download to the controller, but the OI has the code error, no valid data/radio (blinking red), Program state (blinking orange), and radio modem (blinking red) lights on, and the RC mode light is off. I've been looking through the code, and have no idea whats going on.

User Routine-

/*******************************************************************************
* FILE NAME: user_routines.c <FRC VERSION>
*
* DESCRIPTION:
*  This file contains the default mappings of inputs  
*  (like switches, joysticks, and buttons) to outputs on the RC.  
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
*******************************************************************************/


#include <stdio.h>

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "serial_ports.h"
#include "pwm.h"
#include "camera.h"
#include "camera_menu.h"
#include "tracking.h"
#include "tracking_menu.h"
#include "eeprom.h"
#include "terminal.h"


extern unsigned char aBreakerWasTripped;

int intMax(int x, int y)
{
 int max = x;
 if(y > max)
 {
     max = y;
 }
   return max;
}

int intABS(int i)
{
 if (i < 0)
  return -i;
 else
  return i;
}

     rom const int ctbl[255] = {-127, -124.654754, -122.3407176, -120.0583477,
 -117.8073909, -115.5875939, -113.3987034, -111.2404659,
 -109.1126283, -107.0149371, -104.947139, -102.9089807,
 -100.9002089, -98.92057018, -96.96981128, -95.04767885,
 -93.15391954, -91.28828005, -89.45050702, -87.64034713,
 -85.85754704, -84.10185344, -82.37301297, -80.67077232,
 -78.99487816, -77.34507714, -75.72111594, -74.12274122,
 -72.54969966, -71.00173793, -69.47860269, -67.9800406,
 -66.50579835, -65.05562259, -63.62926, -62.22645725,
 -60.84696099, -59.49051791, -58.15687466, -56.84577793,
 -55.55697437, -54.29021066, -53.04523346, -51.82178944,
 -50.61962527, -49.43848762, -48.27812316, -47.13827855,
 -46.01870047, -44.91913557, -43.83933055, -42.77903204,
 -41.73798674, -40.71594131, -39.71264241, -38.72783671,
 -37.76127088, -36.81269159, -35.88184552, -34.96847932,
 -34.07233966, -33.19317322, -32.33072666, -31.48474665,
 -30.65497986, -29.84117296, -29.04307261, -28.26042549,
 -27.49297827, -26.7404776, -26.00267016, -25.27930263,
 -24.57012165, -23.87487392, -23.19330609, -22.52516483,
 -21.87019681, -21.2281487, -20.59876716, -19.98179888,
 -19.37699051, -18.78408872, -18.20284018, -17.63299156,
 -17.07428953, -16.52648076, -15.98931191, -15.46252965,
 -14.94588066, -14.4391116, -13.94196914, -13.45419994,
 -12.97555068, -12.50576802, -12.04459864, -11.5917892,
 -11.14708637, -10.71023681, -10.28098721, -9.859084213,
 -9.444274504, -9.036304746, -8.634921609, -8.239871761,
 -7.85090187, -7.467758605, -7.090188633, -6.717938625,
 -6.350755247, -5.988385169, -5.630575058, -5.277071584,
 -4.927621415, -4.581971219, -4.239867665, -3.901057421,
 -3.565287155, -3.232303537, -2.901853234, -2.573682915,
 -2.247539249, -1.923168903, -1.600318547, -1.278734848,
 -0.958164476, -0.638354098, -0.319050383, 0,
 0.319050383, 0.638354098, 0.958164476, 1.278734848,
 1.600318547, 1.923168903, 2.247539249, 2.573682915,
 2.901853234, 3.232303537, 3.565287155, 3.901057421,
 4.239867665, 4.581971219, 4.927621415, 5.277071584,
 5.630575058, 5.988385169, 6.350755247, 6.717938625,
 7.090188633, 7.467758605, 7.85090187, 8.239871761,
 8.634921609, 9.036304746, 9.444274504, 9.859084213,
 10.28098721, 10.71023681, 11.14708637, 11.5917892,
 12.04459864, 12.50576802, 12.97555068, 13.45419994,
 13.94196914, 14.4391116, 14.94588066, 15.46252965,
 15.98931191, 16.52648076, 17.07428953, 17.63299156,
 18.20284018, 18.78408872, 19.37699051, 19.98179888,
 20.59876716, 21.2281487, 21.87019681, 22.52516483,
 23.19330609, 23.87487392, 24.57012165, 25.27930263,
 26.00267016, 26.7404776, 27.49297827, 28.26042549,
 29.04307261, 29.84117296, 30.65497986, 31.48474665,
 32.33072666, 33.19317322, 34.07233966, 34.96847932,
 35.88184552, 36.81269159, 37.76127088, 38.72783671,
 39.71264241, 40.71594131, 41.73798674, 42.77903204,
 43.83933055, 44.91913557, 46.01870047, 47.13827855,
 48.27812316, 49.43848762, 50.61962527, 51.82178944,
 53.04523346, 54.29021066, 55.55697437, 56.84577793,
 58.15687466, 59.49051791, 60.84696099, 62.22645725,
 63.62926, 65.05562259, 66.50579835, 67.9800406,
 69.47860269, 71.00173793, 72.54969966, 74.12274122,
 75.72111594, 77.34507714, 78.99487816, 80.67077232,
 82.37301297, 84.10185344, 85.85754704, 87.64034713,
 89.45050702, 91.28828005, 93.15391954, 95.04767885,
 96.96981128, 98.92057018, 100.9002089, 102.9089807,
 104.947139, 107.0149371, 109.1126283, 111.2404659,
 113.3987034, 115.5875939, 117.8073909, 120.0583477,
 122.3407176, 124.654754, 127};


int joysticklimit(int inp)
{
 if(inp < 127)
  {
   inp += 1;
   return inp;
  }
 else if (inp > 127)
  {
   inp -= 1;
   return inp;
  }
 else
  {
   return inp;
  }
}

int count = 0;
int nestedcount = 0;
static int oldcount = 0;
int countvert = 0;
static int oldcountvert = 0;
int countvert2 = 0;
static int oldcountvert2 = 0;
int count3 = 0;
static int oldcount3 = 0;
int rotationfw = 0;
int rotationrv = 0;
int groundlevel = 0;
int toplevel = 0;

/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
/* EXAMPLES: (see MPLAB C18 User's Guide, p.9 for all types)
unsigned char wheel_revolutions = 0; (can vary from 0 to 255)
unsigned int  delay_count = 7;       (can vary from 0 to 65,535)
int           angle_deviation = 142; (can vary from -32,768 to 32,767)
unsigned long very_big_counter = 0;  (can vary from 0 to 4,294,967,295)
*/

/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Max
* PURPOSE:       Sets a PWM value to neutral (127) if it exceeds 127 and the
*                limit switch is on.
* CALLED FROM:   this file
* ARGUMENTS:     
*     Argument       Type             IO   Description
*     --------       -------------    --   -----------
*     switch_state   unsigned char    I    limit switch state
*     *input_value   pointer           O   points to PWM byte value to be limited
* RETURNS:       void
*******************************************************************************/
void Limit_Switch_Max(unsigned char switch_state, unsigned char *input_value)
{
  if (switch_state == CLOSED)
  { 
    if(*input_value > 127)
      *input_value = 127;
  }
}


/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Min
* PURPOSE:       Sets a PWM value to neutral (127) if it's less than 127 and the
*                limit switch is on.
* CALLED FROM:   this file
* ARGUMENTS:     
*     Argument       Type             IO   Description
*     --------       -------------    --   -----------
*     switch_state   unsigned char    I    limit switch state
*     *input_value   pointer           O   points to PWM byte value to be limited
* RETURNS:       void
*******************************************************************************/
void Limit_Switch_Min(unsigned char switch_state, unsigned char *input_value)
{
  if (switch_state == CLOSED)
  { 
    if(*input_value < 127)
      *input_value = 127;
  }
}


/*******************************************************************************
* FUNCTION NAME: Limit_Mix
* PURPOSE:       Limits the mixed value for one joystick drive.
* CALLED FROM:   Default_Routine, this file
* ARGUMENTS:     
*     Argument             Type    IO   Description
*     --------             ----    --   -----------
*     intermediate_value    int    I    
* RETURNS:       unsigned char
*******************************************************************************/
unsigned char Limit_Mix (int intermediate_value)
{
  static int limited_value;
  
  if (intermediate_value < 2000)
  {
    limited_value = 2000;
  }
  else if (intermediate_value > 2254)
  {
    limited_value = 2254;
  }
  else
  {
    limited_value = intermediate_value;
  }
  return (unsigned char) (limited_value - 2000);
}


/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE:       This routine is called first (and only once) in the Main function.  
*                You may modify and add to this function.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Initialization (void)
{
  Set_Number_of_Analog_Channels(SIXTEEN_ANALOG);    /* DO NOT CHANGE! */

/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
  digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;

  relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;


/* FOURTH: Set your initial PWM values.  Neutral is 127. */
  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;

/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
  /*   Choose from these parameters for PWM 13-16 respectively:               */
  /*     IFI_PWM  - Standard IFI PWM output generated with Generate_Pwms(...) */
  /*     USER_CCP - User can use PWM pin as digital I/O or CCP pin.           */
//  Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);

  // changed so PWM() can control PWM outputs 13 through 16
  Setup_PWM_Output_Type(USER_CCP,USER_CCP,USER_CCP,USER_CCP);

  /*
     Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:

         CCP2CON = 0x3C;
         PR2 = 0xF9;
         CCPR2L = 0x7F;
         T2CON = 0;
         T2CONbits.TMR2ON = 1;

         Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
  */

  /* Add any other initialization code here. */

  // initialize the CCP PWM hardware
  Initialize_PWM();
 
  // initialize the serial ports
  Init_Serial_Port_One();
  Init_Serial_Port_Two();

  // make sure printf() output goes to the proper port
  #ifdef TERMINAL_SERIAL_PORT_1  
  stdout_serial_port = SERIAL_PORT_ONE;
  #endif

  #ifdef TERMINAL_SERIAL_PORT_2  
  stdout_serial_port = SERIAL_PORT_TWO;
  #endif

  Putdata(&txdata);            /* DO NOT CHANGE! */

  User_Proc_Is_Ready();         /* DO NOT CHANGE! - last line of User_Initialization */
}

/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE:       Executes every 26.2ms when it gets new data from the master 
*                microprocessor.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
  static unsigned char i;

  Getdata(&rxdata);   /* Get fresh data from the master microprocessor. */

  Default_Routine();  /* Optional.  See below. */

  /* Add your own code here. (a printf will not be displayed when connected to the breaker panel unless a Y cable is used) */

  printf("Port1 Y %3d, X %3d, Fire %d, Top %d\r",pwm01,pwm05,p1_sw_trig,p1_sw_top);  /* printf EXAMPLE */

  /* Example code to check if a breaker was ever tripped. */

  Putdata(&txdata);             /* DO NOT CHANGE! */
}

/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE:       Performs the default mappings of inputs to outputs for the
*                Robot Controller.
* CALLED FROM:   this file, Process_Data_From_Master_uP routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/

 void Default_Routine(void)
{

   signed double wL, wR, velocity, rotation, wMax;
 signed int difference, difference2, difference3, difference4;
 signed int prepwm01, prepwm02;
 static char pwm01_1, pwm02_1;

 /*velocity = (p1_y - 127);
 rotation = (p1_x - 127); */

if (rc_dig_in01 == 1) { //If tank pressure is 120... (Jeff didn't know whether the switch gave a 1 or 0 at 120psi)
relay2_fwd = 0;
relay2_rev = 0;
}
else {
relay2_fwd =1;
relay2_rev = 0;
}

/*
 velocity = deadband(p1_y,127,15);
 rotation = deadband(p1_x,127,15);
 */

 velocity = ctbl[joysticklimit(p1_y)];
 rotation = ctbl[joysticklimit(p1_x)];
 
 wL = velocity - rotation;
 wR = velocity + rotation;


   wMax = intMax(127, intABS(wL));
   wMax = intMax(wMax, intABS(wR));


/*Below check if the wheel output thingamajigger is valid or not (less than 127, great than -127).

If it isn't, it will go on and check the next wheel output, and if it's all good, it'll write it to prepwms

*/


 wL = (wL * 127 / wMax);
 if (-127 > wL || wL > 127)
   printf("Received an unexpected number [wL] /n");
else
 {
  wR = (wR * 127 / wMax);
  if (-127 > wR || wR > 127)
    printf("Received an unexpected number [wR] /n");
 
  else
   {

 prepwm01 = wL + 127;
 prepwm02 = 127 - wR;


  // looking at the wheels counter clockwise is postive
  // 7 is too little,
 
 if ((prepwm01 > 127) && (prepwm01 < 180)) {
  prepwm01 = prepwm01 + 12;
 }

 if ((prepwm02 > 127) && (prepwm02 < 180)) {
  prepwm02 = prepwm02 + 12;
 }


 pwm01 = prepwm01;
 pwm02 = prepwm02;

 /*if (((intABS((char)pwm01 - pwm01_1)) > 0 ) || ((intABS((char)pwm02 - pwm02_1)) > 0 ) || ((intABS((char)pwm03 - pwm03_1)) > 0 ) || ((intABS((char)pwm04 - pwm04_1)) > 0 )) {
  printf(" pwm01: %d , pwm02: %d \r", pwm01, pwm02);
  printf(" Port 1 Y: %d , Port 1 X: %d , Port 2 X: %d \r", p1_y, p1_x, p2_x);
 }

 pwm01_1 = intABS(pwm01 - 127);
 pwm02_1 = intABS(pwm02 - 127);


 /*---------- Buttons to Relays----------------------------------------------
  *--------------------------------------------------------------------------
  *  This default code maps the joystick buttons to specific relay outputs.
  */

relay1_fwd = p1_sw_trig;
relay1_rev = p1_sw_top; //Will kick if trigger is pulled, retract if top button is pushed. No button = no kicker action.
 
  
 /*---------- ROBOT FEEDBACK LEDs------------------------------------------------
  *------------------------------------------------------------------------------
  *   This section drives the "ROBOT FEEDBACK" lights on the Operator Interface.
  *   The lights are green for joystick forward and red for joystick reverse.
  *   Both red and green are on when the joystick is centered.  Use the
  *   trim tabs on the joystick to adjust the center.     
  *   These may be changed for any use that the user desires.                       
  */	
  
  if (user_display_mode == 0) /* User Mode is Off */
    
  { /* Check position of Port 1 Joystick */
    if (p1_y >= 0 && p1_y <= 56)
    {                     /* Joystick is in full reverse position */
      Pwm1_green  = 0;    /* Turn PWM1 green LED - OFF */
      Pwm1_red  = 1;      /* Turn PWM1 red LED   - ON  */
    }
    else if (p1_y >= 125 && p1_y <= 129)
    {                     /* Joystick is in neutral position */
      Pwm1_green  = 1;    /* Turn PWM1 green LED - ON */
      Pwm1_red  = 1;      /* Turn PWM1 red LED   - ON */
    }
    else if (p1_y >= 216 && p1_y <= 255)
    {                     /* Joystick is in full forward position*/
      Pwm1_green  = 1;    /* Turn PWM1 green LED - ON  */
      Pwm1_red  = 0;      /* Turn PWM1 red LED   - OFF */
    }
    else
    {                     /* In either forward or reverse position */
      Pwm1_green  = 0;    /* Turn PWM1 green LED - OFF */
      Pwm1_red  = 0;      /* Turn PWM1 red LED   - OFF */
    }  /*END Check position of Port 1 Joystick
    
    /* Check position of Port 2 Y Joystick 
           (or Port 1 X in Single Joystick Drive Mode) */
    if (p2_y >= 0 && p2_y <= 56)
    {                     /* Joystick is in full reverse position */
      Pwm2_green  = 0;    /* Turn pwm2 green LED - OFF */
      Pwm2_red  = 1;      /* Turn pwm2 red LED   - ON  */
    }
    else if (p2_y >= 125 && p2_y <= 129)
    {                     /* Joystick is in neutral position */
      Pwm2_green  = 1;    /* Turn PWM2 green LED - ON */
      Pwm2_red  = 1;      /* Turn PWM2 red LED   - ON */
    }
    else if (p2_y >= 216 && p2_y <= 255)
    {                     /* Joystick is in full forward position */
      Pwm2_green  = 1;    /* Turn PWM2 green LED - ON  */
      Pwm2_red  = 0;      /* Turn PWM2 red LED   - OFF */
    }
    else
    {                     /* In either forward or reverse position */
      Pwm2_green  = 0;    /* Turn PWM2 green LED - OFF */
      Pwm2_red  = 0;      /* Turn PWM2 red LED   - OFF */
    }  /* END Check position of Port 2 Joystick */
    
    /* This drives the Relay 1 and Relay 2 "Robot Feedback" lights on the OI. */
    Relay1_green = relay1_fwd;    /* LED is ON when Relay 1 is FWD */
    Relay1_red = relay1_rev;      /* LED is ON when Relay 1 is REV */
    Relay2_green = relay2_fwd;    /* LED is ON when Relay 2 is FWD */
    Relay2_red = relay2_rev;      /* LED is ON when Relay 2 is REV */

    Switch1_LED = !(int)rc_dig_in01;
    Switch2_LED = !(int)rc_dig_in02;
    Switch3_LED = !(int)rc_dig_in03;
    
  } /* (user_display_mode = 0) (User Mode is Off) */
  
  else  /* User Mode is On - displays data in OI 4-digit display*/
  {
    User_Mode_byte = backup_voltage*10; /* so that decimal doesn't get truncated. */
  }   
  }
}
}