Twitch Software
From DEW Robotics
Revision as of 11:02, 15 June 2009 by Foster (talk | contribs) (Created page with '<BIG> Do not edit this page. It is waiting for Mike Rizzo to add the source code printer extension. When it's installed it should display correctly in lots of pretty colors. Th...')
Do not edit this page. It is waiting for Mike Rizzo to add the source code printer extension. When it's installed it should display correctly in lots of pretty colors. Thanks
The Twitch software was written for the VEX controller. It uses a combination of MPLabs C and the WPILibrary.
#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");
}
}