void MFCBot::TestPeriodic() { lw->Run(); StallableMotor::updateControllers(); if (dont++> 10) { dont = 0; //This kills the bug } WatchDog(); }
void main(void) { ////////////////////// TMUNUM_INFO xx; TMUNUM_INFO yy; /////////////////////////// unsigned char index; static unsigned char num_byte = 0; char y =0; EA = 0; Soft_WDT_Disnable(); mcu_f340_init(); delay_ms(10); var_init(); fmu_cmd_init(); uart1_init(); #ifdef ID_ONE_WIRE one_wire_init( ONE_WIRE_STANDARD_SPEED); #endif delay_ms(10); tmu_address_init(); EA = 1; WatchDog_Init(); index = 0; countnum = 0; port_num = 0; while(1) { xx.c[0] = 2; xx.c[1] = 2; xx.c[2] = 2; xx.c[3] = 2; xx.c[4] = 2; xx.c[5] = 2; write_tmunum_info(&xx); read_tmunum_info(&yy); } while(1) { char x[10]={1,2,3,4,5,6,7,8,9,10}; char temp[10]; int i; unsigned char code * data ptrCode; x[0] = y++; WatchDog(); earseCodeFlash(TMU_NUM_INFO_ADDR); WatchDog(); writeInnerFlash(TMU_NUM_INFO_ADDR, 10, x); ptrCode = (char code *)TMU_NUM_INFO_ADDR; for(i = 0;i<10;i++) { temp[i] = 0; } for (i = 0; i <10; i++) { temp[i] = *ptrCode++; } WatchDog(); } while(1) { WatchDog(); uart_process(); if(flag30ms) { /*sssssssssssssssss*/ flag30ms = 0; if(num_byte == 0) { num_byte = 1; read_id(0,0,(unsigned char*)localid_buf); WatchDog(); } else { num_byte = 0; read_id(0,1,(unsigned char*)localid_buf); WatchDog(); refresh_current_id(); memory_clear((unsigned char*)localid_buf,sizeof(localid_buf)); read_id_96(); } } if( flag60ms ) { flag60ms = 0; index ++; uart1_timeout(); port_state_process(); slot_aging_time(); if(index >= 20) { index = 0; fmu_cmd_update_file_timer_count(); } } } }
void MFCBot::AutonomousPeriodic() { Scheduler::GetInstance()->Run(); WatchDog(); StallableMotor::updateControllers(); }
void MFCBot::TeleopPeriodic() { // SmartDashboard::PutData(Scheduler::GetInstance()); Scheduler::GetInstance()->Run(); WatchDog(); StallableMotor::updateControllers(); if (CommandBase::pterodactyl->isPIDFinished(true)) { trueTicks++; } else { trueTicks = 0; } if (dont++> 25) { thingy->Set(trueTicks>20 ? Relay::kForward : Relay::kOff); int verbosity= SMARTDASH_VERBOSITY; if (ROBOT_VISUALIZATION) { robotState->PutBoolean("jawsClosed", CommandBase::collector->getJawState()); robotState->PutNumber("pterodactylAngle", CommandBase::pterodactyl->getAngle()); robotState->PutBoolean("hasBall", CommandBase::collector->isBallDetected()); robotState->PutNumber( "collectorMotorState", CommandBase::collector->isPIDEnabled() ? (CommandBase::collector->isRollerStalled() ? 2 : 1) : 0); robotState->PutBoolean("shooterWinchStalled", CommandBase::shooter->isShooterMotorStalled()); robotState->PutNumber("shooterStrap", CommandBase::shooter->getTurns()); robotState->PutBoolean("shooterLatched", CommandBase::shooter->isReallyDrawnBack()); robotState->PutNumber("compressorState", CommandBase::pneumatics->isCompressorOn() ? 1 : 0); robotState->PutNumber("driveLeftState", fabs(CommandBase::driveBase->getMotorSpeed())> 0 ? 1 : 0); robotState->PutNumber("driveRightState", fabs(CommandBase::driveBase->getMotorSpeed())> 0 ? 1 : 0); } if (verbosity & 1) { SmartDashboard::PutBoolean("Has Ball", CommandBase::collector->isBallDetected()); SmartDashboard::PutBoolean("Shooter Ready", CommandBase::shooter->isReallyDrawnBack() && CommandBase::shooter->lastReleasePosition > 0.0); SmartDashboard::PutBoolean("Pterodactyl Ready", CommandBase::pterodactyl->isPIDFinished(true)); } if (verbosity & 2) { SmartDashboard::PutNumber("Winch position", CommandBase::shooter->getTurns()); SmartDashboard::PutNumber("pteroangle", CommandBase::pterodactyl->getAngle()); } if (verbosity & 4) { SmartDashboard::PutNumber("LeftWheels", CommandBase::driveBase->getLeftEncoder()->GetDistance()); SmartDashboard::PutNumber("RightWheels", CommandBase::driveBase->getRightEncoder()->GetDistance()); SmartDashboard::PutNumber("Winch rate", CommandBase::shooter->getTurnRate()); SmartDashboard::PutNumber("pterorate", CommandBase::pterodactyl->getRate()); SmartDashboard::PutNumber("Collect Rate", CommandBase::collector->getRollerSpeed()); } if (verbosity & 8) { SmartDashboard::PutBoolean("Shooter Raw Latch", CommandBase::shooter->getRawSLatch()); SmartDashboard::PutBoolean("Shooter Raw Promixity", CommandBase::shooter->getRawProximity()); SmartDashboard::PutBoolean("Shooter Pattern Latched", CommandBase::shooter->isLatchedByPattern()); SmartDashboard::PutBoolean("Shooter Promixity Latched", CommandBase::shooter->isLatchedByProximity()); SmartDashboard::PutBoolean("Shooter Winch Latch", CommandBase::shooter->getWLatch()); SmartDashboard::PutBoolean("Shooter Motor Stalled", CommandBase::shooter->isShooterMotorStalled()); } dont = 0; } }