/*
*
* DAMI-M1
*
* slave-motion-mod1-vx
*
* Shared PIDs for speed and steering
* Adaptative steering pidKs for start + under move above error limit
* Speed Change (step 1 pulse until target)
* - increase: step 1 pulse until target (wait period on start)
* - decrease: distance to decrease speed (Torricelli equation)
*
* LIBRARIES
* Servo (sg90)
* Wire (Arduino I2C slave)
* PID_v1
*
*/
#define outputForPlotter true
#define serialSpeed 115200
#define bodyLeftMotorPwmBase 0
#define bodyRightMotorPwmBase 0
// encoder type
#define bodyEncoderSJ01 true
#define bodyEncoderHC020K false
#define bodyEncoderLM393 false
#if bodyEncoderSJ01
#define bodyEncoderSignal CHANGE
#define bodyEncoderInputSignal INPUT
#endif
#if bodyEncoderHC020K
#define bodyEncoderSignal FALLING
#define bodyEncoderInputSignal INPUT_PULLUP
#endif
#if bodyEncoderLM393
#define bodyEncoderSignal RISING
#define bodyEncoderInputSignal INPUT
#endif
// motor one
#define enA 5
#define in1 4
#define in2 7
// motor two
#define enB 6
#define in3 8
#define in4 12
// LeftMotor
#define bodyLeftMotorEn enA
#define bodyLeftMotorForward in1
#define bodyLeftMotorBackward in2
// RightMotor
#define bodyRightMotorEn enB
#define bodyRightMotorForward in4
#define bodyRightMotorBackward in3
// encoder LeftMotor
#define bodyEncoderLeftInt 0
#define bodyEncoderLeftFunction bodyEncoderLeftCounter
#define bodyEncoderLeftPin 2 // A pin the interrupt pin
#if bodyEncoderSJ01
#define bodyEncoderLeftPinB 9 // B pin: the digital pin
#endif
#define bodyEncoderLeftSignal bodyEncoderSignal
#define bodyEncoderLeftInputSignal bodyEncoderInputSignal
// encoder RightMotor
#define bodyEncoderRightInt 1
#define bodyEncoderRightFunction bodyEncoderRightCounter
#define bodyEncoderRightPin 3 // A pin: the interrupt pin
#if bodyEncoderSJ01
#define bodyEncoderRightPinB 10 // B pin: the digital pin
#endif
#define bodyEncoderRightSignal bodyEncoderSignal
#define bodyEncoderRightInputSignal bodyEncoderInputSignal
// steering PID
#define bodySteeringPidSetPointTarget 0
#define bodySteeringPidSampleTime 2
#define bodySteeringPidMinOutput -100
#define bodySteeringPidMaxOutput 100
// adaptative pidKs
#define bodySteeringPidAdaptativeLimit0 1
#define bodySteeringPidAdaptativeLimit1 3
#define bodySteeringPidKp0 1 //3
#define bodySteeringPidKi0 2 //0
#define bodySteeringPidKd0 0.001
#define bodySteeringPidKp1 1 //6
#define bodySteeringPidKi1 1 //0
#define bodySteeringPidKd1 0
#define bodySteeringPidKp2 2
#define bodySteeringPidKi2 2
#define bodySteeringPidKd2 0
// speed PID
#define bodySpeedPidSetPointBase 15
#define bodySpeedPidSampleTime 25
#define bodySpeedPidMinOutput 50
#define bodySpeedPidMaxOutput 200
// adaptative pidKs
#define bodySpeedPidAdaptativeLimit0 1
#define bodySpeedPidAdaptativeLimit1 5
#define bodySpeedPidKp0 0.3
#define bodySpeedPidKi0 1
#define bodySpeedPidKd0 0
#define bodySpeedPidKp1 0.6
#define bodySpeedPidKi1 2
#define bodySpeedPidKd1 0
#define bodySpeedPidKp2 1.2
#define bodySpeedPidKi2 5
#define bodySpeedPidKd2 0
// speed change acceleration constant
#define bodyMoveAcceleration 10 // used on slow down distance calculation
#include <PID_v1.h>
// odometry / navigation
#define PI2 6.28318530718
//units: mm for high resolution / cm for low resolution
#define bodyWheelRadius 31.9 // wheel diameter / 2
//#define bodyWheelAxisRadius 67.5 // = lenght of axis / 2
#define bodyWheelAxisRadius 99 // = lenght of axis / 2
#define bodyEncoderRotationPulses 1920 // encoder resolution
#define bodyEncoderPulsesPerUnit 0.104392505884918 // = (2 * PI * bodyWheelRadius) / bodyEncoderRotationPulses
// speedChange
#define bodySpeedPidSpeedChangeWaitingCount 80
#define bodySpeedPidSpeedChangeStepMaxCount 25
// I2C Arduino Slave
#include <Wire.h>
#define I2C_NODE_ADDRESS 8
#include <Servo.h>
Servo servo;
int servoPin = 11;
// I2C
byte requestEventInfoType = 1;
byte requestEventCommand = 0;
#if bodyEncoderSJ01
byte bodyEncoderLeftPinLast;
boolean bodyEncoderLeftDirection; // the rotation direction
byte bodyEncoderRightPinLast;
boolean bodyEncoderRightDirection; //the rotation direction
#endif
volatile signed long bodyEncoderLeftPulses = 0; // pulses since last PID computation
volatile signed long bodyEncoderLeftTotalPulses = 0; // total pulses since last reset (use: calculate error left-right)
volatile signed long bodyEncoderRightPulses = 0; // pulses since last PID computation
volatile signed long bodyEncoderRightTotalPulses = 0; // total pulses since move start
signed long bodyEncoderLeftTotalPulsesLast = 0;
signed long bodyEncoderRightTotalPulsesLast = 0;
// steering PID
double bodySteeringPidInput; double bodySteeringPidInputTmp = 0; // tmp for report only
double bodySteeringPidOutput; // Power supplied to the motor PWM value.
double bodySteeringPidSetPoint;
double bodySteeringPidKp=bodySteeringPidKp0, bbodySteeringPidKi=bodySteeringPidKi0, bodySteeringPidKd=bodySteeringPidKd0;
// adaptative pidKs
byte bodySteeringPidAdaptativeStatus = 0;
PID bodySteeringPid(&bodySteeringPidInput, &bodySteeringPidOutput, &bodySteeringPidSetPoint, bodySteeringPidKp, bbodySteeringPidKi, bodySteeringPidKd, REVERSE);
// speed PID
double bodySpeedPidInput; double bodySpeedPidInputTmp = 0; // tmp for report only
double bodySpeedPidOutput; // Power supplied to the motor PWM value.
double bodySpeedPidSetPoint;
double bodySpeedPidKp=bodySpeedPidKp0, bbodySpeedPidKi=bodySpeedPidKi0, bodySpeedPidKd=bodySpeedPidKd0;
// adaptative pidKs
byte bodySpeedPidAdaptativeStatus = 0;
PID bodySpeedPid(&bodySpeedPidInput, &bodySpeedPidOutput, &bodySpeedPidSetPoint, bodySpeedPidKp, bbodySpeedPidKi, bodySpeedPidKd, DIRECT);
// PWM to motors
byte bodyLeftMotorPwmOut = 0;
byte bodyRightMotorPwmOut = 0;
// speedChange
double bodySpeedPidSetPointTarget = 0;
//double bodySpeedPidSetPointTargetStart = 0;
double bodySpeedPidSetPointTargetEnd = 0;
boolean bodySpeedPidSpeedChangeFlag = false;
boolean bodySpeedPidSpeedChangeOnStartFlag = false;
boolean bodySpeedPidSpeedChangeOnEndFlag = false;
double bodySpeedPidSpeedChangeStepIncrement = 0;
unsigned int bodySpeedPidSpeedResultCounter = 0;
unsigned int bodySpeedPidSpeedResultLastCounter = 0;
double bodyMoveDistanceRemain = 0;
signed long bodyMoveSlowDownDistance = 0;
double bodyMoveCurrentSpeedUnitPerSec = 0;
byte bodyMoveProcessStatus = 0;
#define bodyMoveCmdSequenceMax 24
// pulses per sample time = pidSetPoint
// pulses per second = 1000/sampleTime * pidSetPoint
// distance per second = 1000/sampleTime * pidSetPoint * distance
typedef struct {
byte cmd;
unsigned long distance; // distance in mm/cm (depends on resolution), angles on degrees
byte setPointStart; // pulses per pidSampleTime
byte setPointTarget; // 5=2,10=4,15=6,20=8,25=10,40=16,60=24,80=32 cm/s
} bodyMoveCmdType;
//bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{41, 1800}, {31, 90}, {41, 300}, {31, 90}, {41, 300}, {31, 90}, {41, 300}, {32, 90}, {41, 900}, {32, 360}, {41, 900}, {31, 180}};
// 2 bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{31, 360},{31, 360}};
// 6 bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{41, 1000, 5, 30}, {42, 1000, 10, 30}, {41, 1000, 0, 0}, {42, 1000, 0, 10}, {41, 1000, 20, 0}, {42, 1000, 0, 0}};
// 6 bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{41, 100, 5, 30}, {42, 200, 5, 20}, {41, 300, 5, 30}, {42, 400, 5, 30}, {41, 500, 5, 20}, {42, 1000, 5, 30}};
//bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{41, 2000, 0, 60}, {31, 90}, {41, 850, 0, 30}, {32, 90}, {41, 500, 0, 30}, {31, 90}, {41, 180}, {32, 90}, {41, 800, 30, 60}, {41, 2500, 0, 60}, {32, 90}, {41, 900, 0, 40}};
//bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{31, 360, 15, 15}, {32, 180, 10, 10}, {32, 90, 0, bodySpeedPidSetPointBase}, {32, 90, 0, 0}, {0, 0, 0, 0}};
bodyMoveCmdType bodyMoveCmdSequence[bodyMoveCmdSequenceMax] = {{41, 1000, 15, 15}, {32, 180, 10, 15}, {41, 1000, 0, bodySpeedPidSetPointBase}, {31, 180, 15, 15}, {0, 0, 0, 0}};
int bodyMoveCurrentCommand = 0;
unsigned long bodyEncoderLeftPulsesTarget = 0;
unsigned long bodyEncoderRightPulsesTarget = 0;
signed int bodyMoveCurrentDirection = 0;
int bodyMoveCurrentMoveCode = 0; // 0=STOP / 41=F / 42=B / 31=L / 32=R
unsigned long bodyMoveLoopCounter = 0;
unsigned long loopTimeStart = 0;
unsigned long loopTime = 0;
// only for display
signed long bodyEncoderLeftTotalPulsesTmp, bodyEncoderRightTotalPulsesTmp;
// odometry
double bodyTheta = 0;
double bodyPosX = 0;
double bodyPosY = 0;
void setup()
{
Serial.begin(serialSpeed); //Initialize the serial port
pinMode(bodyLeftMotorEn, OUTPUT);
pinMode(bodyLeftMotorForward, OUTPUT);
pinMode(bodyLeftMotorBackward, OUTPUT);
pinMode(bodyRightMotorEn, OUTPUT);
pinMode(bodyRightMotorForward, OUTPUT);
pinMode(bodyRightMotorBackward, OUTPUT);
// steering PID
bodySteeringPidSetPoint = bodySteeringPidSetPointTarget;
bodySteeringPid.SetMode(MANUAL);
bodySteeringPid.SetSampleTime(bodySteeringPidSampleTime);
bodySteeringPid.SetOutputLimits(bodySteeringPidMinOutput, bodySteeringPidMaxOutput);
// speed PID
bodySpeedPidSetPoint = bodySpeedPidSetPointBase; // speed in pulses per sample time
bodySpeedPid.SetMode(MANUAL);
bodySpeedPid.SetSampleTime(bodySpeedPidSampleTime);
bodySpeedPid.SetOutputLimits(bodySpeedPidMinOutput, bodySpeedPidMaxOutput);
#if bodyEncoderSJ01
bodyEncoderLeftDirection = true; //default -> Forward
pinMode(bodyEncoderLeftPinB, bodyEncoderLeftInputSignal);
#endif
attachInterrupt(bodyEncoderLeftInt, bodyEncoderLeftFunction, bodyEncoderLeftSignal);
#if bodyEncoderSJ01
bodyEncoderRightDirection = true; // default -> Forward
pinMode(bodyEncoderRightPinB, bodyEncoderRightInputSignal);
#endif
attachInterrupt(bodyEncoderRightInt, bodyEncoderRightFunction, bodyEncoderRightSignal);
// I2C Arduino Slave
Wire.begin(I2C_NODE_ADDRESS); // join i2c bus with address #8
Wire.onReceive(receiveEvent); // register event
Wire.onRequest(requestEvent); // register event
// servo
delay(50);
servo.attach(servoPin);
//control the servo's direction and the position of the motor
delay(50);
servo.write(0);
delay(1000);
servo.write(90);
delay(1000);
servo.detach();
loopTimeStart = millis();
}
void loop()
{
// just move & display
bodyMoveProcess();
// end loop
bodyMoveLoopCounter++;
loopTime = millis() - loopTimeStart;
loopTimeStart = millis();
}
void bodyMoveProcess() {
boolean bodySteeringPidResult = false;
boolean bodySpeedPidResult = false;
bodyMoveCheck();
if(bodyMoveProcessStatus) {
if(bodyMoveCurrentMoveCode == 0) {
// the moviment end and we are stoped
bodyMoveEnd();
}
// calculate pwm parts for desired speed and steering
bodySpeedPidResult = bodyMoveSpeedPidCompute();
bodySteeringPidResult = bodyMoveSteeringPidCompute();
// set speed
bodyLeftMotorPwmOut = bodyLeftMotorPwmBase + bodySpeedPidOutput - bodySteeringPidOutput;
bodyRightMotorPwmOut = bodyRightMotorPwmBase + bodySpeedPidOutput + bodySteeringPidOutput;
bodyMove(bodyLeftMotorPwmOut, bodyRightMotorPwmOut);
// odometry
updateOdometry();
}
if(bodySteeringPidResult) {
if(bodyEncoderLeftTotalPulses != 0 || bodyEncoderRightTotalPulses != 0 ) {
// display info
Serial.print(abs(bodyEncoderLeftTotalPulses)); Serial.print("\t");
Serial.print(abs(bodyEncoderRightTotalPulses)); Serial.print("\t");
/*
Serial.print(bodySteeringPidOutput); Serial.print("\t");
*/
Serial.print((float)bodyLeftMotorPwmOut); Serial.print("\t"); // divided for scale display
Serial.print((float)bodyRightMotorPwmOut); Serial.print("\t"); // divided for scale display
Serial.print(bodySpeedPidOutput); Serial.print("\t");
Serial.print(bodySteeringPidOutput); Serial.print("\t");
Serial.print(bodySpeedPidSetPoint - bodySpeedPidInputTmp); Serial.print("\t");
Serial.print(bodySteeringPidInputTmp); Serial.print("\t"); // bodySteeringError
Serial.print(bodySpeedPidAdaptativeStatus); Serial.print("\t");
Serial.print(bodySteeringPidAdaptativeStatus); Serial.print("\t");
//Serial.print(bodyMoveDistanceRemain); Serial.print("\t");
//Serial.print(bodyMoveSlowDownDistance); Serial.print("\t");
//Serial.print(bodySpeedPidSetPoint / 10); Serial.print("\t");
//Serial.print(bodyMoveDistanceRemain); Serial.print("\t");
//Serial.print(bodyMoveSlowDownDistance); Serial.print("\t");
Serial.print(bodyTheta * 180 / PI); Serial.print("\t");
Serial.print(bodyPosX); Serial.print("\t");
Serial.print(bodyPosY); Serial.print("\t");
//Serial.print(loopTime); Serial.print("\t");
//Serial.print((bodyEncoderLeftPulsesTarget + bodyEncoderRightPulsesTarget) / 4); Serial.print("\t");
//Serial.print((bodyEncoderLeftPulsesTarget + bodyEncoderRightPulsesTarget - abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses)) / 2); Serial.print("\t");
Serial.println("");
}
}
}
void bodyMoveCheck() {
if(!bodyMoveProcessStatus) {
//
// run ONLY on first loop
//
bodyMoveStart();
} else {
//
// run on ALL but NOT on first loop
//
// calculate distance to target
bodyMoveCheckTargetDistance();
// calculate eventual speedChange
bodyMoveSpeedChangeControl();
// one encoder reach the target?
boolean onTarget = false;
if(abs(bodyEncoderLeftTotalPulses) >= bodyEncoderLeftPulsesTarget) onTarget = true;
if(abs(bodyEncoderRightTotalPulses) >= bodyEncoderRightPulsesTarget) onTarget = true;
if(onTarget) bodyMoveCurrentMoveCode = 0;
}
}
void bodyMoveCheckTargetDistance() {
double targetSpeedUnitPerSec = 0;
unsigned long bodyMoveDistanceRemainPulses;
// calculate distanceRemain
bodyMoveDistanceRemainPulses = (bodyEncoderLeftPulsesTarget + bodyEncoderRightPulsesTarget - abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses)) / 2;
bodyMoveDistanceRemain = bodyMoveDistanceRemainPulses * bodyEncoderPulsesPerUnit;
// convert pulses (pulses per sample time) in speed (distanceUnits per second)
bodyMoveCurrentSpeedUnitPerSec = bodySpeedPidSetPoint * bodyEncoderPulsesPerUnit * (1000 / bodySpeedPidSampleTime); // TODO: (1000 / bodySpeedPidSampleTime) is a constant
targetSpeedUnitPerSec = bodySpeedPidSetPointBase * bodyEncoderPulsesPerUnit * (1000 / bodySpeedPidSampleTime); // TODO: (1000 / bodySpeedPidSampleTime) is a constant
// calculate safeDistance based on torricceli formula
bodyMoveSlowDownDistance = abs((targetSpeedUnitPerSec * targetSpeedUnitPerSec) - (bodyMoveCurrentSpeedUnitPerSec * bodyMoveCurrentSpeedUnitPerSec)) / ( 2 * bodyMoveAcceleration);
// doesnt activate if remains more than a quarter of path
if (bodyMoveDistanceRemainPulses > (bodyEncoderLeftPulsesTarget + bodyEncoderRightPulsesTarget) / 2 * 3/4) {
bodyMoveSlowDownDistance = 0;
}
if(bodyMoveSlowDownDistance > 0 && (bodyMoveSlowDownDistance - bodyMoveDistanceRemain) >= 0) {
if(!bodySpeedPidSpeedChangeOnEndFlag) {
bodySpeedPidSpeedChangeOnEndFlag = true;
bodySpeedPidSpeedChangeFlag = true;
bodySpeedPidSetPointTarget = bodySpeedPidSetPointBase;
bodySpeedPidSpeedChangeStepIncrement = -1;
#if !outputForPlotter
Serial.print("SLOWDOWN START"); Serial.print("\t");
#endif
}
}
}
void bodyMoveSpeedChangeControl() {
// we do not accelerate to target speed at begin of the move
// check if its time to start the accelleration increase
if(bodySpeedPidSpeedChangeOnStartFlag) {
if(bodySpeedPidSpeedResultCounter > bodySpeedPidSpeedChangeWaitingCount) {
bodySpeedPidSpeedChangeFlag = true;
bodySpeedPidSpeedChangeOnStartFlag = false;
}
}
// we are in the middle of an accelleration (increase/decrease)
if(bodySpeedPidSpeedChangeFlag) {
// time to do a speed change step?
if(bodySpeedPidSpeedResultCounter - bodySpeedPidSpeedResultLastCounter > bodySpeedPidSpeedChangeStepMaxCount) {
// update control var
bodySpeedPidSpeedResultLastCounter = bodySpeedPidSpeedResultCounter;
// change speed (update current pidSetPoint)
bodySpeedPidSetPoint += bodySpeedPidSpeedChangeStepIncrement;
bodySpeedPidSetPoint += bodySpeedPidSpeedChangeStepIncrement;
// time to end the speed change?
if(bodySpeedPidSpeedChangeStepIncrement >= 0 ) {
if(bodySpeedPidSetPoint >= bodySpeedPidSetPointTarget) {
bodySpeedPidSetPoint = bodySpeedPidSetPointTarget;
bodySpeedPidSpeedChangeFlag = false;
}
} else {
if(bodySpeedPidSetPoint <= bodySpeedPidSetPointTarget) {
bodySpeedPidSetPoint = bodySpeedPidSetPointTarget;
bodySpeedPidSpeedChangeFlag = false;
}
}
}
}
}
void bodyMoveStart() {
unsigned long tmpVar;
unsigned long tmpLong;
int minSetPoint = 0;
int targetSetPoint = 0;
double minSpeedUnitPerSec;
int targetSpeedUnitPerSec;
int i = 0;
bodyMoveCurrentMoveCode = bodyMoveCmdSequence[bodyMoveCurrentCommand].cmd;
// target distance calculations
switch(bodyMoveCurrentMoveCode) {
case 0:
// stop
break;
case 31:
case 32:
// rotate (left/right)
// set distance : pulses = (((degrees x PI)/180) x axis_radius) x encoderPulsesPerUnit
tmpVar = ((bodyMoveCmdSequence[bodyMoveCurrentCommand].distance * PI) / 180 ) * bodyWheelAxisRadius;
tmpVar = tmpVar / bodyEncoderPulsesPerUnit;
bodyEncoderLeftPulsesTarget = tmpVar;
bodyEncoderRightPulsesTarget = tmpVar;
break;
case 41:
case 42:
// forward / backward
// set distance : pulses = distance / distance_per_pulse
tmpVar = bodyMoveCmdSequence[bodyMoveCurrentCommand].distance / bodyEncoderPulsesPerUnit;
bodyEncoderLeftPulsesTarget = tmpVar;
bodyEncoderRightPulsesTarget = tmpVar;
break;
}
// start, target and end speed setup
minSetPoint = bodyMoveCmdSequence[bodyMoveCurrentCommand].setPointStart;
if(!minSetPoint) {
minSetPoint = bodySpeedPidSetPointBase;
}
targetSetPoint = bodyMoveCmdSequence[bodyMoveCurrentCommand].setPointTarget;
if(!targetSetPoint) {
// calculate good speed for distance with torricelli equation
//f = sqrt(i^2+2ad)
// convert pulses (pulses per sample time) in speed (distanceUnits per second)
minSpeedUnitPerSec = minSetPoint * bodyEncoderPulsesPerUnit * (1000 / bodySpeedPidSampleTime);
// calculate using torricelli equation
targetSpeedUnitPerSec = (int)sqrt((minSpeedUnitPerSec * minSpeedUnitPerSec) + ( 2 * bodyMoveAcceleration) * (bodyMoveCmdSequence[bodyMoveCurrentCommand].distance / 2)) ;
// convert speed (distanceUnits per second) in pulses (pulses per sample time)
targetSetPoint = targetSpeedUnitPerSec / (bodyEncoderPulsesPerUnit * (1000 / bodySpeedPidSampleTime));
//Serial.print(targetSetPoint); Serial.print("\t");
// distance calculation based on torricceli formula
//int stopDistance = abs((minSpeedUnitPerSec * minSpeedUnitPerSec) - (targetSpeedUnitPerSec * targetSpeedUnitPerSec)) / ( 2 * bodyMoveAcceleration);
//Serial.print(stopDistance); Serial.print("\t");
}
// does not allow target speed bellow minimum speed
if(targetSetPoint < minSetPoint) {
targetSetPoint = minSetPoint;
}
// speedChange speeds
bodySpeedPidSetPointTarget = targetSetPoint;
bodySpeedPidSetPointTargetEnd = minSetPoint;
// pulses increment on speedChange
bodySpeedPidSpeedChangeStepIncrement = 1;
// speedChange flags
bodySpeedPidSpeedResultCounter = 0;
bodySpeedPidSpeedChangeOnEndFlag = false;
bodySpeedPidSpeedChangeFlag = false;
if(targetSetPoint != minSetPoint ) {
bodySpeedPidSpeedChangeOnStartFlag = true;
}
// adaptative steeringPid
bodySteeringPid.SetTunings(bodySteeringPidKp2, bodySteeringPidKi2, bodySteeringPidKd2);
bodySteeringPidAdaptativeStatus = 2;
// adaptative speedPid
bodySpeedPid.SetTunings(bodySpeedPidKp2, bodySpeedPidKi2, bodySpeedPidKd2);
bodySpeedPidAdaptativeStatus = 2;
#if !outputForPlotter
Serial.print("STARTMOVE"); Serial.print("\t");
Serial.print(bodyMoveCurrentCommand); Serial.print("\t");
Serial.print(bodyMoveCurrentMoveCode); Serial.print("\t");
Serial.print(bodyMoveCmdSequence[bodyMoveCurrentCommand].distance); Serial.print("\t");
Serial.print(bodyEncoderLeftPulsesTarget); Serial.print("\t");
Serial.print(bodyEncoderRightPulsesTarget); Serial.print("\t");
Serial.print(minSetPoint); Serial.print("\t");
Serial.print(bodySpeedPidSetPointTarget); Serial.print("\t");
Serial.print(bodySpeedPidSpeedChangeStepIncrement); Serial.print("\t");
Serial.println("");
#endif
// lets prepare to start moving
delay(5000);
// steering pid
bodySteeringPidSetPoint = bodySteeringPidSetPointTarget;
bodySteeringPid.SetMode(AUTOMATIC);
// speed PID
bodySpeedPidSetPoint = minSetPoint;
bodySpeedPid.SetMode(AUTOMATIC);
// set move variables
bodyMoveProcessStatus = 1;
bodyMoveLoopCounter = 0;
bodyEncoderLeftTotalPulses = 0;
bodyEncoderRightTotalPulses = 0;
}
void bodyMoveEnd() {
// assure that all motors are stoped and reset vars
bodyMoveStop();
// steering pid
bodySteeringPid.SetMode(MANUAL);
bodySteeringPidOutput = 0; // have last steering
// speed pid
bodySpeedPid.SetMode(MANUAL);
bodySpeedPidOutput = 0; // have last speed error
// wait for last wheel to stop (to get current total pulses)
delay(250);
// some info
// bodyMoveCurrentMoveCode is already 0 (set on bodyMoveEndCheck())
#if !outputForPlotter
Serial.print("ENDMOVE"); Serial.print("\t");
Serial.print(bodyMoveCurrentCommand); Serial.print("\t");
Serial.print(bodyMoveCmdSequence[bodyMoveCurrentCommand].cmd); Serial.print("\t");
Serial.print(bodyMoveCmdSequence[bodyMoveCurrentCommand].distance); Serial.print("\t");
Serial.print(abs(bodyEncoderLeftTotalPulses)); Serial.print("\t");
Serial.print(abs(bodyEncoderRightTotalPulses)); Serial.print("\t");
Serial.print(abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses)); Serial.print("\t");
Serial.print(bodyEncoderLeftPulsesTarget); Serial.print("\t");
Serial.print(bodyEncoderRightPulsesTarget); Serial.print("\t");
Serial.print((signed long)bodyEncoderLeftPulsesTarget - abs(bodyEncoderLeftTotalPulses)); Serial.print("\t");
Serial.print((signed long)bodyEncoderRightPulsesTarget - abs(bodyEncoderRightTotalPulses)); Serial.print("\t");
Serial.println("");
#endif
delay(1000);
// prepare some vars
bodySteeringPidInput = 0;
bodyEncoderLeftTotalPulses = 0;
bodyEncoderRightTotalPulses = 0;
bodyEncoderLeftTotalPulsesLast = 0;
bodyEncoderRightTotalPulsesLast = 0;
bodyMoveCurrentDirection = 0;
// advance command sequence
bodyMoveCurrentCommand++;
if(bodyMoveCurrentCommand == bodyMoveCmdSequenceMax - 1) {
// restart move loop
#if !outputForPlotter
Serial.println("RESTART");
#endif
bodyMoveCurrentCommand = 0;
} else {
if(bodyMoveCmdSequence[bodyMoveCurrentCommand].cmd == 0) {
// restart move loop
#if !outputForPlotter
Serial.println("RESTART");
#endif
bodyMoveCurrentCommand = 0;
}
}
// allow new command processing
bodyMoveProcessStatus = 0;
}
void bodyMove(unsigned int leftPwm, unsigned int rightPwm) {
switch(bodyMoveCurrentMoveCode) {
case 31:
bodyRotateLeft(leftPwm, rightPwm);
break;
case 32:
bodyRotateRight(leftPwm, rightPwm);
break;
case 41:
bodyMoveForward(leftPwm, rightPwm);
break;
case 42:
bodyMoveBackward(leftPwm, rightPwm);
break;
}
}
// calculate pwm wheel diference correction using adaptative pid
boolean bodyMoveSteeringPidCompute() {
boolean pidResult;
byte adaptativeStatus = 0;
// wheel error
bodySteeringPidInput = abs(bodyEncoderLeftTotalPulses) - abs(bodyEncoderRightTotalPulses);
// bof: adaptative ks
unsigned long averagePulses = (abs(bodyEncoderLeftTotalPulses) + abs(bodyEncoderRightTotalPulses)) /2 ;
if(averagePulses > 100) {
if(abs(bodySteeringPidInput) > bodySteeringPidAdaptativeLimit1) {
adaptativeStatus = 2;
} else if(abs(bodySteeringPidInput) > bodySteeringPidAdaptativeLimit0) {
adaptativeStatus = 1;
}
if(bodySteeringPidAdaptativeStatus != adaptativeStatus) {
switch(adaptativeStatus) {
case 0:
bodySteeringPid.SetTunings(bodySteeringPidKp0, bodySteeringPidKi0, bodySteeringPidKd0);
break;
case 1:
bodySteeringPid.SetTunings(bodySteeringPidKp1, bodySteeringPidKi1, bodySteeringPidKd1);
break;
case 2:
bodySteeringPid.SetTunings(bodySteeringPidKp2, bodySteeringPidKi2, bodySteeringPidKd2);
break;
}
bodySteeringPidAdaptativeStatus = adaptativeStatus;
}
}
// eof: adaptative ks
pidResult = bodySteeringPid.Compute();
if(pidResult) {
bodySteeringPidInputTmp = bodySteeringPidInput; // for report only
}
return pidResult;
}
// calculate pwm base for desired speed using adaptative pid
boolean bodyMoveSpeedPidCompute() {
boolean pidResult;
byte adaptativeStatus = 0;
// speed error
//bodySpeedPidInput = (abs(bodyEncoderLeftPulses) + abs(bodyEncoderRightPulses)) / 2;
bodySpeedPidInput = abs(bodyEncoderLeftPulses);
// bof: adaptative ks
unsigned long averagePulses = (abs(bodyEncoderLeftTotalPulses) + abs(bodyEncoderRightTotalPulses)) /2 ;
if(averagePulses > 20) {
if(abs(bodySpeedPidInput) > bodySpeedPidAdaptativeLimit1) {
//adaptativeStatus = 2;
adaptativeStatus = 1;
} else if(abs(bodySpeedPidInput) > bodySpeedPidAdaptativeLimit0) {
adaptativeStatus = 1;
}
if(bodySpeedPidAdaptativeStatus != adaptativeStatus) {
switch(adaptativeStatus) {
case 0:
bodySpeedPid.SetTunings(bodySpeedPidKp0, bodySpeedPidKi0, bodySpeedPidKd0);
break;
case 1:
bodySpeedPid.SetTunings(bodySpeedPidKp1, bodySpeedPidKi1, bodySpeedPidKd1);
break;
case 2:
bodySpeedPid.SetTunings(bodySpeedPidKp2, bodySpeedPidKi2, bodySpeedPidKd2);
break;
}
bodySpeedPidAdaptativeStatus = adaptativeStatus;
}
}
// eof: adaptative ks
// try to handle strange spikes on bodySpeedPidInput
if(bodySpeedPidInput > 50) bodySpeedPidInput = 50;
pidResult = bodySpeedPid.Compute();
if(pidResult) {
bodySpeedPidInputTmp = bodySpeedPidInput; // for report only
bodyEncoderLeftPulses = 0;
bodyEncoderRightPulses = 0;
bodySpeedPidSpeedResultCounter++;
}
return pidResult;
}
void bodyEncoderLeftCounter()
{
#if bodyEncoderSJ01
int Lstate = digitalRead(bodyEncoderLeftPin);
if((bodyEncoderLeftPinLast == LOW) && Lstate==HIGH)
{
int val = digitalRead(bodyEncoderLeftPinB);
if(val == LOW && bodyEncoderLeftDirection)
{
bodyEncoderLeftDirection = false; //Reverse
}
else if(val == HIGH && !bodyEncoderLeftDirection)
{
bodyEncoderLeftDirection = true; //Forward
}
}
bodyEncoderLeftPinLast = Lstate;
if(!bodyEncoderLeftDirection)
{
bodyEncoderLeftPulses++;
bodyEncoderLeftTotalPulses++;
} else {
bodyEncoderLeftPulses--;
bodyEncoderLeftTotalPulses--;
}
#else
bodyEncoderLeftPulses++;
bodyEncoderLeftTotalPulses ++;
#endif
}
void bodyEncoderRightCounter()
{
#if bodyEncoderSJ01
int Lstate = digitalRead(bodyEncoderRightPin);
if((bodyEncoderRightPinLast == LOW) && Lstate==HIGH)
{
int val = digitalRead(bodyEncoderRightPinB);
if(val == LOW && bodyEncoderRightDirection)
{
bodyEncoderRightDirection = false; // Reverse
}
else if(val == HIGH && !bodyEncoderRightDirection)
{
bodyEncoderRightDirection = true; // Forward
}
}
bodyEncoderRightPinLast = Lstate;
if(!bodyEncoderRightDirection)
{
bodyEncoderRightPulses++;
bodyEncoderRightTotalPulses ++;
} else {
bodyEncoderRightPulses--;
bodyEncoderRightTotalPulses --;
}
#else
bodyEncoderRightPulses++;
bodyEncoderRightTotalPulses ++;
#endif
}
void bodyMoveForward( int pmwSpeedLeft, int pmwSpeedRight ){
if(bodyMoveCurrentDirection != 1) {
bodyMoveCurrentDirection = 1;
// set motors direction
setBodyMotorLeftForward();
setBodyMotorRightForward();
}
// set motors speed
analogWrite(bodyLeftMotorEn, pmwSpeedLeft);
analogWrite(bodyRightMotorEn, pmwSpeedRight);
}
void bodyMoveBackward( int pmwSpeedLeft, int pmwSpeedRight ) {
if(bodyMoveCurrentDirection != -1) {
bodyMoveCurrentDirection = -1;
// set motors direction
setBodyMotorLeftBackward();
setBodyMotorRightBackward();
}
// set motors speed
analogWrite(bodyLeftMotorEn, pmwSpeedLeft);
analogWrite(bodyRightMotorEn, pmwSpeedRight);
}
void bodyRotateLeft( int pmwSpeedLeft, int pmwSpeedRight ) {
if(bodyMoveCurrentDirection != -2) {
bodyMoveCurrentDirection = -2;
// set motors direction
setBodyMotorLeftBackward();
setBodyMotorRightForward();
}
// set motors speed
analogWrite(bodyLeftMotorEn, pmwSpeedLeft);
analogWrite(bodyRightMotorEn, pmwSpeedRight);
}
void bodyRotateRight( int pmwSpeedLeft, int pmwSpeedRight ) {
if(bodyMoveCurrentDirection != 2) {
bodyMoveCurrentDirection = 2;
// set motors direction
setBodyMotorLeftForward();
setBodyMotorRightBackward();
}
// set motors speed
analogWrite(bodyLeftMotorEn, pmwSpeedLeft);
analogWrite(bodyRightMotorEn, pmwSpeedRight);
}
void bodyMoveStop() {
bodyMoveCurrentDirection = 0;
bodyMoveStopLeft();
bodyMoveStopRight();
}
void bodyMoveStopLeft() {
// set motor direction
setBodyMotorLeftStop();
// set motor speed
analogWrite(bodyLeftMotorEn, 0);
}
void bodyMoveStopRight() {
// set motor direction
setBodyMotorRightStop();
// set motor speed
analogWrite(bodyRightMotorEn, 0);
}
void setBodyMotorLeftForward() {
digitalWrite(bodyLeftMotorForward, HIGH);
digitalWrite(bodyLeftMotorBackward, LOW);
}
void setBodyMotorLeftBackward() {
digitalWrite(bodyLeftMotorForward, LOW);
digitalWrite(bodyLeftMotorBackward, HIGH);
}
void setBodyMotorRightForward() {
digitalWrite(bodyRightMotorForward, HIGH);
digitalWrite(bodyRightMotorBackward, LOW);
}
void setBodyMotorRightBackward() {
digitalWrite(bodyRightMotorForward, LOW);
digitalWrite(bodyRightMotorBackward, HIGH);
}
void setBodyMotorLeftStop() {
digitalWrite(bodyLeftMotorForward, LOW);
digitalWrite(bodyLeftMotorBackward, LOW);
}
void setBodyMotorRightStop() {
digitalWrite(bodyRightMotorForward, LOW);
digitalWrite(bodyRightMotorBackward, LOW);
}
// 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;
signed int bodySteeringPidOutputTmp;
signed int bodySpeedPidSetPointTmp;
signed int bodySpeedPidInputTmp;
signed int bodySpeedPidOutputTmp;
switch(requestEventInfoType) {
case 1:
// prepare
// bodySteeringPidInput // calculate on master
// bodyMoveDistanceRemain // calculate on master
bodySteeringPidOutputTmp = (signed int) (bodySteeringPidOutput * 1000);
//bodySpeedPidSetPointTmp = (signed int) (bodySpeedPidSetPoint * 1000);
//bodySpeedPidInputTmp = (signed int) (bodySpeedPidInput * 1000);
//bodySpeedPidOutputTmp = (signed int) (bodySpeedPidOutput * 1000);
// send
Wire.write((byte*)&requestEventCommand, 1);
Wire.write((byte*)&loopTime, 1);
Wire.write((byte*)&bodyLeftMotorPwmOut, 1);
Wire.write((byte*)&bodyRightMotorPwmOut, 1);
Wire.write((byte*)&bodyEncoderLeftTotalPulses, 4);
Wire.write((byte*)&bodyEncoderRightTotalPulses, 4);
Wire.write((byte*)&bodySteeringPidOutputTmp, 2);
//Wire.write((byte*)&bodySpeedPidSetPointTmp, 2);
Wire.write((byte*)&bodySteeringPidOutputTmp, 2); // just reserved
//Wire.write((byte*)&bodySpeedPidInputTmp, 2);
//Wire.write((byte*)&bodySpeedPidOutputTmp, 2);
Wire.write((byte*)&bodySteeringPidOutputTmp, 2); // just reserved
Wire.write((byte*)&bodySteeringPidOutputTmp, 2); // just reserved
// Available for 32 bytes
//Wire.write((byte*)&bodySteeringPidOutput, 4);
//Wire.write((byte*)&bodySteeringPidOutput, 4);
//Wire.write((byte*)&bodySteeringPidOutput, 4);
break;
case 2:
tmp = 10;
tmp++; Wire.write((byte*)&tmp, 4);
tmp++; Wire.write((byte*)&tmp, 4);
tmp++; Wire.write((byte*)&tmp, 4);
tmp++; Wire.write((byte*)&tmp, 4);
tmp++; Wire.write((byte*)&tmp, 4);
//tmp++; Wire.write((byte*)&tmp, 4);
//tmp++; Wire.write((byte*)&tmp, 4);
//tmp++; Wire.write((byte*)&tmp, 4);
requestEventInfoType = 1;
break;
}
}
// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
byte i2c_byte[32];
int i = 0;
while (Wire.available()) { // slave may send less than requested
char c = Wire.read(); // receive a byte as character
i2c_byte[i] = c;
i++;
}
switch(i2c_byte[0]) {
case 82:
requestEventInfoType = 2;
break;
}
}
void updateOdometry() {
float SL = bodyEncoderPulsesPerUnit * (abs(bodyEncoderLeftTotalPulses) - bodyEncoderLeftTotalPulsesLast);
float SR = bodyEncoderPulsesPerUnit * (abs(bodyEncoderRightTotalPulses) - bodyEncoderRightTotalPulsesLast);
bodyEncoderLeftTotalPulsesLast = abs(bodyEncoderLeftTotalPulses);
bodyEncoderRightTotalPulsesLast = abs(bodyEncoderRightTotalPulses);
double meanDistance = (SL + SR)/2;
//bodyTheta += (SR - SL) / (bodyWheelAxisRadius * 2);
bodyTheta += atan((SR - SL) / (bodyWheelAxisRadius * 2));
if(bodyTheta > PI2)
bodyTheta -= PI2;
else if(bodyTheta < -PI2)
bodyTheta += PI2;
bodyPosX = bodyPosX + meanDistance*cos(bodyTheta);
bodyPosY = bodyPosY + meanDistance*sin(bodyTheta);
}