#include
#include // instantiated as ArxRobot at end of class header
#include
#include // I2C support
//#define MCP4017_ADDRESS 0x2F

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//servodriver is used for the 4 motors and 2 servos on the pcb shield
ArxRobot ArxRobot; // instantiated as ArxRobot at end of class header

Servo servoA; // Instantiate new Servo object
Servo servoB;
Servo servoC;
Servo servoD;

#define SERVO_C 22
#define SERVO_D 17
//these are the two new commands we are adding, move is an old command that we are changing

#define BLINK 0x40
#define SERVO 0x41

Motor motorA; //create motor A
Motor motorB; //create motor B

const uint8_t CMD_LIST_SIZE = 3; // we are adding 3 commands (MOVE, BLINK, SERVO)
/*
* User Defined Command BLINK (0x40) Example
* A5 01 40 E4
*/
void blinkHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
Serial1.write(cmd); // blink command = 0x40
Serial1.write(n); // number of param = 0
for (int i=0;i
{
// Serial.write (param[i]);
}
} // blinkHandler

/*
* Override MOVE (0x01) Command Example
* A5 05 01 01 80 01 80 A1
*/
void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
// Serial.write(cmd); // move command = 0x01
// Serial.write(n); // number of param = 4
/* for (int i=0;i
{
Serial.write (param[i]);
}*/
motorA.go(param[0],param[1]); //param[0]=direction: forward(0x01) backwards(0x02) brake(0x03) neutral(0x04)
//param[1]=speed(0-255)
motorB.go(param[2],param[3]);

//The motors controlled by the pcb shield are controlled by uint16_t so we need to map our 8bit pwm signal to 16bits and then send it to the motors
uint16_t pwmLeft = map(param[1],0,255,0,4096);
uint16_t pwmRight = map(param[3],0,255,0,4096);

//left side
if (param[0]==0x01){
forwardLeft(pwmLeft);
}
if (param[0]==0x02){
backwardLeft(pwmLeft);
}
if (param[0]==0x04){
neutralLeft();
}
//right side
if (param[2]==0x01){
forwardRight(pwmRight);
}
if (param[2]==0x02){
backwardRight(pwmRight);
}
if (param[2]==0x04){
neutralRight();
}
} // moveHandler

void forwardLeft(uint16_t pwmSet){
// Drive each PWM in a ‘wave’
//pwm is set out of 4096
//Motor J5
pwm.setPWM(6, 0, pwmSet); //PWMA
pwm.setPWM(7,0,4096); //AIN1
pwm.setPWM(8,4096,0); //AIN2
//Motor J4
pwm.setPWM(5, 0, pwmSet); //PWMB
pwm.setPWM(4,4096,0); //BIN1
pwm.setPWM(3,0,4096); //BIN2
}
void forwardRight(uint16_t pwmSet){
//Motor J6
pwm.setPWM(11, 0, pwmSet); //PWMB
pwm.setPWM(10,4096,0); //BIN1
pwm.setPWM(9,0,4096); //BIN2
//Motor J3
pwm.setPWM(0, 0, pwmSet); //PWMB
pwm.setPWM(1,0,4096); //BIN1
pwm.setPWM(2,4096,0); //BIN2

}

void backwardLeft(uint16_t pwmSet){
// Drive each PWM in a ‘wave’
//pwm is set out of 4096
//Motor J5
pwm.setPWM(6, 0, pwmSet); //PWMA
pwm.setPWM(8,0,4096); //AIN1
pwm.setPWM(7,4096,0); //AIN2

//Motor J4
pwm.setPWM(5, 0, pwmSet); //PWMB
pwm.setPWM(4,0,4096); //BIN1
pwm.setPWM(3,4096,0); //BIN2

}
void backwardRight(uint16_t pwmSet){
//Motor J6
pwm.setPWM(11, 0, pwmSet); //PWMB
pwm.setPWM(9,4096,0); //BIN1
pwm.setPWM(10,0,4096); //BIN2
//Motor J3
pwm.setPWM(0, 0, pwmSet); //PWMB
pwm.setPWM(1,4096,0); //BIN1
pwm.setPWM(2,0,4096); //BIN2

}

void neutralLeft(){
// Drive each PWM in a ‘wave’
//pwm is set out of 4096
//Motor J5
pwm.setPWM(7,0,4096); //AIN1
pwm.setPWM(8,0,4096); //AIN2

//Motor J4
pwm.setPWM(4,0,4096); //BIN1
pwm.setPWM(3,0,4096); //BIN2
}
void neutralRight(){
//Motor J3
pwm.setPWM(1,0,4096); //BIN1
pwm.setPWM(2,0,4096); //BIN2
//Motor J6
pwm.setPWM(9,0,4096); //BIN1
pwm.setPWM(10,0,4096); //BIN2

}
/*
* User Defined Command SERVO (0x41) Example
* Rotate servo to 90 degrees
* A5 02 41 90 76
*/
void servoHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
/*//Serial.write(cmd); // servo command = 0x41
//Serial.write(n); // number of param = 1
for (int i=0;i
{
// Serial.write (param[i]);
}*/
uint16_t servoPos=map(param[0],0,255,1000,2000);
servoA.writeMicroseconds(servoPos);
servoB.writeMicroseconds(servoPos);
servoC.writeMicroseconds(servoPos);
servoD.writeMicroseconds(servoPos);
//1000 and 2000 are the min and max for the servo positions in the writeMicroseconds command

return false; //return false since we are not intercepting a predefined command
} // servoHandler

/*
* Command Example
* Step 2: Register commands by linking IDs to their corresponding command handlers
* In our example when the MOVE command is intercepted the moveHandler is to be run.
* In a similar fashion the BLINK command calls the blinkHandler and SERVO the
* servoHandler.
*/

ArxRobot::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{MOVE,moveHandler}, {BLINK,blinkHandler}, {SERVO,servoHandler}};

/*
* Telemetry Example
* Step 1: Instantiate packet
* In our example we simulate a current sensor wired to MOTOR 2. MOTOR2_CURRENT_ID
* is defined as 0x02 in Configure.h
* To simulate the data stream coming from the sensor we will read ATmega32U4
* Register OCR4D which controls the duty cycle of MOTOR 2.
*/
Packet motorPWM(MOTOR2_CURRENT_ID); // initialize the packet properties to default values

void setup()
{
Serial.begin(9600); // default = 115200
ArxRobot.begin();
ArxRobot.setCurrentLimit(64); //set current limit to ~= 500mA per motor

/*
* Command Example
* Step 3: Tell 3DoT Robot software about new commands
*/
ArxRobot.setOnCommand(onCommand, CMD_LIST_SIZE);

/* Telemetry Example
* Step 2: Modify default values assigned to internal properties as needed.
* Before a packet is created and sent, it is qualified. Specifically,
* the data in a packet must change by some amount from the previous
* packet and may not be sent with at a period less than some value.
* In most cases you can leave these values at their default values.
*/
motorPWM.setAccuracy(1); // change sensor accuracy from +/-2 DN to +/-1 DN (– this line is optional –)
motorPWM.setSamplePeriod(500); // change sample period from 1 second to 0.5 seconds (– this line is optional –)
motorA.begin(AIN1,AIN2,PWMA);
motorB.begin(BIN1,BIN2,PWMB);

//our new code for the other 4 motors. From pwmtest on adafruit
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pwm.begin();
// In theory the internal oscillator is 25MHz but it really isn’t
// that precise. You can ‘calibrate’ by tweaking this number till
// you get the frequency you’re expecting!
pwm.setOscillatorFrequency(27000000); // The int.osc. is closer to 27MHz
pwm.setPWMFreq(1600); // This is the maximum PWM frequency

//run the servos
servoA.attach(SERVO_A);
servoB.attach(SERVO_B);
servoC.attach(SERVO_C);
servoD.attach(SERVO_D);

// if you want to really speed stuff up, you can go into ‘fast 400khz I2C’ mode
// some i2c devices dont like this so much so if you’re sharing the bus, watch
// out for this!
Wire.setClock(400000);

}

void loop()
{
ArxRobot.loop();
}