void setup()
{
Serial.begin(9600);
Serial.setTimeout(100);
pinMode(pin_motor_in, INPUT_PULLUP);
pinMode(pin_servo_in, INPUT_PULLUP);
pinMode(pin_button_in, INPUT_PULLUP);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_motor_in), motor_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_servo_in), servo_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_button_in), button_interrupt, CHANGE);
servo.attach(pin_servo_out);
motor.attach(pin_motor_out);
}
void setup()
{
Serial.begin(9600);
Serial.print("setup()");
pinMode(ledPin, OUTPUT);
pinMode(pinMotorIn, INPUT_PULLUP);
pinMode(pinServoIn, INPUT_PULLUP);
pinMode(pinButtonIn, INPUT_PULLUP);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinMotorIn), motor_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinServoIn), servo_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinButtonIn), button_interrupt, CHANGE);
servo.attach(servoPin);
motor.attach(motorPin);
}
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
//Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
send_status(MPU_INITIALIZE, STATUS_OK);
// verify connection
send_status(MPU_CONNECTION, mpu.testConnection() ? STATUS_OK : STATUS_FAIL);
// load and configure the DMP
// 0 = DMP OK
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
ua_dev_status = mpu.dmpInitialize();
send_status(DMP_INITIALIZE, ua_dev_status);
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(120);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-185);
mpu.setZAccelOffset(1688); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (ua_dev_status == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachPinChangeInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
ua_mpu_interrupt_status = mpu.getIntStatus();
send_status(DMP_INTERRUPT, ua_mpu_interrupt_status);
b_dmp_ready = true;
// get expected DMP packet size for later comparison
uh_packet_size = mpu.dmpGetFIFOPacketSize();
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
void motor_servo_setup()
{
// Establishing serial communication with host system. The best values for these parameters
// is an open question. At 9600 baud rate, data can be missed.
//Serial.begin(115200);
Serial.setTimeout(5);
// Setting up three input pins
pinMode(PIN_BUTTON_IN, INPUT_PULLUP);
pinMode(PIN_SERVO_IN, INPUT_PULLUP);
pinMode(PIN_MOTOR_IN, INPUT_PULLUP);
// LED out
pinMode(PIN_LED_OUT, OUTPUT);
// Attach interrupt service routines to pins. A change in signal triggers interrupts.
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_BUTTON_IN),
button_interrupt_service_routine, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_SERVO_IN),
servo_interrupt_service_routine, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_MOTOR_IN),
motor_interrupt_service_routine, CHANGE);
// Attach output pins to ESC (motor) and steering servo.
servo.attach(PIN_SERVO_OUT);
motor.attach(PIN_MOTOR_OUT);
}
