void setup() { Wire.begin(); encoderArm.init(1, MOTOR_393_TIME_DELTA); encoderArm.zero(); armMotor.attach(3,1000,2000); driveL.attach(4,1000,2000); driveR.attach(5,1000,2000); gripper.attach(19,600,2400); time = micros(); upTimer = time; Serial.begin(115200); if(mpuEnabled) setupMPU6050(); }
void initAll(void) { initUART(); setupMPU6050(I2C1, FALSE); // I2C bus, and AD0 logic state. mPORTFSetPinsDigitalOut(BIT_0); // LED5. mPORTGSetPinsDigitalOut(BIT_6); // LED4. }