void InitBot(void) { //Call initialization functions. LockoutProtection(); InitializeMCU(); initUART(); InitializeEncoders(true,false); PresetEncoderCounts(0,0); InitializeMotors(true,true); InitializeServos(); InitializeLineSensor(); SetDischargeTime(350); //Modified for 4 ADC Ports (assuming all are IR). initADCPorts(); //Initialize state variables. angle = 90; shooting = false; rotate = rotate_time; move = move_time; IRtrig = false; lose_obj = false; //Pin PA3 is James. 0xff is off; 0x00 is on. GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_3); GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_3, 0xff); //Pin PA2 is the trigger; set the pin to perform weak pull-up (not open-drain). SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); GPIOPinTypeGPIOInput(GPIO_PORTA_BASE, GPIO_PIN_2); GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_2,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU); //Setup an interrupt on pin PA2 (flipPancake). IntEnable(INT_GPIOA); GPIOIntTypeSet(GPIO_PORTA_BASE, GPIO_PIN_2,GPIO_FALLING_EDGE); GPIOPinIntEnable(GPIO_PORTA_BASE, GPIO_PIN_2); }
int main(void){ char i; unsigned char data[16]; short wiichuck[7], xinit=0, yinit=0, l_vel, r_vel; int xpow, ypow; LockoutProtection(); InitializeMCU(); InitializeUART(); InitializeI2C(); InitializeServos(); SetServoPosition(SERVO_0, 140); InitializeMotors(true, false); InitializeEncoders(true, false); // UARTprintf("Initializing Nunchuck\n\n"); // I2CSend(0x52<<1, 2, 0x40, 0x00); // Wait(25); init_nunchuck(); // Wireless Nunchucks Zero @ 128 xinit = yinit = 128; while(1){ //Start Recalculating Values Wait(1); I2CSend(0x52<<1, 1, 0x00); Wait(1); I2CSend(0x52<<1, 1, 0x00); Wait(1); I2CSend(0x52<<1, 1, 0x00); if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){ UARTprintf("Send Zero Error:\n"); switch(I2CMasterErr(I2C0_MASTER_BASE)){ case I2C_MASTER_ERR_ADDR_ACK: UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n"); break; case I2C_MASTER_ERR_DATA_ACK: UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n"); break; case I2C_MASTER_ERR_ARB_LOST: UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n"); break; default: UARTprintf("WTF: %d\n", I2CMasterErr(I2C0_MASTER_BASE)); } // Reinitialize Nunchuck on error init_nunchuck(); }else{ Wait(1); I2CRecieve(0x52<<1, data, 6); // Nunchuck data is 6 bytes, but for whatever reason, MEMOREX Wireless Nunchuck wants to send 8... if (I2CMasterErr(I2C0_MASTER_BASE) != I2C_MASTER_ERR_NONE){ UARTprintf("Send Zero Error:\n"); switch(I2CMasterErr(I2C0_MASTER_BASE)){ case I2C_MASTER_ERR_ADDR_ACK: UARTprintf(" I2C_MASTER_ERR_ADDR_ACK\n"); break; case I2C_MASTER_ERR_DATA_ACK: UARTprintf(" I2C_MASTER_ERR_DATA_ACK\n"); break; case I2C_MASTER_ERR_ARB_LOST: UARTprintf(" I2C_MASTER_ERR_ARB_LOST\n"); break; } // Reinitialize Nunchuck on error init_nunchuck(); }else{ //for(i=0; i<6; i++) // data[i] = (data[i] ^ 0x17) + 0x17; // Nintendo decided to encrypt thir data... // Save Joystick Data wiichuck[0] = data[1]; // X Axis Joystick wiichuck[1] = data[0]; // Y Axis Joystick wiichuck[2] = (((unsigned short) data[2]) << 2) + (((unsigned short) data[5]) & (3<<2)); // X Axis Accel wiichuck[3] = (((unsigned short) data[3]) << 2) + (((unsigned short) data[5]) & (3<<4)); // Y Axis Accel wiichuck[4] = (((unsigned short) data[4]) << 2) + (((unsigned short) data[5]) & (3<<6)); // Z Axis Accel wiichuck[5] = data[5] & (1 << 1) ? 0 : 1; //'C' Button wiichuck[6] = data[5] & (1 << 0) ? 0 : 1; //'Z' Button //if (xinit == 0 && yinit == 0){ // xinit = wiichuck[0]-127; // yinit = wiichuck[1]-127; //}else{ xpow = (wiichuck[0]-xinit)/2; ypow = (wiichuck[1]-yinit)/2; l_vel = (xpow - ypow)*2; r_vel = (xpow + ypow)*2; l_vel = l_vel > 127 ? 127 : l_vel; r_vel = r_vel > 127 ? 127 : r_vel; l_vel = l_vel < -127 ? -127 : l_vel; r_vel = r_vel < -127 ? -127 : r_vel; //UARTprintf("X: %d\tY: %d\n", xpow*2, ypow*2); SetMotorPowers(l_vel / (wiichuck[5]==0 ? 2 : 1), r_vel / (wiichuck[5]==0 ? 2 : 1)); UARTprintf("Motor L: %d\tMotor R: %d\n", l_vel, r_vel); SetServoPosition(SERVO_0, wiichuck[6]==1 ? 255 : 140); UARTprintf("Nunchuck Data:\n"); for(i=0; i<7; i++){ UARTprintf(" %d\n", wiichuck[i]); }NL; Wait(100); } } } }
//Here, the distinctions between the autonomous personality and driver personality is defined. task main() { InitializeLCDScreen(); InitializePIDControllers(); InitializeDebugStream(); InitializeTimers(); InitializeEncoders(); SenAutoPot = SensorValue(AutoSelector); UpdateAutonomousRoutine(); while(true) { Input(); if(!IsRobotDisabled) //Simulation Allowed but Competiton still works if defined { if (IsRobotInAutonomousMode || IsRobotInVirtualAutonomousMode || (DriverMode != DriverJoystickControl && DriverMode != DriverMotorTest)) { AutonomousControl(); UpdateScreen(DispAutonomousMode); #ifdef LOGENCODERS LogEncoders(); #endif if (!IsRobotInAutonomousMode) if (!JoystickCheck(true)) IsRobotInVirtualAutonomousMode = false; } else { AutonomousStep=0; UpdateAutonomousRoutine(); UpdateScreen(DispDriverMode); if (DriverMode == DriverJoystickControl) { AutonomousReset(Finish); SubroutineCheck(); DriveControl(); IntakeControl(); LiftControl(); DescorerControl(); } else if (DriverMode == DriverMotorTest) { if (!JoystickCheck(false)) DriverMode = DriverJoystickControl; } } } else { AutonomousStep=0; AutonomousReset(Finish); UpdateAutonomousRoutine(); UpdateScreen(DispDisabledMode); } Output(); MainNonDelayedLoopTime = time1[T4]; while (time1[T4] < MinLoopMS) {} MainLoopTime = time1[T4]; ClearTimer(T4); } }
//-----------------------Private functions-----------------------------// int main(void) { uint16_t i; for( i = 0; i < 64000; i++) asm("nop"); InitializeClock(); InitializeSysTick(); #ifdef _USE_DISPLAY InitializeDisplay(); #endif #ifdef _USE_MOTORS InitializeMotors(); #endif #ifdef _USE_ADC_BATTERY InitializeBattery(); #endif #ifdef _USE_ADC_SHARP InitializeSharp(); #endif #ifdef _USE_ENCODERS InitializeEncoders(); #endif #ifdef _USE_SERVOS InitializeServos(); #endif #ifdef _USE_LED_NUCLEO InitializeLedNucleo(); #endif #ifdef _USE_LED_14 InitializeLed14(); #endif #ifdef _USE_LED_EYE InitializeLedEye(); #endif #ifdef _USE_BT InitializeBT(); #endif #ifdef _USE_WIFI InitializeWifi(); #endif #ifdef _USE_MPU InitializeMPU(); #endif #if 0 if(//MPU error)) { //sth bad had happened... } #endif oMotors.SetSpeedLeft( 0.0f ); oMotors.SetSpeedRight( 0.0f ); oServos.SetAngleArmLeft( 0.0f ); oServos.SetAngleArmRight( 0.0f ); oServos.SetAngleCamVer( 0.0f ); oServos.SetAngleCamHor( -15.0f ); while (1) { } return 0; }
void initEncoders(void) { InitializeEncoders(false, true); }