task main()
{
clearTimer(timer1);
bool toggle1 = false;
int toggletime = 200;
bool togglecheck = false;
int X1 = 1;
int Y1 = 1;
while (1 == 1)
{
if (time1[timer1] >= toggletime)
{
togglecheck = true;
clearTimer(timer1);
}
if (togglecheck == true)
{
if (vexRT[Btn8D] == 1)
{
if (toggle1 == false)
toggle1 = true;
else
if (toggle1 == true)
toggle1 = false;
}
}
if (toggle1 == true)
{
X1 = X1 * -1;
Y1 = Y1 * -1;
}
togglecheck = false;
}
}
////////////////////////////RPM CALCULATION TASK
task RPMCALC()
{
int count = 0;
int status = 0;
clearTimer(T1);
while(1)
{
if (SensorValue[TAC] == 0 && status == 0)
{
count++;
status = 1;
} else if (SensorValue[TAC] == 1)
{
status = 0;
}
if (count == 5)
{
CurrentSpeed = (1000*5*30)/time1[T1];
clearTimer(T1);
count = 0;
}
if (time1[T1] > 500)
{
CurrentSpeed = 0;
}
}
}
void roboControl(void){ //For autonomous control
setLauncherSpeed(30);
launcherSpeed_new = 127;//Lets start this shoot speed
goForwardFor_time(10);//Go forward 10 seconds
goBackwardFor_time(1);
clearTimer(T1);
while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
/*goBackwardFor_time(1);
turnRight(0.1);
setMotor(hozBelt,-100);
goForwardFor_time(.5);
turnRight(0.1);
goForwardFor_time(5);*/
goForwardFor_time(7);//Go forward 7 seconds
goBackwardFor_time(1); //Get to launching distance
clearTimer(T1);
while(time1[T1] < 15000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
goBackwardFor_time(1);
turnRight(1);
setMotor(hozBelt,-100);
goForwardFor_time(.5);
turnRight(0.25);
goForwardFor_time(5);
}
void ofxGenericAnimatedImageView::timer_fired( ofPtr< ofxGenericTimer > timer )
{
if ( timer == _frameTimer )
{
int newFrame = _currentFrame + _animationDirection;
if ( newFrame >= (int) _frames.size() )
{
if ( _delegate )
{
_delegate.lock()->animatedImage_animationEnded( dynamic_pointer_cast< ofxGenericAnimatedImageView >( _this ) );
}
if ( _loopMode == ofxGenericAnimatedImageLoopTypeWrap )
{
newFrame = 0;
}
else if ( _loopMode == ofxGenericAnimatedImageLoopTypePingPong )
{
_animationDirection = -1;
newFrame = _frames.size() - 1;
}
else if ( _loopMode == ofxGenericAnimatedImageLoopTypeClamp )
{
newFrame = _frames.size() - 1;
clearTimer();
}
else if ( _loopMode == ofxGenericAnimatedImageLoopTypeOnce )
{
newFrame = 0;
clearTimer();
}
}
else if ( newFrame < 0 )
{
if ( _loopMode == ofxGenericAnimatedImageLoopTypePingPong )
{
_animationDirection = 1;
}
else if ( _loopMode == ofxGenericAnimatedImageLoopTypeClamp )
{
clearTimer();
}
if ( _animationDirection < 0 && _delegate )
{
_delegate.lock()->animatedImage_animationEnded( dynamic_pointer_cast< ofxGenericAnimatedImageView >( _this ) );
}
newFrame = 0;
}
_currentFrame = newFrame;
showFrame( _currentFrame );
}
}
task waitUntilNotFiringTask() {
firing = true;
wait1Msec(initialWait);
clearTimer(firingTimer);
while (time1(firingTimer) < timeWithoutFiring) {
if (Error > notFiringCutoff) clearTimer(firingTimer);
}
firing = false;
}
task main()
{
clearTimer(T1); //reset timer
while(true){
while(time1[T1] > 200) { //set timer to 100 so it doesn't overflow
writeDebugStreamLine("%d", SensorValue(sonarSensor)); //display sonar sensor in robotC debug stream
clearTimer(T1); //reset timer
} //while time1
}//while true
} //task
void ControlFunction(MOTOR_PI* motorA, MOTOR_PI* motorB){
int storeTime;
storeTime=time1[T1];
if(storeTime >= SAMPLE_PERIOD/2){
ReadSpeed(motorA, storeTime);
ReadSpeed(motorB, storeTime);
clearTimer(T1);
}
if(time1[T2] >= SAMPLE_PERIOD){
PI_Control(motorA);
PI_Control(motorB);
clearTimer(T2);
}
}
task autonomous()
{
TargetSpeed = 950;
setpower = 104;
startTask(motorcontrol);
motor[Fly1] = power;
motor[Fly2] = power;
motor[Fly3] = power;
motor[Fly4] = power;
wait1Msec(1000);
clearTimer(T1);
while(true)
{
if(abs(Error) < 60)
{
motor[intkae] = 127;
motor[feed] = 127;
}
else
{
motor[intkae] = 0;
motor[feed] = 0;
}
}
}
///////////////////////////////////////TURN BASED ON GYRO DEGREES EXAMPLE // TurnDegree(-90, 127);//THIS TURNS LEFT 90 DEGREES AT 127 POWER
void TurnDegree(int degrees, int speed, int Timeout)
{
int left = 0, right = 0;
SensorValue[Gyro] = 0;
if (degrees < 0){left = -1; right = 1;}
else if (degrees > 0){left = 1; right = -1; }
float ticks = abs(degrees*6.7);
clearTimer(T3);
while(abs(SensorValue[Gyro]) < ticks)
{
////////////////////////////////////FAILASFE TIMEOUT
if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}
////////////////////////////////////////////////////
motor[Left1] = speed*left;
motor[Left2] = speed*left;
motor[Left3] = speed*left;
motor[Right1] = speed*right;
motor[Right2] = speed*right;
motor[Right3] = speed*right;
}
motor[Left1] = (speed*left*-1)/9;
motor[Left2] = (speed*left*-1)/9;
motor[Left3] = (speed*left*-1)/9;
motor[Right1] = (speed*right*-1)/9;
motor[Right2] = (speed*right*-1)/9;
motor[Right3] = (speed*right*-1)/9;
wait1Msec(250);
StopDrive();
}
task waitUntilNotFiringTask() {
firing = true;
//if (initialWait == 0) {
// while (SensorValue[flywheelSwitch] == 1) { EndTimeSlice(); }
//}
//else {
wait1Msec(initialWait);
//}
clearTimer(T2/*firingTimer*/);
while (time1[T2/*firingTimer*/] < timeWithoutFiring) {
if (SensorValue[flywheelSwitch] == 0) clearTimer(firingTimer);
EndTimeSlice();
}
firing = false;
}
task flywheelStabilization() { //modulates motor powers to maintain constant flywheel velocity
clearTimer(T1);
float prevError;
float error;
float integral;
int numbbup = 0; //debug
float totalError = 0;
int numloops = 0;
while (true)
{
prevError = targetVelocity - flywheelVelocity;
integral = 0;
while (abs(targetVelocity - flywheelVelocity) < bangBangErrorMargin * targetVelocity && targetVelocity > 0/*true*/) { EndTimeSlice(); }
//bang bang control
bangBangCount += 1;
numbbup += (targetVelocity > flywheelVelocity ? 1 : 0);
bbpercentup = 100 * numbbup / bangBangCount;
bangBangPerSec = (float)((float)bangBangCount * 1000) / (float)(time1(T1) + .1);
while (abs(targetVelocity - flywheelVelocity) > bangBangErrorMargin * flywheelVelocity * 0.75 && targetVelocity > 0) {
setLauncherPower((targetVelocity > flywheelVelocity) ? (127) : ( 0));
EndTimeSlice();
}
setLauncherPower(defaultPower);
while (targetVelocity == 0) { EndTimeSlice(); } //pauses while
}
}
//Function used to determine which button has been pushed without polling
void testAndRespondToButtonPush(char buttonToTest) {
if (buttonToTest & P1IFG) {
if (buttonToTest & P1IES) {
if (flag == 5) {
LCDclr();
location = initPlayer();
printPlayer(location);
flag = 0;
}
else {
//This code is used to prevent the player (*) from moving outside of the game bounds.
int mod = 0;
mod = movePlayerInResponseToButtonPush(buttonToTest);
clearPlayer(location);
location += mod;
location = movePlayer(location, mod);
clearTimer();
}
//Debounces the button
} else {
debounce();
}
//Toggles between reading rising and falling edge
P1IES ^= buttonToTest;
P1IFG &= ~buttonToTest;
}
}
/*******************************************************************************
Function: CNVSTimer::exit()
Description: 退出定时检测线程
Calls:
Called By:
Input: 无
Output: 无
Return: 无
*******************************************************************************/
void CNVSTimer::exit()
{
if(NULL == m_pVosThread)
{
IVS_RUN_LOG_ERR("NVSTimer exit: m_pVosThread is null");
return;
}
//先停止线程
this->m_bExit = VOS_TRUE;
errno = 0;
long ret_val = VOS_ThreadJoin(m_pVosThread);
if (ret_val != VOS_OK)
{
IVS_RUN_LOG_ERR("Wait timer thread exit failed. ret_val(%ld). error(%d):%s", ret_val, errno, strerror(errno));
}
//释放内存
VOS_free(m_pVosThread);
m_pVosThread = VOS_NULL;
//再释放资源
clearTimer();
//释放锁
(void)VOS_DestroyMutex(m_pMutexListOfTrigger);
m_pMutexListOfTrigger = VOS_NULL;
IVS_DBG_LOG("FILE(%s)LINE(%d): CNVSTimer::exit: exit complete.", _TIMER_FL_);
return;
};
byte reserveAvailableTimer(tMotor motorPort1, tMotor motorPort2, byte fullSpeed, byte slowSpeed, int ms, tSensors encoderPort, int encoderSlowPos, int encoderStopPos, bool brake)
{
for (byte i = 0; i < 4; i++)
{
// Check for existing port first to reset, afterwards look for inactive slot to activate
if ((motorCtrl[i].motorPort1 == motorPort1 && motorCtrl[i].motorPort2 == motorPort2) || !motorCtrl[i].active) {
clearTimer(motorCtrl[i].timer);
motorCtrl[i].ms = (ms > 32767 ? 32767 : ms);
motorCtrl[i].motorPort1 = motorPort1;
motorCtrl[i].motorPort2 = motorPort2;
motorCtrl[i].powerPct = fullSpeed;
motorCtrl[i].slowPct = slowSpeed;
motorCtrl[i].encoderInUse = true;
motorCtrl[i].encoderPort = encoderPort;
motorCtrl[i].encoderSlowPos = encoderSlowPos;
motorCtrl[i].encoderStopPos = encoderStopPos;
motorCtrl[i].lastKnownPos = SensorValue[encoderPort];
motorCtrl[i].brake = brake;
motorCtrl[i].active = true; // Activate last
//writeDebugStreamLine("Reserve Timer: %d", i);
return i;
}
}
return -1;
}
task autonomous()
{
wait1Msec(5000);
startTask(FlywheelController);
SetTarget(1600,80);
startTask(recoverFromShots);
wait1Msec(4500);
SensorValue[ANGLE] = 0;
clearTimer(T3);
while(time1[T3] <2550)
{
motor[rdy] = motor[rd] = -127 - (SensorValue[ANGLE]); // was 8
motor[ld] = motor[ldy] = -127 + (SensorValue[ANGLE]);
wait1Msec(10);
}
motor[rd] = motor[rdy] = 0;
motor[ld] = motor[ldy] = 0;
startTask(autoConveyor);
while(true)
{}
// AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
}
task driveStraightTask()
{
driveStraightRunning = true;
int coeff = 15;
int totalClicks = 0;
int slavePower = drivePower - 2;
int error = 0;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
SensorValue[gyro] = 0;
clearTimer(driveTimer);
while (abs(totalClicks) < clicks && time1(driveTimer) < timeout)
{
setDrivePower(drivePower * direction, slavePower * direction);
error = SensorValue[gyro];
slavePower += error / coeff;
totalClicks += (abs(SensorValue[leftEncoder]) + abs(SensorValue[rightEncoder])) / 2;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
wait1Msec(100);
}
setDrivePower(0, 0);
wait1Msec(delayAtEnd);
driveStraightRunning = false;
}
///////////////////////////////////////INTAKE SHOOT BASED ON RPM AND TOLERANCES
void IntakeShoot(int TimeOut, int Tolerance = 8)
{
if (BallCounter == 1) Tolerance = 12;
clearTimer(T3);
while(BallCounter > 0)
{
while(SensorValue[IR4] == 1)//MOVE BALL UP TO SENSOR
{
//BREAK STATEMENTS
if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}
if(BallCounter == 0) break;
//BREAK STATEMENTS
motor[Intake]= 127;
motor[feeder]= 127;
}
if(rpmError < Tolerance)
{
motor[Intake]= 127;
motor[feeder]= 127;
}else
{
motor[Intake]= 0;
motor[feeder]= 0;
}
if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}//BREAK STATEMENTS
}
}
task feederWait () {
wait1Msec(1500); //Wait time to delay for auto feeder
clearTimer(T4);
while(true) {
if(!SensorValue[ballHigh]) {
motor[feeder] = 127;
} else if(time1[T4]>=feederWaitTime) {
motor[feeder] = 127;
clearTimer(T4);
wait1Msec(200);
} else {
motor[feeder] = 0;
}
wait1Msec(25);
}
}
task usercontrol()
{
// User control code here, inside the loop
bool toggle1 = false;
int toggletime = 200;
bool togglecheck = false;
int X2 = 0, Y1 = 0, X1 = 0, threshold = 10;
while (true)
{
if (time1[timer1] >= toggletime)
{
togglecheck = true;
clearTimer(timer1);
}
//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
//Create "deadzone" for Y1/Ch3
if(abs(vexRT[Ch3]) > threshold)
Y1 = vexRT[Ch3];
else
Y1 = 0;
//Create "deadzone" for X1/Ch4
if(abs(vexRT[Ch4]) > threshold)
X1 = vexRT[Ch4];
else
X1 = 0;
//Create "deadzone" for X2/Ch1
if(abs(vexRT[Ch1]) > threshold)
X2 = vexRT[Ch1];
else
X2 = 0;
if (togglecheck == true)
{
if (vexRT[Btn8D] == 1)
{
if (toggle1 == false)
toggle1 = true;
else
if (toggle1 == true)
toggle1 = false;
}
}
if (toggle1 == true)
{
X1 = X1 * -1;
Y1 = Y1 * -1;
}
togglecheck = false;
//Remote Control Commands
motor[frontRight] = Y1 - X2 - X1;
motor[backRight] = Y1 - X2 + X1;
motor[frontLeft] = Y1 + X2 + X1;
motor[backLeft] = Y1 + X2 - X1;
}
}
void goBackwardFor_time(int time){ //Goes Backwards for a set ammount of time
setLeftMotors(-autoSpeed);
setRightMotors(-autoSpeed);
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();}
stopLeftMotors();
stopRightMotors();
}
task autonomous()
{
clearTimer(T2);
startTask(shooterDJ);
while(time1[T2]<13000) {wait1Msec(25)}
shooterPowerDown();
}
void roboControl(void){ //For autonomous control //This is test code
setLauncherSpeed(30);
launcherSpeed_new = 127;//Lets start this
goForwardFor_time(5.3,3);//Go forward 6.1 seconds
clearTimer(T1);
while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);
updateLauncherSpeed(2);
}
}
void ofxGenericAnimatedImageView::start( float frameRate )
{
if ( frameRate >= 0 )
{
_frameRate = frameRate;
}
clearTimer();
_frameTimer = ofxGenericTimer::create( 1.0f / _frameRate, true, dynamic_pointer_cast< ofxGenericTimerDelegate >( _this ) );
}
void resetIME ()
{
resetMotorEncoder(right);
resetMotorEncoder(left);
resetMotorEncoder(grabL);
resetMotorEncoder(armR3);
resetMotorEncoder(armL3);
clearTimer(T1);
}
/* User Control */
task usercontrol()
{
initMotor(&motorLeftShooter, leftShooter1, leftShooter2, leftShooterSensor);
initMotor(&motorRightShooter, rightShooter1, rightShooter2, rightShooterSensor);
clearAll(actOnSensors);
clearTimer(T1);
clearTimer(T2);
while(true)
{
drive();
intake();
shooter(&motorLeftShooter, &motorRightShooter);
velocidade_motor_esquerdo=motorLeftShooter.speedRead;
potencia_motor_esquerdo=motorLeftShooter.controller_output;
velocidade_motor_direito=motorRightShooter.speedRead;
potencia_motor_direito=motorRightShooter.controller_output;
}
}
task intakeControl () {
while(true) {
motor[intake] = ((vexRT(Btn5U)||intakeAutonomousIntake)-vexRT(Btn5D))*100;
//Move ball from high limit switch to low limit switch
if(vexRT(Btn6D) && SensorValue[indexHigh]) {
motor[indexer] = -127;
delay(intakeMoveDownTime);
}
//Shooting control
if (vexRT(Btn6U) || intakeAutonomousShoot) {
//if(intakeLongShot?abs(currentShot.velocity-flywheelVelocity)<currentShot.velocityThreshold:true)) {
if(time1[T1]>300 || !intakeLongShot) {
writeDebugStreamLine("%d", flywheelVelocity);
motor[indexer] = 127;
wait1Msec(150);
clearTimer(T1);
}
else {
motor[indexer] = (SensorValue[indexHigh])?0:127;
}
}
//Move ball down even if there is a sensor we want
else if (vexRT(Btn5D))
motor[indexer] = -127;
//Stop ball if ball is at a sensor
else if(SensorValue[indexLow]<intakeLightThreshold && !SensorValue[indexHigh]) {
motor[indexer] = 70;
clearTimer(T2);
while(time1[T2] < intakeMoveUpTime && !SensorValue[indexHigh]) { delay(20); }
motor[indexer] = 0;
}
else
motor[indexer] = 0;
delay(25);
}
}
task main()
{
clearTimer(T1);
while(true)
{
drive();
intake();
shooter();
timenow = time1[T1];
if(timenow>=30){
encoderr = SensorValue[rightShooterSensor];
speedReadr = abs((1000.0*(encoderr - lastr))/timenow); //that is not a unit. You should multiply for a constant. I will leave this way by now.
lastr = encoderr;
checktime=timenow;
clearTimer(T1);
}
}
}
task autonomous()
{
// .....................................................................................
//Insert user code here.
clearTimer(T1);
while(getTimerValue(T1)<1000){
motor[frightMotor] = -80;
motor[fleftMotor] = -80;
}
// .....................................................................................
}
/*
void goForwardFor_time(int time){ //Goes forward for a set ammount of time
setMotor(leftMotor,autoSpeed);//Use -8 to go forward
setMotor(rightMotor,(autoSpeed-5));
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopMotor(leftMotor);
stopMotor(rightMotor);
}
*/
void goForwardFor_time(int time, int launcherSpeedInc){ //Goes forward for a set ammount of time
setMotor(leftMotor,autoSpeed);
setMotor(leftMotor2,autoSpeed);
setMotor(rightMotor,autoSpeed);
setMotor(rightMotor2,autoSpeed);//Use -8 to go forward
setRightMotors(autoSpeed-20);
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopLeftMotors();
wait(.8);
stopRightMotors();
}
task autonomous()
{
clearTimer(T2);
speed = 97;
feederWaitTime = 800;
startTask(shooterDJ);
motor[intake1] = 127;
while(time1[T2]<5800) {wait1Msec(25);}
motor[intake1] = 0;
startTask(driveForwardEndAutonomous);
shooterPowerDown();
}
