void rotate_time(int degree) {
SetMotorPowers(80,-80);
WaitUS(6874*degree);
SetMotorPowers(0,0);
angle += degree;
return;
}
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 ReadSwitch_Debounce ( switch_t switc )
{
int result1 = 2, result2 = 3, result3 = 4, readings = 0;
/* Get three readings */
while (!((result1 == result2) && (result2 == result3))) {
result1 = (int) GPIOPinRead(GPIO_PORTA_BASE, switc);
WaitUS(SWITCH_DEBOUNCE_TIME);
result2 = (int) GPIOPinRead(GPIO_PORTA_BASE, switc);
WaitUS(SWITCH_DEBOUNCE_TIME);
result3 = (int) GPIOPinRead(GPIO_PORTA_BASE, switc);
/* Break out of loop if stuck */
if (readings == MAX_SWITCH_DEBOUNCE_READINGS) {
UARTprintf("\nError: Switch won't debounce\n");
return -1;
}
readings++;
}
return result1;
}
void rotate_enc(int degree) {
int ldiff = 0, rdiff = 0;
long l = 0, r = 0, count = 0;//oldl = 0, oldr = 0,
long break_val = (BOT_WIDTH * ABS(degree)) * (epr/(WHEEL_RAD*180));
PresetEncoderCounts(0,0);
if (degree > 0) SetMotorPowers(80,-80);
else SetMotorPowers(-80,80);
while (count * 4 < break_val)
{
GetEncoderCounts(&l, &r);
ldiff = (l>r)?0:r-l;
rdiff = (r>l)?0:l-r;
SetMotorPowers(127-ldiff,-128+rdiff);
count = (ABS(l)+ABS(r))/2;
UARTprintf("Count: %10d %10d\r",count,break_val);
//oldl = l; oldr = r;
WaitUS(1000);
}
}
int main(void) {
LockoutProtection();
InitializeMCU();
//init uart
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTStdioInit(0);
LED_Init();
Jumper_Init();
ADC_Init();
Sonar_Init();
usemotors = Jumper_Value & 0x8;
if ((Jumper_Value & 0x7) == 0x1) {
if (usemotors) {
Motor_Init(false,true);
Motor_Set(127,127);
}
avoid_sonar(0);
avoid_ir(filtered_ir);
for (;;);
}
if ((Jumper_Value & 0x7) == 0x2) {
Travel_Init(usemotors);
Travel_Go(FULL_SPEED);
for (;;);
}
if ((Jumper_Value & 0x7) == 0x3) {
if (usemotors) {
Motor_Init(false,true);
Motor_Set(127,127);
}
for (;;);
}
//if no jumpers are set, enter debug mode
Encoder_Init(true,false);
for (;;c++) {
ADC_Background_Read(0);
Sonar_Background_Read(0);
Encoder_Background_Read(0);
Jumper_Read();
UARTprintf("ADC[%3d %3d %3d %3d %3d %3d %3d %3d] S[%7d] E[%3d %3d] J[%1x] c:%d\n",
ADC_Values[0],ADC_Values[1],ADC_Values[2],ADC_Values[3],ADC_Values[4],ADC_Values[5],ADC_Values[6],ADC_Values[7],
Sonar_Value,
Encoder_Values[0],Encoder_Values[1],
Jumper_Value,
c
);
LED_Set(LED_0,c);
LED_Set(LED_1,c+64);
LED_Set(LED_2,c+128);
LED_Set(LED_3,c+192);
WaitUS(20000);
}
}
void move_time(int distance) {
SetMotorPowers(-110,-127);
WaitUS(distance*500);
SetMotorPowers(0,0);
}
// Summary: waits a specified amount of time
// Parameters:
// ms: the number of milliseconds to wait
// Note: Uses TIMER1. If you use Wait() or WaitUS(), you should not use TIMER1 elsewhere in your code;
// If you use TIMER1 in your code, you should not use Wait() or WaitUS()
void Wait(unsigned long ms)
{
//PRINT_U(ms); TAB; PRINT_U(ms*1000L); NL;
WaitUS(ms*1000L);
}