robotBaseWorking.c

#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(Sensor, in8,    gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  rightDriveEnc,  sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftDriveEnc,   sensorQuadEncoder)
#pragma config(Motor,  port1,           motorA,        tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           rightDrive,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           leftDrive,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          motorB,        tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
//VEX stuff

#pragma competitionControl(Competition)
#pragma autonomousDuration(60)
#pragma userControlDuration(60)

#include "Vex_Competition_Includes.c"

//Define static values
#define PI 3.14159265358979323846264338327
#define DEADZONE  10
#define speed 127

typedef struct robot
{
  /* Struct to hold robots x, y, and theta relative to a starting position */
  float currX, currY;
  float prevX, prevY;
  float currTheta, prevTheta;
  float turretAngle;
}robot;

typedef struct encoder
{
  //Struct to hold encoder information and address
  int currTick;
  int prevTick;
}encoder;



/*  Movement types

Planning on adding movements
for holonomic drive
                    */
int Fwd = 11;
int Rev = -11;
int turL = 9;
int turR = -9;




//User Control Tasks and Methods
void drive();



//Autonomous Tasks and Methods
void run()
{
	int up = vexRT[Btn6U];
	int down = vexRT[Btn6D];
	int Speed = up == 1 ? speed : down == 1 ? -speed : 0;
	motor[motorA] = Speed;
	motor[motorB] = Speed;
}



//Mapping Tasks and Methods
/*
  Mapping will be using an x,y, theta system
  The initial start of the vehicle will determine
  the x direction with the reference angle along
  the x axis, perpendicular to this will be the
  y axis. The initial position will be measured
  and after this the position will be updated.
  Currently the method of updating will be through
  gyroscope and drive encoders
*/

task updatePosition();
void zeroOutSensors();
int average(int value1, int value2);
int getDistance();
void initializeSturctures(robot *pos, encoder *left, encoder *right);

/*  Initialize robot position  */
struct robot robotBase;


/*  Initialize encoders  */
struct encoder leftEnc, rightEnc;


/* Pre Auton */
void pre_auton(){

}


/*                          Autonomous                            */
/*________________________________________________________________*/

task autonomous(){

}


/*                          User Control                          */
/*________________________________________________________________*/

task usercontrol()
{
  zeroOutSensors();

	initializeSturctures(robotBase, leftEnc, rightEnc);

  startTask(updatePosition);
	bLCDBacklight = true;

  while(true)
    {
    	clearLCDLine(0);
  		clearLCDLine(1);

    	drive();
    	run();

    	char angle[20];
    	sprintf(angle, "Angle: %.4f", robotBase.currTheta);
  		displayLCDCenteredString(1, angle);

			char position[30];
      sprintf(position, "%.1f,%.1f",robotBase.currX, robotBase.currY);
      displayLCDCenteredString(0, position);
      wait1Msec(30);
    }
}



/*                             Funtions                           */
/*________________________________________________________________*/

void zeroOutSensors()
{
  SensorValue[gyro] = 0;
  SensorValue[leftDriveEnc] = 0;
  SensorValue[rightDriveEnc] = 0;
}

void initializeSturctures(robot *pos, encoder *left, encoder *right)
{
	pos->currX = 0;
	pos->currY = 0;
	pos->prevX = 0;
	pos->prevY = 0;
	pos->currTheta = 0;
	pos->prevTheta = 0;

	left->currTick = 0;
	left->prevTick = 0;

	right->currTick = 0;
	right->prevTick = 0;

}

void drive()
{
  int Y1 = abs(vexRT[Ch3]) < DEADZONE ? 0 : vexRT[Ch3];
  int X2 = abs(vexRT[Ch1]) < DEADZONE ? 0 : vexRT[Ch1];
  motor[leftDrive] =  Y1 + X2;    /*   leftPower  */
  motor[rightDrive] = - Y1 + X2;   /*   rightPower */
}


task updatePosition()
{
  /*
    Use movement type to define how to get distance
    Use gyro to define the angle
    Create vector and add to original values
   */

  while(true){
    leftEnc.currTick = SensorValue[leftDriveEnc];
    rightEnc.currTick = SensorValue[rightDriveEnc];


    int distance = getDistance();//Get distance

    robotBase.currTheta = SensorValue[gyro] / 10. * PI / 180.;//Get angle

    robotBase.currX +=  cos(robotBase.currTheta) * distance;
    robotBase.currY +=  sin(robotBase.currTheta) * distance;

    time1[T3] = 0;

    while(time1[T3] < 100){} /*  Wait for 100 miliseconds to update */

    leftEnc.prevTick = leftEnc.currTick;
    rightEnc.prevTick = rightEnc.currTick;

  }
}


int getDistance()
{

  int driveConfig = 0;

  if(motor[leftDrive] > 0)
  {
  	driveConfig += 10;
  }
  else if(motor[leftDrive] < 0)
  {
  	 driveConfig -= 10;
	}

  if(motor[rightDrive] < 0)
  {
	 	driveConfig += 1;
  }
  else if(motor[rightDrive] > 0)
  {
   	driveConfig -= 1;
	}


	if(driveConfig == Fwd)
	{
    return average((leftEnc.currTick - leftEnc.prevTick), (rightEnc.currTick - rightEnc.prevTick));
  }
  else if(driveConfig == Rev)
  {
    return average((leftEnc.currTick - leftEnc.prevTick), (rightEnc.currTick - rightEnc.prevTick));
  }
  else if(driveConfig == turL || driveConfig == turR)
  {
    return 0; //Only angle should change
  }
  else /*No motor power*/
  {
    return 0;
  }
}

int average(int value1, int value2)
{
  return (value1 + value2) / 2;
}