I am trying to use some functions in robotc to make my life easier, but my code did not work. Is this the correct place to define functions?
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1, rightpot, sensorPotentiometer)
#pragma config(Sensor, in2, leftpot, sensorPotentiometer)
#pragma config(Sensor, dgtl1, autoncolor, sensorTouch)
#pragma config(Sensor, dgtl2, autonleftright, sensorTouch)
#pragma config(Sensor, dgtl3, claw, sensorQuadEncoder)
#pragma config(Sensor, dgtl7, piston1, sensorDigitalOut)
#pragma config(Sensor, dgtl8, piston2, sensorDigitalOut)
#pragma config(Sensor, dgtl12, light, sensorDigitalOut)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, llift, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, leftlift, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port3, clawright, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port4, liftright, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port5, chassisleft, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port6, chassisright, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor, port7, liftleft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port8, clawleft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port9, rightlift, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port10, rlift, tmotorVex393_HBridge, openLoop, reversed)
/*---------------------------------------------------------------------------*/
/* */
/* Description: Competition template for VEX EDR */
/* */
/*---------------------------------------------------------------------------*/
// This code is for the VEX cortex platform
#pragma platform(VEX2)
// Select Download method as "competition"
#pragma competitionControl(Competition)
//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"
float inches;
float degrees;
float liftvalue;
void driveforward(float inches)
{
while(SensorValue[I2C_1] + SensorValue[I2C_2]/2 < (inches * 31.185362));
{
if(SensorValue[I2C_1] < SensorValue[I2C_2])
{
motor[chassisleft] = -127;
motor[chassisleft] = 100;
}
else if(SensorValue[I2C_1] > SensorValue[I2C_2])
{
motor[chassisleft]= -100;
motor[chassisright]= 127;
}
else
{
motor[chassisleft]= -127;
motor[chassisright]= 127;
}
}
}
void drivebackwards(float inches)
{
while(SensorValue[I2C_1] + SensorValue[I2C_2]/2 > (inches * 31.185362 * -1));
{
if(SensorValue[I2C_1] < SensorValue[I2C_2])
{
motor[chassisleft] = 127;
motor[chassisleft] = -100;
}
else if(SensorValue[I2C_1] > SensorValue[I2C_2])
{
motor[chassisleft]= 100;
motor[chassisright]= -127;
}
else
{
motor[chassisleft]= 127;
motor[chassisright]= -127;
}
}
}
void turnleft(float degrees)
{
while((SensorValue[I2C_1] *-1) + SensorValue[I2C_2]/2 < (inches * 31.185362 * 0.06544444444));
{
if((SensorValue[I2C_1] * -1)< SensorValue[I2C_2])
{
motor[chassisleft] = 127;
motor[chassisleft] = 100;
}
else if((SensorValue[I2C_1] *-1) > SensorValue[I2C_2])
{
motor[chassisleft]= 100;
motor[chassisright]= 127;
}
else
{
motor[chassisleft]= 127;
motor[chassisright]= 127;
}
}
}
void turnright(float degrees)
{
while(SensorValue[I2C_1] + (SensorValue[I2C_2]* -1)/2 < (inches * 31.185362 * 0.06544444444));
{
if(SensorValue[I2C_1]< (SensorValue[I2C_2] * -1))
{
motor[chassisleft] = -127;
motor[chassisleft] = -100;
}
else if(SensorValue[I2C_1] > (SensorValue[I2C_2]* -1))
{
motor[chassisleft]= -100;
motor[chassisright]= -127;
}
else
{
motor[chassisleft]= -127;
motor[chassisright]= -127;
}
}
}
void liftup(float liftvalue)
{
while(SensorValue[leftpot] + SensorValue[rightpot] /2 < liftvalue)
{
if(SensorValue[leftpot] < SensorValue[rightpot])
{
motor[leftlift] = 127;
motor[llift] = 127;
motor[liftleft] = 127;
motor[rightlift] = 115;
motor[rlift] = 115;
motor[liftright] = 115;
}
else if(SensorValue[leftpot] > SensorValue[rightpot])
{
motor[leftlift] = 115;
motor[llift] = 115;
motor[liftleft] = 115;
motor[rightlift] = 127;
motor[rlift] = 127;
motor[liftright] = 127;
}
else
{
motor[leftlift] = 127;
motor[llift] = 127;
motor[liftleft] = 127;
motor[rightlift] = 127;
motor[rlift] = 127;
motor[liftright] = 127;
}
}
}
void liftdown(float liftvalue)
{
while(SensorValue[leftpot] + SensorValue[rightpot] /2 > liftvalue)
{
if(SensorValue[leftpot] > SensorValue[rightpot])
{
motor[leftlift] = -127;
motor[llift] = -127;
motor[liftleft] = -127;
motor[rightlift] = -115;
motor[rlift] = -115;
motor[liftright] = -115;
}
else if(SensorValue[leftpot] < SensorValue[rightpot])
{
motor[leftlift] = -115;
motor[llift] = -115;
motor[liftleft] = -115;
motor[rightlift] = -127;
motor[rlift] = -127;
motor[liftright] = -127;
}
else
{
motor[leftlift] = -127;
motor[llift] = -127;
motor[liftleft] = -127;
motor[rightlift] = -127;
motor[rlift] = -127;
motor[liftright] = -127;
}
}
}
void resetstuff()
{
SensorValue[I2C_1] = 0;
SensorValue[I2C_2] = 0;
motor[port1]=0;
motor[port2]=0;
motor[port3]=0;
motor[port4]=0;
motor[port5]=0;
motor[port6]=0;
motor[port7]=0;
motor[port8]=0;
motor[port9]=0;
motor[port10]=0;
}
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks
// running between Autonomous and Driver controlled modes. You will need to
// manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
// used by the competition include file, for example, you might want
// to display your team name on the LCD in this function.
// bDisplayCompetitionStatusOnLcd = false;
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
task autonomous()
{
resetstuff();
driveforward(12);
turnleft(90);
resetstuff();
drivebackwards(6);
resetstuff();
turnright(90);
}