View source

From STEMRobotics

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>

Return to Twitch Software.