void InitializeUCSlave(void){
InitializeUART();
InitializeTime();
InitializeServo();
InitializeWatchdog();
InitializeVelocityControl();
//InitializeIMU();
EnableSensorFeedbackMessages = 0;
}
//Initializes motors and IR Sebsors
void initMotorsSensors(void) {
if (!initialized) {
initialized = true;
Motors[0] = InitializeServoMotor(PIN_B6, false);
Motors[1] = InitializeServoMotor(PIN_B7, false);
Motors[2] = InitializeServoMotor(PIN_C4, false);
Motors[3] = InitializeServoMotor(PIN_C5, false);
marservo = InitializeServo(PIN_B2);
pingservo = InitializeServo(PIN_F3);
SetServo(marservo,0.1f);
SetServo(pingservo,0.1f);
GPIOPadConfigSet(PORT_VAL(PIN_B6), PIN_VAL(PIN_B6), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_B7), PIN_VAL(PIN_B7), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_C4), PIN_VAL(PIN_C4), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
GPIOPadConfigSet(PORT_VAL(PIN_C5), PIN_VAL(PIN_C5), GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
adc[0] = InitializeADC(PIN_D0);
adc[1] = InitializeADC(PIN_D1);
adc[2] = InitializeADC(PIN_D2);
adc[3] = InitializeADC(PIN_D3);
turn=0;
gls = InitializeGPIOLineSensor(
PIN_B5,
PIN_B0,
PIN_B1,
PIN_E4,
PIN_E5,
PIN_B4,
PIN_A5,
PIN_A6
);
}
}
int main(void) {
//char input;
PIDStruct s;
CallEvery(blink, 0, 0.5);
//armServo = InitializeServo(PIN_B2);
handServo = InitializeServo(PIN_B3);
InitGearMotor();
initializeIRSensors();
initBootlegMotor();
//grabBall();
//Wait(3.0);
//dropBall();
//while(1);
//while(1) __asm("");
//SetGearMotor(.5, .5);
//setBootlegMotor(0.7);
grabBall();
dropBall();
while(true)
{
//grabBall();
//float hup = getPosition();
//Printf("%f \n", (hup));
// runPID(&s, 0);
//get input somehow?
// if(input == 32) //also needs input to activate sprinting speed
// manualControl();
//setBootlegMotor(0.5);
//Wait(2.0);
//setBootlegMotor(1.0);
}
}
void initServo(void) {
servo = InitializeServo(PIN_B0);
}