//i2c_3t test with Adafruit Motor Shield v2
#include <SPI.h>
//#include <Wire.h>
#include <i2c_t3.h> //Add in future to increase bus speed
#include <Adafruit_MotorShield.h>
#include <PID_v2.h>
#include <stdint.h>
#include "TeensyTimerTool.h"
using namespace TeensyTimerTool;
//TwoWire *_i2c;
i2c_t3 *_i2c;
//Inputs
PeriodicTimer t1(TCK);
PeriodicTimer t2(TCK);
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// And connect a DC motor to port M1
Adafruit_DCMotor *needleMotor = AFMS.getMotor(1);
Adafruit_DCMotor *wireMotor = AFMS.getMotor(2);
Adafruit_DCMotor *catheterMotor = AFMS.getMotor(3);
//end calibration values
//////////////////////////////////////////////
//*****************************************************
void setup()
//*****************************************************
{
//_i2c = &Wire;
//_i2c->begin();
Wire.begin();
Wire.setClock(400000);
//Setup for Master mode, pins 18/19, external pullups, 400kHz, 200ms default timeout
//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
//Wire.setDefaultTimeout(200000); // 200ms
Serial.begin(115200); // set up Serial library at 9600 bps
Serial.print("System Initializing");
Serial.print("\r\n");
AFMS.begin(); // create with the default frequency 1.6KHz
//AFMS.begin(1000); // OR with a different frequency, say 1KHz
//t1.begin(encoderLoop, 2_kHz);
//t2.begin(dataLoop, 1_kHz);
}
//*****************************************************
void loop()
//*****************************************************
{
needleMotor->run(FORWARD);
needleMotor->setSpeed(100);
delay(100);
}//end loop