Our PID control works outside of the Competition code however when we attach it to the Comp code and ask it to attach it to a button we either get no response from the robot or we cannot escape the PID loop. Any advice is appreciated.
#pragma config(Sensor, in1, in6, sensorAnalog)
#pragma config(Sensor, dgtl1, ShooterEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, Limit, sensorTouch)
#pragma config(Sensor, dgtl4, LeftQuad, sensorQuadEncoder)
#pragma config(Sensor, dgtl6, RightQuad, sensorQuadEncoder)
#pragma config(Motor, port1, IntakeLeft, tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, ShooterLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, ShooterLeft2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, RightBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, LiftLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, LiftRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, LeftBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, ShooterRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, ShooterRight2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port10, IntakeRight, tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//////////////////////////////////////////////////////////////////////////////////////
float currentEncoder ;
float currentRPM ;
float error ;
float percentError ;
float lastAdjustment ;
float currentAdjustment ;
float truePower;
int time ;
int buffer = 0;
int Toggle=0;
int test;
//////////////////////////////////////////////////////////////////////////////////////
float TIMER_MSEC = 12.5;
float RPM = 60*1000/TIMER_MSEC ;
void CONTROLL_LOOP(float Target, float motorPower)
{
if (test==0)
{
clearAll(actOnSensors);
motor[ShooterLeft] = motorPower;
motor[ShooterLeft2] = motorPower;
motor[ShooterRight] = motorPower;
motor[ShooterRight] = motorPower;
test =1;
}
////////////////////////////////////////////////////////////////////////
clearAll(actOnSensors);
clearTimer (T1);
Time=0;
while (time < TIMER_MSEC)
{
time = time1[T1];
}
// currentEncoder = (SensorValue[ShooterEncoder]); // Poll Encoder
currentEncoder = (SensorValue[ShooterEncoder]);
currentRPM = currentEncoder / 360*RPM; // RPM calc
error = (Target - currentRPM); // Error in RPM
percentError = (error/Target); // error in percent
////////////////////////////////////////////////////////////////////////
if(percentError < 1)
{
currentAdjustment = percentError * 127 * 0.0039;
}
if(buffer== 0)
{
truePower = motorPower;
//lastAdjustment=currentAdjustment;
buffer = 1;
}
else
{
truePower = (truePower + currentAdjustment);
}
clearAll(actOnSensors);
motor[ShooterRight] = (truePower);
motor[ShooterRight2] = (truePower);
motor[ShooterLeft] = (truePower);
motor[ShooterLeft2] = (truePower);
}
//////////////////////////////////////////////////////////////////////////////////////
/*
task main
{
while(true)
{
CONTROLL_LOOP (230,45);
}
}*/
#pragma config(Sensor, in1, AimBot, sensorGyro)
#pragma config(Sensor, dgtl1, ShooterEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, Limit, sensorTouch)
#pragma config(Sensor, dgtl4, LeftQuad, sensorQuadEncoder)
#pragma config(Sensor, dgtl6, RightQuad, sensorQuadEncoder)
#pragma config(Motor, port1, IntakeLeft, tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, ShooterLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, ShooterLeft2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, RightBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, LiftLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, LiftRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, LeftBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, ShooterRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, ShooterRight2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port10, IntakeRight, tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
#include "LCD_Display.c"
#include "RangeFinder.c"
#include "ReVamp_PID.c"
#include "Auto library.c"
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
//Display();
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
task autonomous()
{
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
int manual = 0;
int ShooterSpeed=0;
task usercontrol()
{
while(true)
{
//while(manual == 0)
//{
CONTROLL_LOOP (0,0);
motor[LeftBase] = vexRT(Ch2);
motor[RightBase] = vexRT(Ch3);
motor[IntakeLeft] = vexRT(Ch3Xmtr2);
motor[IntakeRight] = vexRT(Ch3Xmtr2);
while(vexRT(Btn5DXmtr2) == true )//&& Toggle ==0) // RedLower
{
test =0;
CONTROLL_LOOP(215,110);
//RangeFind = 1;
//RedSideLowerBlock();
//BlueSideLowerBlock();
}
}
//}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*while(manual == 1)
{
motor[LeftBase]= vexRT(Ch2);
motor[RightBase] = vexRT(Ch3);
motor[ShooterLeft] = ShooterSpeed;
motor[ShooterLeft2] = ShooterSpeed;
motor[ShooterRight] = ShooterSpeed;
motor[ShooterRight2] = ShooterSpeed;
if(vexRT[Btn7DXmtr2] == 1)
{
ShooterSpeed = 127;
}
if(vexRT[Btn7RXmtr2] == 1)
{
ShooterSpeed = 100; // Walls
}
if(vexRT[Btn7UXmtr2] == 1)
{
ShooterSpeed = 75; // Center Feild
}
if(vexRT[Btn8DXmtr2] == 1) // Speed Adjusment on motors -5
{
ShooterSpeed = ShooterSpeed - 5;
while(vexRT[Btn8DXmtr2] == 1)
{
}
}
if(vexRT[Btn8UXmtr2] == 1) // Speed Adjusment on motors +5
{
ShooterSpeed = ShooterSpeed + 5;
while(vexRT[Btn8UXmtr2] == 1)
{
}
}
if(vexRT(Btn5UXmtr2) == true)
{
motor[IntakeLeft] = 127;
motor[IntakeRight] = 127;
}
if(vexRT(Btn5DXmtr2) == true)
{
motor[IntakeLeft] = -127;
motor[IntakeRight] = -127;
}
if(vexRT(Btn5DXmtr2) == false && vexRT(Btn5U) == false)
{
motor[IntakeLeft] = 0;
motor[IntakeRight] = 0;
}
if (vexRT(Btn7LXmtr2) == true && vexRT(Btn8RXmtr2) == true && manual==1)
{
manual = 0;
while(vexRT(Btn7LXmtr2) == true && vexRT(Btn8RXmtr2) == true)
{
}
}
}*/
}