slave-motor-mod2-v5
/*
*
* DAMI-M3
*
* slave-motor-mod2-vx
*
* LIBRARIES
* Wire (Arduino I2C Master)
* AccelStepper
*
*/
/*
O rascunho usa 10562 bytes (73%) do espaço de armazenamento do programa. O máximo é 14336 bytes.
Variáveis globais usam 619 bytes (60%) de memória dinâmica, restando 405 bytes para variáveis locais.
*/
#define serialSpeed 115200
#ifndef PI2
#define PI2 6.28318530718
#endif
#define LED_PIN 13
int internalLedBlinkDelay = 1000;
// I2C Arduino Slave
#include <Wire.h>
#define I2C_NODE_ADDRESS 8
#include <AccelStepper.h>
// motor one
#define bodyLeftMotorPin1 4 // IN1 on the ULN2003 driver 1
#define bodyLeftMotorPin2 5 // IN2 on the ULN2003 driver 1
#define bodyLeftMotorPin3 6 // IN3 on the ULN2003 driver 1
#define bodyLeftMotorPin4 7 // IN4 on the ULN2003 driver 1
// motor two
#define bodyRightMotorPin1 8 // IN1 on the ULN2003 driver 2
#define bodyRightMotorPin2 9 // IN2 on the ULN2003 driver 2
#define bodyRightMotorPin3 10 // IN3 on the ULN2003 driver 2
#define bodyRightMotorPin4 11 // IN4 on the ULN2003 driver 2
AccelStepper bodyMotorLeft(AccelStepper::HALF4WIRE, bodyLeftMotorPin1, bodyLeftMotorPin3, bodyLeftMotorPin2, bodyLeftMotorPin4);
AccelStepper bodyMotorRight(AccelStepper::HALF4WIRE, bodyRightMotorPin1, bodyRightMotorPin3, bodyRightMotorPin2, bodyRightMotorPin4);
byte bodyMotorWheelRadius;
byte bodyMotorWheelDistance;
int bodyMotorWheelResolution;
int bodyMotorAccelerationBase;
int bodyMotorSpeedBase;
int bodyMotorSpeedTop;
float bodyMotorStepsPerUnit;
signed long bodyMotorUnitsTarget;
signed long bodyMotorStepsTarget;
signed long bodyMotorLeftStepsToGo;
signed long bodyMotorRightStepsToGo;
// speed
float bodyMotorLeftSpeed;
float bodyMotorRightSpeed;
float bodyMotorSpeedMin;
float bodyMotorSpeedMax;
float bodyMotorAcceleration;
// move information structure
typedef struct {
byte id;
byte cmd;
signed long units; // distance in mm/cm (depends on resolution), angles on degrees
int minSpeed;
int acceleration;
int maxSpeed;
} bodyMoveCmdType;
bodyMoveCmdType bodyMoveCmd = {0, 0, 0, 0, 0, 0};
byte bodyMoveProcessStatus = 0;
unsigned long bodyMoveLoopCounter = 0; // TODO: does I realy need this
// I2C
bodyMoveCmdType bodyMoveCmdRequest;
byte requestEventInfoType = 0;
byte receiveEventType = 0;
boolean bodyMoveCmdRequestWait = false;
byte bodyMoveCmdRecId = 0;
byte bodyMoveCmdEndId = 0;
// I2C byte conversion
unsigned long loopTimeStart = 0;
byte loopTime = 0;
// reset function
void (* resetFunc) (void) = 0;
void setup() {
pinMode(LED_PIN, OUTPUT); // configure internal LED for output
Serial.begin(serialSpeed);
//while (!Serial);
// I2C Arduino Slave
Wire.begin(I2C_NODE_ADDRESS); // join i2c bus with address #8
Wire.onReceive(receiveEvent); // register event
Wire.onRequest(requestEvent); // register event
loopTimeStart = millis();
}
void loop() {
static boolean internalLedBlinkState = false;
static unsigned long internalLedTimeStart = millis() + internalLedBlinkDelay;
bodyMoveProcess();
// blink LED to indicate activity
if(millis() >= internalLedTimeStart) {
internalLedTimeStart = millis() + internalLedBlinkDelay;
internalLedBlinkState = !internalLedBlinkState;
digitalWrite(LED_PIN, internalLedBlinkState);
}
// end loop
loopTime = millis() - loopTimeStart;
loopTimeStart = millis();
}
void bodyMoveProcess() {
static unsigned long outputTimeStart = millis() + 250;
bodyMoveCheck();
if(bodyMoveProcessStatus) {
// dynamics
bodyMove();
// odometry
bodyMotorLeftSpeed = bodyMotorLeft.speed();
bodyMotorRightSpeed = bodyMotorRight.speed();
bodyMotorLeftStepsToGo = bodyMotorLeft.distanceToGo();
bodyMotorRightStepsToGo = bodyMotorRight.distanceToGo();
bodyMoveLoopCounter++;
}
/*
// info
if(millis() >= outputTimeStart) {
outputTimeStart = millis() + 250;
Serial.print(bodyMotorStepsTarget); Serial.print("\t");
Serial.print(bodyMotorLeftStepsToGo); Serial.print("\t");
Serial.print(bodyMotorRightStepsToGo); Serial.print("\t");
Serial.print(bodyMoveCmdRecId); Serial.print("\t");
Serial.print(bodyMoveCmd.id); Serial.print("\t");
Serial.print(bodyMoveCmd.cmd); Serial.print("\t");
Serial.print(bodyMoveCmd.units); Serial.print("\t");
Serial.print(bodyMoveProcessStatus); Serial.print("\t");
Serial.print(bodyMoveLoopCounter); Serial.print("\t");
Serial.print(requestEventInfoType); Serial.print("\t");
Serial.print(loopTime); Serial.print("\t");
Serial.println("");
}
*/
}
void bodyMoveCheck() {
// process imediate stop on ALL loops
if(bodyMoveCmdRequestWait && bodyMoveCmdRequest.cmd == 1) {
// Stop Request
// prepare the ic2 reply
bodyMoveCmdEndId = bodyMoveCmd.id;
requestEventInfoType = 1;
// load move command
setBodyMoveCmd();
// reset request (only the bodyMoveCmdRequest.id)
bodyMoveCmdRequest.id = 0;
bodyMoveCmdRequestWait = false;
receiveEventType = 1;
// allow processing imediate stop commmand
bodyMoveProcessStatus = 0;
}
if(!bodyMoveProcessStatus) {
//
// run ONLY on first loop
//
// add single command to begin of sequence
if(bodyMoveCmdRequestWait) {
// load move command
setBodyMoveCmd();
// reset request (only the bodyMoveCmdRequest.id)
bodyMoveCmdRequest.id = 0;
/*
bodyMoveCmdRequest.cmd = 0;
bodyMoveCmdRequest.units = 0;
bodyMoveCmdRequest.minSpeed = 0;
bodyMoveCmdRequest.acceleration = 0;
bodyMoveCmdRequest.maxSpeed = 0;
*/
bodyMoveCmdRequestWait = false;
receiveEventType = 2;
}
bodyMoveStart();
} else {
//
// run on ALL but NOT on first loop
//
if (bodyMotorLeftStepsToGo == 0 && bodyMotorRightStepsToGo == 0) {
// target reached
bodyMoveEnd();
}
}
}
void bodyMoveStart() {
int minSpeed = 0;
float acceleration;
int maxSpeed = 0;
if(bodyMoveCmd.cmd > 0) {
// control led feedback
internalLedBlinkDelay = 250;
// start, target, speeds and acceleration setup
minSpeed = bodyMoveCmd.minSpeed;
if(!minSpeed) minSpeed = bodyMotorSpeedBase;
acceleration = bodyMoveCmd.acceleration;
if(!acceleration) acceleration = bodyMotorAccelerationBase;
maxSpeed = bodyMoveCmd.maxSpeed;
if(!maxSpeed) maxSpeed = bodyMotorSpeedTop;
// does not allow target speed bellow minimum speed
if(maxSpeed < minSpeed) maxSpeed = minSpeed;
// speedChange speeds
bodyMotorSpeedMin = minSpeed;
bodyMotorSpeedMax = maxSpeed;
setBodySpeed (minSpeed);
setBodyAcceleration(acceleration);
setBodyMaxSpeed(maxSpeed);
// reset position
// must be done after set speed and accel
// not before that
bodyMotorLeft.setCurrentPosition(0);
bodyMotorRight.setCurrentPosition(0);
// reset some vars
bodyMoveProcessStatus = 1;
//Serial.println("STARTMOVE");
// lets prepare to start moving
switch(bodyMoveCmd.cmd) {
case 1:
bodyStop();
break;
case 2:
bodyRunStart(bodyMoveCmd.units);
break;
case 3:
bodyRotateStart(bodyMoveCmd.units);
break;
}
}
}
void bodyMoveEnd() {
int i;
//Serial.println("ENDMOVE");
// control led feedback
internalLedBlinkDelay = 1000;
// prepare the ic2 reply
bodyMoveCmdEndId = bodyMoveCmd.id;
switch(receiveEventType) {
case 1:
receiveEventType = 0;
requestEventInfoType = 1;
case 2:
receiveEventType = 0;
requestEventInfoType = 2;
}
// reset current command data
bodyMoveCmd.id = 0;
bodyMoveCmd.cmd = 0;
bodyMoveCmd.units = 0;
bodyMoveCmd.minSpeed = 0;
bodyMoveCmd.acceleration = 0;
bodyMoveCmd.maxSpeed = 0;
// reset targets
bodyMotorUnitsTarget = 0;
bodyMotorStepsTarget = 0;
// allow new command processing
bodyMoveLoopCounter = 0;
bodyMoveProcessStatus = 0;
}
void bodyRunStart(float units) {
signed long steps;
bodyMotorUnitsTarget = units;
steps = unitsToSteps(units);
bodyMotorStepsTarget = steps;
bodyMotorLeftStepsToGo = bodyMotorRightStepsToGo = bodyMotorStepsTarget;
bodyMotorLeft.moveTo(steps);
//bodyMotorRight.move(steps);
bodyMotorRight.moveTo(-steps);
}
void bodyRotateStart(float deg) {
signed long steps;
bodyMotorUnitsTarget = ((deg * PI) / 180) * (bodyMotorWheelDistance / 2);
steps = unitsToSteps(bodyMotorUnitsTarget);
bodyMotorStepsTarget = steps;
bodyMotorLeftStepsToGo = bodyMotorRightStepsToGo = bodyMotorStepsTarget;
// if(deg > 0) {} else {}
// need to use symetric value to do a
// positive rotation (anti clock direction)
bodyMotorLeft.moveTo(-steps);
bodyMotorRight.moveTo(-steps);
}
void bodyStop() {
bodyMotorLeft.stop();
bodyMotorRight.stop();
bodyMotorStepsTarget = bodyMotorLeft.targetPosition();
bodyMotorLeftStepsToGo = bodyMotorRightStepsToGo = bodyMotorStepsTarget;
bodyMotorUnitsTarget = stepsToUnits(bodyMotorStepsTarget);
}
void bodyMove() {
if(bodyMotorLeft.distanceToGo() != 0) bodyMotorLeft.run();
if(bodyMotorRight.distanceToGo() != 0) bodyMotorRight.run();
}
void setBodyMaxSpeed (float speed) {
bodyMotorSpeedMax = speed;
bodyMotorLeft.setMaxSpeed(speed);
bodyMotorRight.setMaxSpeed(speed);
}
void setBodyAcceleration (float acceleration) {
bodyMotorAcceleration = acceleration;
bodyMotorLeft.setAcceleration(acceleration);
bodyMotorRight.setAcceleration(acceleration);
}
void setBodySpeed (float speed) {
bodyMotorLeftSpeed = speed;
bodyMotorRightSpeed = speed;
bodyMotorLeft.setSpeed(speed);
bodyMotorRight.setSpeed(speed);
}
long unitsToSteps(float distance) {
return (long)(distance / bodyMotorStepsPerUnit);
}
long stepsToUnits(float steps) {
return ((long)((float)(steps * bodyMotorStepsPerUnit)));
}
void setBodyMoveCmd() {
bodyMoveCmd.id = bodyMoveCmdRequest.id;
bodyMoveCmd.cmd = bodyMoveCmdRequest.cmd;
bodyMoveCmd.units = bodyMoveCmdRequest.units;
bodyMoveCmd.minSpeed = bodyMoveCmdRequest.minSpeed;
bodyMoveCmd.acceleration = bodyMoveCmdRequest.acceleration;
bodyMoveCmd.maxSpeed = bodyMoveCmdRequest.maxSpeed;
}
// BOF I2C
// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
// respond with message of N bytesas expected by master
// I2C limit write to 32 bytes
void requestEvent() {
double tmp = 0;
switch(requestEventInfoType) {
case 0:
case 1:
case 2:
// prepare
//bodySteeringPidOutputTmp = (signed int) (bodySteeringPidOutput * 1000);
// send
Wire.write((byte*)&requestEventInfoType, 1);
Wire.write((byte*)&bodyMoveProcessStatus, 1);
Wire.write((byte*)&bodyMoveCmd.cmd, 1);
Wire.write((byte*)&bodyMoveCmd.id, 1);
Wire.write((byte*)&bodyMoveCmdRecId, 1);
Wire.write((byte*)&bodyMoveCmdEndId, 1);
Wire.write((byte*)&loopTime, 1);
Wire.write((byte*)&loopTime, 1);
Wire.write((byte*)&bodyMotorStepsTarget, 4);
Wire.write((byte*)&bodyMotorLeftStepsToGo, 4);
Wire.write((byte*)&bodyMotorLeftSpeed, 4);
Wire.write((byte*)&bodyMotorRightStepsToGo, 4);
Wire.write((byte*)&bodyMotorRightSpeed, 4);
Wire.write((byte*)&bodyMoveLoopCounter, 4);
bodyMoveCmdEndId = 0;
bodyMoveCmdRecId = 0;
requestEventInfoType = 0;
break;
}
}
// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
union u_tag {
byte b[4];
int i[2];
long l;
double d;
} i2c_data[3];
byte i2c_byte[32];
int i = 0;
// fill i2c_byte buffer
while (Wire.available()) { // slave may send less than requested
char c = Wire.read(); // receive a byte as character
i2c_byte[i] = c;
i++;
}
// set i2c_data array
for(i=0; i < 3; i++) {
i2c_data[i].b[0] = i2c_byte[i*4 + 0];
i2c_data[i].b[1] = i2c_byte[i*4 + 1];
i2c_data[i].b[2] = i2c_byte[i*4 + 2];
i2c_data[i].b[3] = i2c_byte[i*4 + 3];
}
// store command code
switch(i2c_byte[0]) {
case 1: // stop now
case 2: // move forward
case 3: // rotate
//Serial.print(i2c_data[0].i[0]); Serial.print("\t");
bodyMoveCmdRequest.cmd = i2c_data[0].b[0];
bodyMoveCmdRequest.id = i2c_data[0].b[1];
bodyMoveCmdRequest.units = i2c_data[0].i[1];
bodyMoveCmdRequest.minSpeed = i2c_data[1].i[0];
bodyMoveCmdRequest.acceleration = i2c_data[1].i[1];
bodyMoveCmdRequest.maxSpeed = i2c_data[2].i[0];
bodyMoveCmdRecId = i2c_data[0].b[1];
bodyMoveCmdRequestWait = true;
break;
case 90: // basic config
bodyMotorWheelRadius = i2c_data[0].b[2];
bodyMotorWheelDistance = i2c_data[0].b[3];
bodyMotorWheelResolution = i2c_data[1].i[0];
bodyMotorAccelerationBase = i2c_data[1].i[1];
bodyMotorSpeedBase = i2c_data[2].i[0];
bodyMotorSpeedTop = i2c_data[2].i[1];
bodyMotorStepsPerUnit = (float)((float)(bodyMotorWheelRadius * 3.14159265358979 * 2.0) / bodyMotorWheelResolution);
bodyMoveCmdRecId = i2c_data[0].b[1];
break;
case 254: // remote reset
resetFunc();
break;
}
}
// EOF I2C