Add files via upload
This commit is contained in:
98sila78
GitHub
parent
cb16abdff6
commit
16647bc5ff
184
Motors.ino
Normal file
184
Motors.ino
Normal file
@ -0,0 +1,184 @@
|
||||
/* Copyright (C) 2013-2015 Kristian Lauszus, TKJ Electronics. All rights reserved.
|
||||
This software may be distributed and modified under the terms of the GNU
|
||||
General Public License version 2 (GPL2) as published by the Free Software
|
||||
Foundation and appearing in the file GPL2.TXT included in the packaging of
|
||||
this file. Please note that GPL2 Section 2[b] requires that all works based
|
||||
on this software must also be made publicly available under the terms of
|
||||
the GPL2 ("Copyleft").
|
||||
Contact information
|
||||
-------------------
|
||||
Kristian Lauszus, TKJ Electronics
|
||||
Web : http://www.tkjelectronics.dk
|
||||
e-mail : kristianl@tkjelectronics.dk
|
||||
*/
|
||||
|
||||
void updatePID(float restAngle, float offset, float turning, float dt) {
|
||||
/* Brake */
|
||||
if (steerStop) {
|
||||
int32_t wheelPosition = getWheelsPosition();
|
||||
int32_t positionError = wheelPosition - targetPosition;
|
||||
if (cfg.backToSpot) {
|
||||
if (abs(positionError) > zoneA) // Inside zone A
|
||||
restAngle -= (float)positionError / positionScaleA;
|
||||
else if (abs(positionError) > zoneB) // Inside zone B
|
||||
restAngle -= (float)positionError / positionScaleB;
|
||||
else if (abs(positionError) > zoneC) // Inside zone C
|
||||
restAngle -= (float)positionError / positionScaleC;
|
||||
else // Inside zone D
|
||||
restAngle -= (float)positionError / positionScaleD;
|
||||
} else {
|
||||
if (abs(positionError) < zoneC)
|
||||
restAngle -= (float)positionError / positionScaleD;
|
||||
else
|
||||
targetPosition = wheelPosition;
|
||||
}
|
||||
restAngle -= (float)wheelVelocity / velocityScaleStop;
|
||||
|
||||
restAngle = constrain(restAngle, cfg.targetAngle - 10, cfg.targetAngle + 10); // Limit rest Angle
|
||||
}
|
||||
/* Drive forward and backward */
|
||||
else {
|
||||
if ((offset > 0 && wheelVelocity < 0) || (offset < 0 && wheelVelocity > 0) || offset == 0) // Scale down offset at high speed - wheel velocity is negative when driving forward and positive when driving backward
|
||||
offset += (float)wheelVelocity / velocityScaleMove; // We will always compensate if the offset is 0, but steerStop is not set
|
||||
restAngle -= offset;
|
||||
}
|
||||
|
||||
restAngle = constrain(restAngle, lastRestAngle - 1, lastRestAngle + 1); // Don't change restAngle with more than 1 degree in each loop
|
||||
lastRestAngle = restAngle;
|
||||
|
||||
/* Update PID values */
|
||||
float error = restAngle - pitch;
|
||||
float pTerm = cfg.P * error;
|
||||
iTerm += cfg.I * 100.0f * error * dt; // Multiplication with Ki is done before integration limit, to make it independent from integration limit value
|
||||
iTerm = constrain(iTerm, -100.0f, 100.0f); // Limit the integrated error - prevents windup
|
||||
float dTerm = (cfg.D / 100.0f) * (error - lastError) / dt;
|
||||
lastError = error;
|
||||
float PIDValue = pTerm + iTerm + dTerm;
|
||||
|
||||
/* Steer robot sideways */
|
||||
if (turning < 0) { // Left
|
||||
turning += abs((float)wheelVelocity / velocityScaleTurning); // Scale down at high speed
|
||||
if (turning > 0)
|
||||
turning = 0;
|
||||
}
|
||||
else if (turning > 0) { // Right
|
||||
turning -= abs((float)wheelVelocity / velocityScaleTurning); // Scale down at high speed
|
||||
if (turning < 0)
|
||||
turning = 0;
|
||||
}
|
||||
|
||||
float PIDLeft = PIDValue + turning;
|
||||
float PIDRight = PIDValue - turning;
|
||||
|
||||
PIDLeft *= cfg.leftMotorScaler; // Compensate for difference in some of the motors
|
||||
PIDRight *= cfg.rightMotorScaler;
|
||||
|
||||
/* Set PWM Values */
|
||||
if (PIDLeft >= 0)
|
||||
moveMotor(left, forward, PIDLeft);
|
||||
else
|
||||
moveMotor(left, backward, -PIDLeft);
|
||||
if (PIDRight >= 0)
|
||||
moveMotor(right, forward, PIDRight);
|
||||
else
|
||||
moveMotor(right, backward, -PIDRight);
|
||||
}
|
||||
|
||||
void moveMotor(Command motor, Command direction, float speedRaw) { // Speed is a value in percentage 0-100%
|
||||
if (speedRaw > 100.0f)
|
||||
speedRaw = 100.0f;
|
||||
setPWM(motor, speedRaw * (float)PWMVALUE / 100.0f); // Scale from 0-100 to 0-PWMVALUE
|
||||
if (motor == left) {
|
||||
if (direction == forward) {
|
||||
leftA::Clear();
|
||||
leftB::Set();
|
||||
}
|
||||
else {
|
||||
leftA::Set();
|
||||
leftB::Clear();
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (direction == forward) {
|
||||
rightA::Set();
|
||||
rightB::Clear();
|
||||
}
|
||||
else {
|
||||
rightA::Clear();
|
||||
rightB::Set();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stopMotor(Command motor) {
|
||||
setPWM(motor, PWMVALUE); // Set high
|
||||
if (motor == left) {
|
||||
leftA::Set();
|
||||
leftB::Set();
|
||||
}
|
||||
else {
|
||||
rightA::Set();
|
||||
rightB::Set();
|
||||
}
|
||||
}
|
||||
|
||||
void setPWM(Command motor, uint16_t dutyCycle) { // dutyCycle is a value between 0-ICR1
|
||||
if (motor == left)
|
||||
OCR1A = dutyCycle;
|
||||
else
|
||||
OCR1B = dutyCycle;
|
||||
}
|
||||
|
||||
void stopAndReset() {
|
||||
stopMotor(left);
|
||||
stopMotor(right);
|
||||
lastError = 0;
|
||||
iTerm = 0;
|
||||
targetPosition = getWheelsPosition();
|
||||
lastRestAngle = cfg.targetAngle;
|
||||
}
|
||||
|
||||
/* Interrupt routine and encoder read functions */
|
||||
// It uses gray code to detect if any pulses are missed. See: https://www.circuitsathome.com/mcu/reading-rotary-encoder-on-arduino and http://en.wikipedia.org/wiki/Rotary_encoder#Incremental_rotary_encoder.
|
||||
|
||||
#if defined(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) && defined(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT)
|
||||
static const int8_t enc_states[16] = { 0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0 }; // Encoder lookup table if it interrupts on every edge
|
||||
|
||||
ISR(PIN_CHANGE_INTERRUPT_VECTOR_LEFT) {
|
||||
leftEncoder();
|
||||
#if BALANDUINO_REVISION >= 13
|
||||
}
|
||||
ISR(PIN_CHANGE_INTERRUPT_VECTOR_RIGHT) {
|
||||
#endif
|
||||
rightEncoder();
|
||||
}
|
||||
#elif BALANDUINO_REVISION < 13
|
||||
#warning "Only interrupting on every second edge!"
|
||||
static const int8_t enc_states[16] = { 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0 }; // Encoder lookup table if it only interrupts on every second edge - this only works on revision 1.2 and older
|
||||
#endif
|
||||
|
||||
void leftEncoder() {
|
||||
static uint8_t old_AB = 0;
|
||||
old_AB <<= 2; // Remember previous state
|
||||
old_AB |= (leftEncoder2::IsSet() >> (leftEncoder2::Number - 1)) | (leftEncoder1::IsSet() >> leftEncoder1::Number);
|
||||
leftCounter -= enc_states[ old_AB & 0x0F ];
|
||||
}
|
||||
|
||||
void rightEncoder() {
|
||||
static uint8_t old_AB = 0;
|
||||
old_AB <<= 2; // Remember previous state
|
||||
old_AB |= (rightEncoder2::IsSet() >> (rightEncoder2::Number - 1)) | (rightEncoder1::IsSet() >> rightEncoder1::Number);
|
||||
rightCounter += enc_states[ old_AB & 0x0F ];
|
||||
}
|
||||
|
||||
int32_t readLeftEncoder() { // The encoders decrease when motors are traveling forward and increase when traveling backward
|
||||
return leftCounter;
|
||||
}
|
||||
|
||||
int32_t readRightEncoder() {
|
||||
return rightCounter;
|
||||
}
|
||||
|
||||
int32_t getWheelsPosition() {
|
||||
return leftCounter + rightCounter;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user