View source

The Twitch software was written for the VEX controller.  It uses a combination of MPLabs C and the WPILibrary.  

<source lang="c">

#define _VEX_BOARD
#include "API.h"
#include "BuiltIns.h"
/*
Twich Robot Wheel and Orientation

   1 -- N -- 2 
   |         |
   W         E
   |         |
   3 -- S -- 4

From the drawing, when North is the front, then the right wheels are 2,4.  
When West is the front, then the right wheels are 1,2.  

When we built the robot we had colored tape (red, yellow, green, white) 
on the bumpers and corresponding tape on the joystick.  

The driver taps the joystick to the color they want to be front, if needed 
the robot "twitches" and the direction of the front moves. 

Team 1640 -- Foster Schucker -- July 2008 -- Initial version

*/
#define MOTOR_1 1
#define MOTOR_2 2
#define MOTOR_3 3
#define MOTOR_4 4

#define TWITCH_MOTOR_1 6
#define TWITCH_MOTOR_2 7
#define LIMIT_RELEASED  1   
#define LIMIT_PRESSED   0  
#define FRONT_LIMIT 11  
#define BACK_LIMIT  10  
unsigned char front_limit_val, back_limit_val;

unsigned char twitch_state;
#define NORTHSOUTH 1 
#define EASTWEST 2
#define N_TRIGGER 60 
// needs to be 0 - 127
#define E_TRIGGER 200
// needs to be 127 - 255
#define S_TRIGGER 200
// needs to be 127 - 255
#define W_TRIGGER 60
// needs to be 0 - 127

unsigned char front;
#define NORTH 1
#define EAST 2
#define SOUTH 3
#define WEST 4

unsigned char t_ns,t_ew;  // We put the values in here from the transmitter

unsigned char m_right, m_left, m_twitch;

void IO_Initialization(void) 
{
	PrintToScreen("IO_Initialization\n\r");
	SetCompetitionMode(0,1100);
}

void Initialize(void) // When the robot is turned on this gets done first 
{
	PrintToScreen("Twitch 1.0 - Team 1640\n\r");
} // End of Initialize code

void Autonomous(void) // Autonomous code
{
		PrintToScreen("Auton\n\r");
} // End of Autonomous code

void OperatorControl(void) // Driver control code
{ 
	PrintToScreen("OperatorControl\n\r");

} // End of Driver code 

void StopMotor(void)
{
	// stops all the motors while we twitch
	SetPWM(MOTOR_1,127);
	SetPWM(MOTOR_2,127);
	SetPWM(MOTOR_3,127);
	SetPWM(MOTOR_4,127);
} // End of StopMotor

void CheckTwitch(void) // Check the twitch positions and if needed change state
{
	t_ns = GetRxInput(1,3);
	t_ew = GetRxInput(1,4);

	// PrintToScreen("NS = %d, EW= %d ",(int)t_ns, (int)t_ew);
	if (t_ns < N_TRIGGER)
	{
		front = NORTH;
		twitch_state = NORTHSOUTH;
	} else if (t_ew > E_TRIGGER)
	{
		front = EAST;
		twitch_state = EASTWEST;
	}else if (t_ns > S_TRIGGER)
	{
		front = SOUTH;
		twitch_state = NORTHSOUTH;
	} else if (t_ew < W_TRIGGER)
	{
		front = WEST;
		twitch_state = EASTWEST;
	}


	front_limit_val = GetDigitalInput ( FRONT_LIMIT ) ; 
               // Recieves input for the front limit switch
        back_limit_val = GetDigitalInput ( BACK_LIMIT ) ; 
              // Recieves input for the back limit switch
	
        // Work out if we need to twitch and then move
	if ( ( front_limit_val != LIMIT_PRESSED ) && ( twitch_state == NORTHSOUTH ) )
	{
		StopMotor();
		while (front_limit_val != LIMIT_PRESSED )
		{ 
			PrintToScreen("Twitch 255, state = %d", (int)twitch_state); 
			front_limit_val = GetDigitalInput ( FRONT_LIMIT ) ; 
                            // Recieves input for the front limit switch
			SetPWM(TWITCH_MOTOR_1, 255);
			SetPWM(TWITCH_MOTOR_2, 0);
		}
	}
	if ( ( back_limit_val != LIMIT_PRESSED ) && ( twitch_state == EASTWEST ) )
	{
		StopMotor();
		while (back_limit_val != LIMIT_PRESSED)
		{ 
			PrintToScreen("Twitch 0");
			back_limit_val = GetDigitalInput ( BACK_LIMIT ) ; 
                           // Recieves input for the back limit switch
			SetPWM(TWITCH_MOTOR_1, 0);
			SetPWM(TWITCH_MOTOR_2, 255);
		}
	}
	SetPWM(TWITCH_MOTOR_1,127);  // turn off the twitch motors 
	SetPWM(TWITCH_MOTOR_2,127);
        // End of Twitch action
	
        // Diagnostic code to make sure we Twitched correctly
	PrintToScreen("fl = %d , bl = %d state = %d, front = %d",
            (int)front_limit_val,(int)back_limit_val, (int)twitch_state, (int)front); 
} // End of CheckTwitch

void x_DriveRobot(void)
/* This is our custom drive routine for arcade mode.  
   It was written as an adjunct to the routinge below that uses the WPILibrary Arcade mode.
*/
{
	unsigned char m_left, m_right;
	unsigned char stick_x, stick_y;

	// Inputs are the X and Y stick
        stick_x = GetRxInput(1,1);
		stick_y = GetRxInput(1,2);
		m_left = (int) stick_y + ((int) stick_x - 127);
		m_right = (int) stick_y - ((int) stick_x - 127);

		// need to get the value back in the range 0..254

		if (m_left < 0) {
			m_left = 0;
		}

		if (m_left > 254) {
			m_left = 254;
		}

		if (m_right < 0) {
			m_right = 0;
		}   
		if (m_right > 254) {
				m_right = 254;
		}
		switch (front) //Front has the "front" of the robot, which way to go
				{
			case NORTH: { // North
				SetPWM(MOTOR_2,m_right); // right side motors are 2 and 4
				SetPWM(MOTOR_4,m_right);
				SetPWM(MOTOR_1,m_left); // left side motors are 1 and 3
				SetPWM(MOTOR_3,m_left);
				PrintToScreen("North ");
				break;
            }
			case EAST: { // East
				SetPWM(MOTOR_4,m_right); 
				SetPWM(MOTOR_3,m_right);
				SetPWM(MOTOR_2,m_left); 
				SetPWM(MOTOR_1,m_left);
				PrintToScreen("East ");
				break;
            }
			case SOUTH: { // South
				SetPWM(MOTOR_3,m_right); 
				SetPWM(MOTOR_1,m_right);
				SetPWM(MOTOR_4,m_left); 
				SetPWM(MOTOR_2,m_left);
				PrintToScreen("South ");
				break;
            }
			case WEST: { // West
				SetPWM(MOTOR_1,m_right); 
				SetPWM(MOTOR_2,m_right);
				SetPWM(MOTOR_3,m_left); 
				SetPWM(MOTOR_4,m_left);
				PrintToScreen("West ");
				break;
            }
        }

} // End of x_DriveRobot

void DriveRobot(void)
//  This makes the robot move. We cheat and use the WPILibrary calls
//  rather than doing the hard work of doing the motors
/*
This is the prototype function for the 4 wheel drive arcade. 

void Arcade4(unsigned char port,  // Which transmitter 1 or 2
            unsigned char ucMoveChannel, unsigned char ucRotateChannel, // which joystick channel 
            unsigned char ucLeftfrontPWM, unsigned char ucRightfrontPWM,
            unsigned char ucLeftrearPWM, unsigned char ucRightrearPWM,
            unsigned char ucLeftfrontInvert, unsigned char ucRightfrontInvert,
            unsigned char ucLeftrearInvert, unsigned char ucRightrearInvert);
*/
{
switch (front) //Front has the "front" of the robot, which way to go
{
   case NORTH:{ // North
	Arcade4(1,
            2, 1,
            MOTOR_1, MOTOR_2,
            MOTOR_3, MOTOR_4,
            1, 1,
            1, 1);
		PrintToScreen("North ");
		break;
	}
   case EAST:{ // East
	Arcade4(1,
            2, 1,
            MOTOR_2, MOTOR_4,
            MOTOR_1, MOTOR_3,
            1, 1,
            1, 1);
		PrintToScreen("East ");
		break;
	}
   case SOUTH:{ // South
	Arcade4(1,
            2, 1,
            MOTOR_4, MOTOR_3,
            MOTOR_2, MOTOR_1,
            1, 1,
            1, 1);
		PrintToScreen("South ");
		break;
	}
   case WEST:{ // West
	Arcade4(1,
            2, 1,
            MOTOR_3, MOTOR_1,
            MOTOR_4, MOTOR_2,
            1, 1,
            1, 1);
		PrintToScreen("West ");
		break;
	}
    }
} // End of DriveRobot

void main(void)
{

	PrintToScreen("Twitch\n\r");     
        front = NORTH; // set direction
	twitch_state = NORTHSOUTH; // set intial twitch state
	// The robot will move to this state when it gets turned on
	// Issue is one match to another, what should the twitch state be?
        // Future code should have a jumper to check
    while (1){
	CheckTwitch();
	DriveRobot();
	PrintToScreen("\n\r");
    }
}
</source>