void servoDemo(void) {
UARTprintf("Press:\n a-'up' 10\n w-'up' 1\n s-'down' 1\n");
UARTprintf(" d-'down' 10\n enter-quit\n");
{
unsigned char position = 0;
char newline = 13;
char ch;
UARTprintf("position: %u ",position);
ch = getc();
while(ch != newline) {
if (ch == 'w')
position++;
else if (ch == 's')
position--;
else if (ch == 'a')
position+=10;
else if (ch == 'd')
position-=10;
SetServoPosition(SERVO_0, position);
SetServoPosition(SERVO_1, position);
SetServoPosition(SERVO_2, position);
SetServoPosition(SERVO_3, position);
UARTprintf("%u ",position);
ch = getc();
}
UARTprintf("\n");
}
}
// Summary: Initializes the appropriate PWMs for servo output
// Note: Always call this function before any other servo-related functions
void InitializeServos(void)
{
unsigned long ulServoPeriod;
//
// Enable the peripherals used by the servos.
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); // servos 0 & 1
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); // servos 2 & 3
//
// Set GPIO B0, B1, D0, and D1 as PWM pins.
// They are used to output the PWM0, PWM2, PWM3, and PWM3 signals.
//
GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0 | GPIO_PIN_1);
GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//
// Compute the PWM period based on the system clock.
//
ulServoPeriod = g_ulPWMTicksPerSecond / 50;
//
// Set the PWM period to 50 Hz = 20ms.
//
PWMGenConfigure(PWM_BASE, PWM_GEN_0,
PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM_BASE, PWM_GEN_0, ulServoPeriod);
PWMGenConfigure(PWM_BASE, PWM_GEN_1,
PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM_BASE, PWM_GEN_1, ulServoPeriod);
us600 = ulServoPeriod * 3 / 100; // 20 ms * 3 / 100 = 600 us
us2400 = us600 * 4; // 600 us * 4 = 2400 us
//
// Enable the PWM0, PWM1, PWM2, and PWM3 output signals.
//
PWMOutputState(PWM_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT | PWM_OUT_2_BIT | PWM_OUT_3_BIT, true);
//
// Enable the PWM generator.
//
PWMGenEnable(PWM_BASE, PWM_GEN_0);
PWMGenEnable(PWM_BASE, PWM_GEN_1);
// Default to center
SetServoPosition(SERVO_0, SERVO_NEUTRAL_POSITION);
SetServoPosition(SERVO_1, SERVO_NEUTRAL_POSITION);
SetServoPosition(SERVO_2, SERVO_NEUTRAL_POSITION);
SetServoPosition(SERVO_3, SERVO_NEUTRAL_POSITION);
}
void flipPancake(void) {
GPIOPinIntDisable(GPIO_PORTA_BASE, GPIO_PIN_2);
WaitUS(2000);
GPIOPinIntClear(GPIO_PORTA_BASE, GPIO_PIN_2);
if(!GPIOPinRead(GPIO_PORTA_BASE,GPIO_PIN_2))
{
UARTprintf("Triggered.\n");
SetServoPosition(PANCAKE_POSITION,100*pancake);
pancake = !pancake;
}
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);
}
}
}
}
/* ******************************************************************** */
void LowerBay ( void )
{
SetServoPosition(SERVO_PORT, SERVO_LOAD_POS);
}
/* ******************************************************************** */
void RaiseBay ( void )
{
SetServoPosition(SERVO_PORT, SERVO_DUMP_POS);
}
