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);
}
