// @(#) $Header: /usr/local/scalawags_cvs/Scalawags/Frc2006/user_routines_new.c,v 1.26 2006/03/09 21:25:45 abrown Exp $
/****************************************************************************
* FILE NAME: user_routines_alt.c
*
* DESCRIPTION:
* This file contains user written code to control the robot.
*
* 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 "printf_lib.h"
#include <stdio.h>
#include "p18f8722.h"
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines_alt.h"
#include "user_SerialDrv.h"
#include "supportfuncs.h"
#include "ifibyte.h"
#include "itrig.h"
#include "quadrature.h"
#include "turn_nav.h"
#include "navigate.h"
#include "go2coord.h"
#if defined (FRC_COPROCESSOR) || defined (FRC_NOCOPROCESSOR)
#include <usart.h>
#define NEAR near
#define ROM rom
#define SHORTLONG short long
#define INLINE
#else
#include "usart.h"
#define NEAR
#define ROM
#define SHORTLONG short
#define INLINE inline
#endif
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
#include "msgxfer.h"
#endif
// DEFINE USER VARIABLES AND INITIALIZE THEM HERE
#if _USE_CMU_CAMERA
#include "user_camera.h"
#include "more_camera.h"
#endif // _USE_CMU_CAMERA
#define ROBOT2006 1
unsigned char olddata = 0;
// unsigned char percent_expo = 50;
int transferchar = 0;
char enabled=0;
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
msgxfer_state instate;
msgxfer_state outstate;
unsigned char buffercount = 0;
void WrongLengthMsg(void);
// Define buffers for communication with coprocessor
char input_msg_buffer[10];
msgxfer_state input_msg_state;
char output_msg_buffer[10];
msgxfer_state output_msg_state;
#endif
int delayed = 0;
int16 autonomous_action_state = 254;
static unsigned char autonomous_switches;
int16 stopped = 0;
int derror = 0;
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
// If we are on Linux, then we need to talk to a file descriptor.
int msgxfer_fd; // file descriptor of simulated port to coprocessor
#endif
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
// Define serial data input buffer
#define SD_SIZE 8 // Number of characters allowed in buffer
char sd_buffer[SD_SIZE+1]; // Small input buffer for serial port chars
char sd_input = 0;
char sd_output = 0;
#endif
/*****************************************************************************
* 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;
digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
digital_io_18 = INPUT; // Used for pneumatic pressure switch.
// SECOND: Set up the I/O pins you want to use as digital OUTPUTS.
digital_io_17 = OUTPUT; // Example - Not used in Default Code.
// THIRD: Initialize the values on the digital outputs.
rc_dig_out17 = 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(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.
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
msgxfer_init_read(&instate, msg_reader);
msgxfer_init_write(&outstate, msg_writer);
#endif
#if ROBOT2006
quad_left = quadrature_new( &PORTBbits.full8bits, 2, &PORTBbits.full8bits, 3, 50*INCH/100 );
quad_right = quadrature_new( &PORTBbits.full8bits, 4, &PORTBbits.full8bits, 5, 50*INCH/100 );
navigate = navigate_new( 20*INCH );
go2coord = go2coord_new( &leftspeed, &rightspeed );
#else
#define QUADCAL 10000
quad_left = quadrature_new( &PORTCbits.full8bits, 0, &PORTJbits.full8bits, 3, QUADCAL );
quad_right = quadrature_new( &PORTJbits.full8bits, 5, &PORTJbits.full8bits, 4, QUADCAL );
#endif
rightspeed = 0;
leftspeed = 0;
//Serial_Driver_Initialize();
#if _USE_CMU_CAMERA
camera_state_init();
#endif
User_Proc_Is_Ready(); // DO NOT CHANGE! - last line of User_Initialization
} // User_Initialization()
/*****************************************************************************
* FUNCTION NAME: autonomous_init(void)
* PURPOSE: This routine is called from main each time the mode
* first becomes autonomous. On the robot this generally
* only happens once when we start up in auto mode.
* You may modify and add to this function.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
void autonomous_init(void)
{
autonomous_switches = 0;
#if ROBOT2006
if( rc_dig_in15 ) autonomous_switches |= 1;
if( rc_dig_in16 ) autonomous_switches |= 2;
if( rc_dig_in17 ) autonomous_switches |= 4;
if( rc_dig_in18 ) autonomous_switches |= 8;
#else
if( rc_dig_in04 ) autonomous_switches |= 1;
if( rc_dig_in05 ) autonomous_switches |= 2;
if( rc_dig_in06 ) autonomous_switches |= 4;
if( rc_dig_in07 ) autonomous_switches |= 8;
#endif
// pwm07 = (unsigned char)autonomous_switches;
printf("NOTE autonomous_switches=%d\n", autonomous_switches);
switch( autonomous_switches )
{
case 0x0000: // Do nothing
autonomous_action_state = 0;
break;
case 0x0001: //
autonomous_action_state = 20;
break;
case 0x0002: //
autonomous_action_state = 40;
break;
case 0x0003: // Navigation Way Points
autonomous_action_state = 60;
//autonomous_action_state = 80;
//autonomous_action_state = 100;
break;
default: // Do nothing
autonomous_action_state = 255;
break;
}
rightspeed = 0;
leftspeed = 0;
//pwm04 = autonomous_switches;
return;
} // autonomous_init()
/****************************************************************************
* JoystickRegular
* Normal joystick to motor mapping.
*/
INLINE void JoystickRegular(int16 joyx, int16 joyy, int16 *left, int16 *right)
{
*left = joyy - joyx;
*right = joyy + joyx;
} // JoystickRegular(void)
/*
* JoystickTank
* Tank joystick to motor mapping.
*/
INLINE void JoystickTank(int16 joyleft, int16 joyright, int16 *left, int16 *right)
{
*left = joyleft;
*right = joyright;
} // JoystickTank(void)
/*****************************************************************************
* FUNCTION NAME: User_Slow_Pre
* PURPOSE: This routine is called from main in the slow loop.
* It is called first, before deciding if we are in manual
* or auto mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in both manual or auto mode.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Slow_Pre (void)
{
int16 quad_error;
quad_error = quadrature_poll(quad_left);
if( quad_error )
{
reportstart(__FILE__,__LINE__,ERROR,"quad_error(quad_left)=");
reportint(quad_error);
reportend(".");
}
quad_error = quadrature_poll(quad_right);
if( quad_error )
{
reportstart(__FILE__,__LINE__,ERROR,"quad_error(quad_right)=");
reportint(quad_error);
reportend(".");
}
pwm01 = (unsigned char) quadrature_distance(quad_left);
pwm02 = (unsigned char) quadrature_distance(quad_right);
pwm05 = quadrature_error(quad_left);
pwm06 = quadrature_error(quad_right);
#if 0
reportstart(__FILE__,__LINE__,NOTE,"distance=");
reportint(quadrature_distance(quad_left));
reporttext(",");
reportint(quadrature_distance(quad_right));
reporttext(", portbb=");
reportint(PORTBbits.full8bits);
reportend("");
#endif
} // User_Slow_Pre()
/*****************************************************************************
* FUNCTION NAME: User_Slow_Manual
* PURPOSE: This routine is called from main in the slow loop.
* It is called only if we are in manual mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in only manual mode and in the slow loop.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Slow_Manual (void)
{
int16 joy1x, joy1y, joy2y;
joy1x = ifibyte2int(p1_x,IFI_NORM);
joy1y = ifibyte2int(p1_y,IFI_NORM);
joy2y = ifibyte2int(p2_y,IFI_NORM);
if( p3_sw_trig )
{
JoystickRegular(joy1x,joy1y,&leftspeed,&rightspeed);
}
else
{
JoystickTank(joy1y,joy2y,&leftspeed,&rightspeed);
}
// leftspeed /= 2;
// rightspeed /= 2;
if (rightspeed < 5 && rightspeed > -5)
{
rightspeed = 0;
}
if (leftspeed < 5 && leftspeed > -5)
{
leftspeed = 0;
}
} // User_Slow_Manual()
/*****************************************************************************
* FUNCTION NAME: User_Slow_Auto
* PURPOSE: This routine is called from main in the slow loop.
* It is called only if we are in auto mode.
* You may modify and add to this function.
* his function can contain code that will need to be
* executed in only auto mode and in the slow loop.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Slow_Auto (void)
{
int32 leftstart=0, rightstart=0;
int32 left_curr, right_curr;
int32 x_curr, y_curr, heading_curr=0;
// int32 lefttarget, righttarget;
switch (autonomous_action_state)
{
// Auto Task Cal distance =============================================
case 0: // Go forward uncontrolled for a distance of 100.
leftspeed = rightspeed = 80;
if( quadrature_distance(quad_left) > 100*INCH )
{
autonomous_action_state ++;
}
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
break;
case 1: // Stop
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
break;
// Auto Task Cal turn ==================================================
case 20: // Pirouette
leftspeed = 80;
rightspeed = -80;
if( quadrature_distance(quad_left) > 100*INCH )
{
autonomous_action_state ++;
}
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
break;
case 21: // Stop
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
break;
// Auto Task Cal turn ==================================================
case 40: // Curve
leftstart = quadrature_distance(quad_left);
rightstart = quadrature_distance(quad_right);
autonomous_action_state ++;
// No break, so fallthru to next case.
case 41: // Continue or stop
(void) turn_nav( leftstart, quadrature_distance(quad_left), &leftspeed,
rightstart, quadrature_distance(quad_right), &rightspeed,
125, 0, LEFT, 20 );
if( quadrature_distance(quad_left)+quadrature_distance(quad_right)
> 200*INCH )
{
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
}
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
break;
// Navigation Way Points ============================================
case 60: // Setup for point 1.
leftstart = quadrature_distance(quad_left);
rightstart = quadrature_distance(quad_right);
navigate_init( navigate, leftstart, rightstart,
269*FOOT/10, 26*FOOT, 0*DEGREE);
go2coord_target( go2coord, 25*FOOT, 30*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
// No break, so fallthru to next case.
case 61: // Go to way point 1.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 2.
go2coord_target( go2coord, 10*FOOT, 20*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 62: // Go to way point 2.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 3.
go2coord_target( go2coord, 5*FOOT, 39*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 63: // Go to way point 3.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 4.
go2coord_target( go2coord, 5*FOOT, 5*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 64: // Go to way point 4 and then stop.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Stop.
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
}
break;
// Navigation Way Points ============================================
case 80: // Setup for point 10.
leftstart = quadrature_distance(quad_left);
rightstart = quadrature_distance(quad_right);
navigate_init( navigate, leftstart, rightstart,
269*FOOT/10, 26*FOOT, 0*DEGREE);
go2coord_target( go2coord, 20*FOOT, 35*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
// No break, so fallthru to next case.
case 81: // Go to way point 10.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 11.
go2coord_target( go2coord, 19*FOOT, 25*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 82: // Go to way point 11.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 12.
go2coord_target( go2coord, 20*FOOT, 15*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 83: // Go to way point 12.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 13.
go2coord_target( go2coord, 20*FOOT, 5*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 84: // Go to way point 13.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 12 again.
go2coord_target( go2coord, 20*FOOT, 15*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 85: // Go back to way point 12 and then stop.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Stop.
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
}
break;
// Navigation Way Points ============================================
case 100: // Setup for point 13.
leftstart = quadrature_distance(quad_left);
rightstart = quadrature_distance(quad_right);
navigate_init( navigate, leftstart, rightstart,
269*FOOT/10, 26*FOOT, 0*DEGREE);
go2coord_target( go2coord, 20*FOOT, 5*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
// No break, so fallthru to next case.
case 101: // Go to way point 13.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 12.
go2coord_target( go2coord, 20*FOOT, 15*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 102: // Go to way point 12.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 11.
go2coord_target( go2coord, 19*FOOT, 25*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 103: // Go to way point 11.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 10.
go2coord_target( go2coord, 20*FOOT, 35*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 104: // Go to way point 10.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Set up way point 11 again.
go2coord_target( go2coord, 19*FOOT, 25*FOOT,
6*INCH, 3*DEGREE, 125, -15, -125);
autonomous_action_state ++;
}
break;
case 105: // Go back to way point 11 and then stop.
left_curr = quadrature_distance(quad_left);
right_curr = quadrature_distance(quad_right);
navigate_poll(navigate, left_curr, right_curr);
navigate_xy( navigate, &x_curr, &y_curr );
heading_curr = navigate_heading( navigate );
if( GO2DONE == go2coord_update(go2coord, x_curr, y_curr, heading_curr) )
{
// Stop.
leftspeed = rightspeed = 0;
autonomous_action_state = 255;
}
break;
case 255: // ======== STOP and sit quietly =================
case 255+256:
leftspeed = rightspeed = stopped;
if( autonomous_action_state == 255 )
{
reportstart(__FILE__,__LINE__,NOTE,"autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
autonomous_action_state += 256;
}
break;
default: // ========== This should never happen ============
reportstart(__FILE__,__LINE__,ERROR,"Internal error. autonomous_action_state=");
reportint(autonomous_action_state);
reportend(".");
leftspeed = rightspeed = stopped;
break;
} // switch( autonomous_action_state )
pwm03 = (unsigned char) autonomous_action_state;
pwm04 = (unsigned char) (heading_curr/DEGREE)+0;
} // User_Slow_Auto()
/*****************************************************************************
* FUNCTION NAME: User_Slow_Post
* PURPOSE: This routine is called from main in the slow loop.
* It is called last, after running the code for explicitly
* manual or auto mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in both manual or auto mode.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Slow_Post (void)
{
// pwm's 9 and 10 are left side. pwm's 11 and 12 are right side.
#if ROBOT2006
pwm16 = int2ifibyte(leftspeed,IFI_INV);
pwm15 = int2ifibyte(rightspeed,IFI_NORM);
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
#else
pwm09 = pwm10 = int2ifibyte(leftspeed,IFI_INV);
pwm11 = pwm12 = int2ifibyte(rightspeed,IFI_NORM);
#endif
} // User_Slow_Post()
/*****************************************************************************
* FUNCTION NAME: User_Fast_Pre
* PURPOSE: This routine is called from main in the fast loop.
* It is called first, before deciding if we are in manual
* or auto mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in both manual or auto mode.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Fast_Pre (void)
{
int16 quad_error;
quad_error = quadrature_poll(quad_left);
if( quad_error )
{
reportstart(__FILE__,__LINE__,ERROR,"quad_error(quad_left)=");
reportint(quad_error);
reportend(".");
}
quad_error = quadrature_poll(quad_right);
if( quad_error )
{
reportstart(__FILE__,__LINE__,ERROR,"quad_error(quad_right)=");
reportint(quad_error);
reportend(".");
}
pwm01 = (unsigned char) quadrature_distance(quad_left);
pwm02 = (unsigned char) quadrature_distance(quad_right);
pwm05 = quadrature_error(quad_left);
pwm06 = quadrature_error(quad_right);
#if 0
reportstart(__FILE__,__LINE__,NOTE,"distance=");
reportint(quadrature_distance(quad_left));
reporttext(",");
reportint(quadrature_distance(quad_right));
reporttext(", portbb=");
reportint(PORTBbits.full8bits);
reportend("");
#endif
} // User_Fast_Pre()
/*****************************************************************************
* FUNCTION NAME: User_Fast_Manual
* PURPOSE: This routine is called from main in the fast loop.
* It is called only if we are in manual mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in only manual mode and in the fast loop.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Fast_Manual (void)
{
} // User_Fast_Manual()
/*****************************************************************************
* FUNCTION NAME: User_Fast_Auto
* PURPOSE: This routine is called from main in the fast loop.
* It is called only if we are in auto mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in only auto mode and in the fast loop.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Fast_Auto (void)
{
} // User_Fast_Auto()
/*****************************************************************************
* FUNCTION NAME: User_Fast_Post
* PURPOSE: This routine is called from main in the fast loop.
* It is called last, after running the code for explicitly
* manual or auto mode.
* You may modify and add to this function.
* This function can contain code that will need to be
* executed in both manual or auto mode.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
INLINE void User_Fast_Post (void)
{
} // User_Fast_Post()
/*****************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE: Low priority interrupt vector
* CALLED FROM: nowhere by default
* ARGUMENTS: none
* RETURNS: void
* DO NOT MODIFY OR DELETE THIS FUNCTION
*****************************************************************************
*/
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
void InterruptVectorLow (void)
{
}
#endif
#if defined(FRC_NOCOPROCESSOR) || defined(FRC_COPROCESSOR)
# pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
_asm
goto InterruptHandlerLow /*jump to interrupt routine*/
_endasm
}
#endif
/*****************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE: Low priority interrupt handler
* If you want to use these external low priority interrupts or any
* of the peripheral interrupts then you must enable them in your
* initialization routine. Innovation First, Inc. will not provide
* support for using these interrupts, so be careful. There is great
* potential for glitchy code if good interrupt programming practices
* are not followed. Especially read p. 28 of the "MPLAB(R) C18 C
* Compiler User's Guide" for information on context saving.
* CALLED FROM: this file, InterruptVectorLow routine
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
#if defined(FRC_NOCOPROCESSOR) || defined(FRC_COPROCESSOR)
# pragma code
# pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */
#endif
void InterruptHandlerLow (void)
{
unsigned char int_byte;
if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) // The INT2 pin is RB2/DIG I/O 1.
{
INTCON3bits.INT2IF = 0;
}
if (INTCON3bits.INT3IF && INTCON3bits.INT3IE) // The INT3 pin is RB3/DIG I/O 2.
{
INTCON3bits.INT3IF = 0;
}
if (INTCONbits.RBIF && INTCONbits.RBIE) // DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed.
{
int_byte = PORTB; // You must read or write to PORTB
INTCONbits.RBIF = 0; // and clear the interrupt flag
} // to clear the interrupt condition.
else
{
CheckUartInts(); // For Dynamic Debug Tool or buffered printf features.
}
} // InterruptHandlerLow (void)
/*****************************************************************************
* FUNCTION NAME: Serial_Char_Callback
* PURPOSE: Interrupt handler for the TTL_PORT.
* CALLED FROM: user_SerialDrv.c
* ARGUMENTS:
* Argument Type IO Description
* -------- ---- -- -----------
* data unsigned char I Data received from the TTL_PORT
* RETURNS: void
*****************************************************************************
*/
void Serial_Char_Callback(unsigned char data)
{
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
// Put the character into a ring buffer for receipt by the Serial_Read_Bufr routine
char nextIn = sd_input + 1;
if (nextIn >= sizeof(sd_buffer))
nextIn = 0; // Rewind the input to beginning
if (nextIn != sd_output)
{
// Room for character - store it
sd_buffer[sd_input] = data;
sd_input = nextIn;
}
else
{
// No room: drop character
}
#endif
// Add code to handle incomming data (remember, interrupts are still active)
#if _USE_CMU_CAMERA
/*****************************************************************************
* This is the section of the UART rx interrupt routine that handle the
* camera buffer. There are two different modes parse_mode=0 and parse_mode=1
*
* parse_mode=0
* is for reading ACKs back from the camera. It will buffer an entire
* line until it reads a '\r' character. It should be used for
* non-streaming data from the camera.
*
* parse_mode=1
* is for reading tracking packets that are streaming from the camera. It
* assumes the camera is in raw mode and will read until it gets a 'T'
* character. It then buffers until it gets a 255 bytes which in raw mode
* signifies the end of line.
*
* cam_index_ptr - is the counter that keeps track of the current location
* in the buffer.
*
* data_rdy - is a flag that remains 0 until the entire data packet is
* ready at which point it becomes 1. Once data_rdy is 1, new packets
* will be ignored.
*****************************************************************************/
if (data_rdy==0)
{
if (parse_mode==0) // Grab single line packets, such as ACKS or NCKS
{
if (data=='\r' || cam_index_ptr==MAX_BUF_SIZE)
{
// Once the end of a packet is found, assert data_rdy and wait
cam_uart_buffer[cam_index_ptr]=0;
data_rdy=1;
}
else
{
cam_uart_buffer[cam_index_ptr]=data;
cam_index_ptr++;
}
}
if (parse_mode==1) // Grab streaming packets
{
if (data=='T' )
{
++User_Mode_byte;
data_rdy=0;
cam_uart_buffer[0]=data;
cam_index_ptr=1;
return;
}
if (cam_index_ptr>0) // Added this to potentially stop unnecessary delays
{
if (data==255 || cam_index_ptr==MAX_BUF_SIZE)
{
// if (cam_index_ptr==0 )return;
// Once the end of a packet is found, assert data_rdy and wait
cam_uart_buffer[cam_index_ptr]=0;
data_rdy=1;
}
else
{
cam_uart_buffer[cam_index_ptr]=data;
cam_index_ptr++;
}
}
}
}
#endif // _USE_CMU_CAMERA
} // Serial_Char_Callback(unsigned char data)
/****************************************************************************
* $Log: user_routines_new.c,v $
* Revision 1.26 2006/03/09 21:25:45 abrown
* Adjust the quadrature distance from 0.9 inch to 0.5 inch.
*
* Revision 1.25 2006/03/06 20:50:32 abrown
* Only report autonomous_action_state=255 at the start of that state.
*
* Revision 1.24 2006/03/06 20:23:05 abrown
* Fix a log line. Remove other log lines.
*
* Revision 1.23 2006/03/06 19:30:03 abrown
* Add two autonomous tasks in order to test the navigation algorithm.
*
* Revision 1.22 2006/03/05 20:50:59 abrown
* Use varsizes.h. Reduce INCH resolution to see if we get fewer
* overflows. Add FOOT defn and use it in our new autonomous tasks
* that use go2coord and navigate. Use varsizes.h.
*
* Revision 1.21 2006/02/18 03:50:06 abrown
* Oops, cannot use printf_lib.h. Switch back to stdio.h.
*
* Revision 1.20 2006/02/18 03:47:20 abrown
* For the ROBOT2006 conditional, use value rather than defined().
*
* Revision 1.19 2006/02/17 20:24:02 abrown
* Clean up whitespace.
*
* Revision 1.18 2006/02/17 20:15:08 abrown
* Change almost all comments to the // type, making it easier to comment
* out blocks of code. If we are using MPLab, include printf_lib.h
* rather than stdio.h. Don't initialize pwms in User_Initialization(),
* this is handed in main(). But do initialize rightspeed and leftspeed
* in User_Initialization() and autonomous_init().
*
* Revision 1.17 2006/02/12 16:52:43 abrown
* Define inches and use that in quadrature_distance() comparisons.
*
* Revision 1.16 2006/02/12 01:53:11 abrown
* Need Generate_Pwms() because we are using pwm13-pwm16.
*
* Revision 1.15 2006/02/11 21:46:43 abrown
* Put in conditionals for 2006 code.
*
* Revision 1.14 2006/02/08 18:58:19 abrown
* Move the "do not change" code from user_routines_new.c to main.c.
* Remove some scruff and improve formatting.
*
* Revision 1.13 2006/02/08 04:42:47 abrown
* MPLab doesn't support inline keyword.
*
* Revision 1.12 2006/02/05 18:48:59 abrown
* Make functions inline.
*
* Revision 1.11 2006/02/05 02:14:17 abrown
* Move the second auto task to 40 so we have a consistent 20
* digits between tasks. Also wire it into the auto switches.
* State 40 was missing the autonomous_action_state++. In
* turn_nav, only use 0 for the slow speed. Put autonomous_action_state
* out to pwm03 so we can watch it. Increase our target distance in
* task 40 to match the calibration value we are using: 0.1 inch per count.
*
* Revision 1.10 2006/02/04 23:22:34 abrown
* Don't call quadrature_pulsecount(), only quadrature_distance().
*
* Revision 1.9 2006/02/04 22:00:54 abrown
* Even tho there is no malloc in MPLab I am able to simulate it in
* quadrature_new() using a static array. This eliminates
* quadrature_new_1() and quadrature_new_2(), which were ugly warts.
*
* Revision 1.8 2006/02/04 19:26:45 abrown
* Rewrite joystick code.
*
* Revision 1.7 2006/02/04 05:35:42 abrown
* One of the FRC channels is dead. Move quadrature sensors to good ones.
*
* Revision 1.6 2006/02/04 00:27:46 abrown
* Optimization of the task using turn_nav().
*
* Revision 1.5 2006/02/03 22:23:39 abrown
* Make an autonomous task that uses turn_nav().
*
* Revision 1.4 2006/02/02 04:03:36 abrown
* Paste in Serial_Char_Callback defn for MPLab's sake.
*
* Revision 1.3 2006/02/02 03:44:12 abrown
* Map quadrature to the right sense lines. Create simple autonomous task.
*
* Revision 1.2 2006/02/01 01:28:18 abrown
* Add quadrature_poll() calls to the slow loop. Improve error checking
* of quad stuff.
*
* Revision 1.1 2006/02/01 00:46:37 abrown
* Created a new structure to replace user_routines.c and user_routines_fast.c.
*/
syntax highlighted by Code2HTML, v. 0.9.1