Omnibot Code

#define _VEX_BOARD
#include "API.h"
#include "BuiltIns.h"
#include "stdio.h"
/*  
	Fire fight robot 
	Omni drive
	Last update - 13 November 2007
	Rebuilt on 6 November 2008
*/
 
#define sonar 11
#define front 1
#define right 2
#define left  3
#define back  4
#define sensorservo 7
#define co2servo 8
 
void IO_Initialization(void) 
{
 
	SetCompetitionMode(0,240); // Autonomous mode is 10 seconds, driver time is 240 seconds
	SetDirection(sonar,0);
	PrintToScreen("IO_Initialization Complete\n\r");
 
}
 
void Initialize(void) // When the robot is turned on this gets done first 
{
	PrintToScreen("Version 1 - FS\n\r");
	Set_Number_of_Analog_Channels(4);
}
 
void Autonomous(void) // Autonomous code
{
#define speed 120	
#define offset 20
 
} // End of Autonomous code
 
void OperatorControl(void) // Driver control code
{
 
int LF; // Left Front
int RF; // Right Front
int LR; // Left Rear
int RR; // Right Rear
int leftx;
int lefty;
int rightx;
int spin;
      while ( 1 )
      {
            // Get Data 
            leftx = GetRxInput ( 1 , 4 ) ; // Left Joystick, X Axis
            lefty = GetRxInput ( 1 , 3 ) ; // Left Joystick, Y Axis
            rightx = GetRxInput ( 1 , 1 ) ; // Right Joystick, X Axis
            // Half the input signal (so code does not overflow past 255)
            leftx = leftx / 2 ;
            lefty = lefty / 2 ;
            spin = rightx / 2 ;
            // Drive Code Algorithim
            LF = RR = lefty - leftx + 127 ;
            RF = LR = 255 - lefty - leftx ;
            RR = 255 - RR ; // Reverse Direction of RR motor
            LR = 255 - LR ; // Reverse Direction of LR motor
            // Spin Code Algorithim
            RF = RF - spin + 63 ;
            RR = RR - spin + 63 ;
            LF = LF - spin + 63 ;
            LR = LR - spin + 63 ;
            // Code overflow prevention
            if ( LF < 0 )
            {
                  LF = 0 ;
            }
            else if ( LF > 255 )
            {
                  LF = 255 ;
            }
            if ( RF < 0 )
            {
                  RF = 0 ;
            }
            else if ( RF > 255 )
            {
                  RF = 255 ;
            }
            if ( RR < 0 )
            {
                  RR = 0 ;
            }
            else if ( RR > 255 )
            {
                  RR = 255 ;
            }
            if ( LR < 0 )
            {
                  LR = 0 ;
            }
            else if ( LR > 255 )
            {
                  LR = 255 ;
            }
            // Set Motors
            SetPWM ( 1 , RF ) ;
            SetPWM ( 2 , LF ) ;
            SetPWM ( 3 , LR ) ;
            SetPWM ( 4 , RR ) ;
      }
 
} // End of Driver code 
 
/*
void OperatorControl(void) // Driver control code
{
unsigned char forward;
unsigned char side;
unsigned char spin;
unsigned char search;
unsigned char co2;
printf('In Operator Mode\r');
while (1 == 1)
	{
		forward = GetRxInput ( 1 , 2 ) ; // Right Joystick, X Axis
        side    = GetRxInput ( 1 , 1 ) ; // Right Joystick, Y Axis
        spin    = GetRxInput ( 1 , 4 ) ; // Left Joystick, X Axis
 
		SetPWM (right, forward);
		SetPWM (left, 255 - forward);
		SetPWM (front, side);
		SetPWM (back, 255-side);
 
		search = GetRxInput(1,5);  // Back button
		SetPWM (sensorservo,search);
 
		co2 = GetRxInput(1,6);  // back button
		SetPWM (co2servo, co2); 
	}
}
*/
 void main(void)
{
	while (1) 
	{
		PrintToScreen("This is the main loop\n\r");
                OperatorControl();
		Wait(2000);
	}
}