int main()
{
// Handle CTLR + C
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = handleSignal;
sigemptyset(&sigIntHandler.sa_mask);
sigaction(SIGINT, &sigIntHandler, NULL);
quit = false;
auto lead = new car("lead");
auto follower = new car("follower");
auto controller = new pidController();
follower->setSpeed(0);
lead->setSpeed(0);
controller->setSetPoint(lead->getSpeed());
controller->setKi(1);
controller->setKd(1);
controller->setKp(.2);
auto pvThread = std::thread(runPv, controller);
auto pidThread = std::thread(runPid, controller);
auto simThread = std::thread(runSim, lead, follower, controller);
std::random_device rd; // only used once to initialise (seed) engine
std::mt19937 rng(rd()); // random-number engine used (Mersenne-Twister in this case)
std::uniform_int_distribution<int> uni(0,4); // guaranteed unbiased
while (!quit)
{
int randomSpeed = uni(rng);
lead->setSpeed(randomSpeed);
controller->setSetPoint(lead->getSpeed());
sleepSim(2);
}
pvThread.join();
simThread.join();
pidThread.join();
delete lead;
delete follower;
delete controller;
return 0;
}
void loopDeControlYAlarmas(void* arg){
byte i;
struct ArgLoop* argTmp=(struct ArgLoop*)arg;
for(i=0;i<argTmp->CantCanales;i++){
changeSensor(i);
setSetPoint(i);
}
if(isADCready()){
for(i=0;i<argTmp->CantCanales;i++) {
calculaValorFinal(i);
//Salida de control 1
calculosDeControl(&(argTmp->_control[i]));
}
for(i=0;i<argTmp->CantAlarmas;i++)
//Salida de Alarma 1
calculosDeAlarma(&(argTmp->_alarmas[i]));
FlagCleaner = 0; //lo limpio despues de procesar todos los flags
setFlagADCready(FALSE);
Mostrar_Proc=TRUE; //Mostrar los valores en la pantalla principal (se procesa en Main Handler)
}
}
void operatorControl() {
int speed = 4000;
int lastLCDState = 0;
while (1)
{
if(lcdReadButtons(uart1) != lastLCDState)
{
if(lcdReadButtons(uart1) == 1) speed -= 100;
else if(lcdReadButtons(uart1) == 4) speed += 100;
}
lastLCDState = lcdReadButtons(uart1);
setSetPoint(&shooter, speed);
updateShooter(&shooter);
runShooter(&shooter);
if(isShooterUpToSpeed(&shooter))
{
lcdSetText(uart1, 1, "YES :)");
}
else
{
lcdSetText(uart1, 1, "NO :(");
}
delay(20);
}
}
static state_t menuSetpoint(button_t button)
{
if (button == BUTTON_ENTER)
{
lcd.clear();
lcd.print(F("Cooling setpoint"));
editInt = g_SetPoint;
}
else if (isEditing && button == BUTTON_CENTER)
{
setSetPoint(editInt);
}
return menuNumberEdit(button, 1, -40, 100, PSTR(" %3d"DEGREE"%c "));
}
task autonomous()
{
pre_auton();
while(true)
{
if(vexRT[AButton] == 1)
{
setArmSpeed(calcPID(PID));
encoderDriveStraight(44.0/12.0);
setCollectorSpeed(MAX_TREAD);
wait10Msec(400);
setCollectorSpeed(0);
setSetPoint(PID, armGround);
enable(PID);
encoderDriveStraight(-44.0/12.0);
}
}
}
void shooterSetSetPoint(SingleFlyWheelShooter *shooter, int setPoint)
{
setSetPoint((*shooter).controller, (double) setPoint);
}
