void initMotors(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);
}
}
// intialize io objects
void initialize_pins(){
left_motor = InitializeServoMotor(left_motor_pin, false);
right_motor = InitializeServoMotor(right_motor_pin, true);
//in_motor = InitializeServoMotor(in_motor_pin, true);
//brush_motor = InitializeServoMotor(brush_motor_pin, true);
//release_servo = InitializeServo(release_servo_pin);
side_ir_sensor = InitializeADC(side_ir_sensor_pin);
front_ir_sensor = InitializeADC(front_ir_sensor_pin);
goal_ir_sensor = InitializeADC(goal_ir_sensor_pin);
}
//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
);
}
}
void initMotors(void) {
// Left
Motors[0] = InitializeServoMotor(PIN_B6, false);
// Right
Motors[1] = InitializeServoMotor(PIN_B7, true);
}
void initMotor(void) {
leftMotor = InitializeServoMotor(PIN_C4,false); //left motor
rightMotor = InitializeServoMotor(PIN_C5,true); //right motor
}
