t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed real <> sim transfer

Browse Source
This commit is contained in:
Tim Korjakow
2023-06-22 21:03:35 +02:00
parent 951058e644
commit ebb17be5af
12 changed files with 68 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/dcaitirobot/embedded/include/communication/UARTCom.hpp
View File

@ -14,7 +14,6 @@
#include "Arduino.h"
#include "constants.h"
#include "R2WD.h"
class UARTCom {
private:

5
src/dcaitirobot/embedded/src/communication/UARTCom.cpp
View File

@ -8,7 +8,8 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH];
uint8_t currentParseIndex;
int payloadLength;
uint32_t currentTimestamp = 0;
extern R2WD _2WD;
extern MotorWheel wheelLeft;
extern MotorWheel wheelRight;
/**
* @brief extracts frametype from data package
@ -169,7 +170,7 @@ void handleRequest (uint8_t* d)
break;
}
case DRIVE_ENCODER: {
UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0);
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
break;
}
case PID_PARAMETER: {

24
src/dcaitirobot/embedded/src/communication/UCommands.cpp
View File

@ -1,7 +1,7 @@
#include "communication/UCommands.hpp"
#include "misc/Utils.hpp"
#include "R2WD.h"
extern R2WD _2WD;
extern MotorWheel wheelLeft;
extern MotorWheel wheelRight;
void UCommands::setSetpoint (uint8_t* d, U_Component c)
{
@ -15,16 +15,17 @@ void UCommands::setSetpoint (uint8_t* d, U_Component c)
d[COMMAND_POS + 6],
d[COMMAND_POS + 7],
d[COMMAND_POS + 8]);
_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
_2WD.wheelRightSetSpeedMMPS(abs(speed2*1000), speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
wheelRight.setGearedRPM(speed2, speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
break;
}
case RIGHT_MOTOR: {
_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
wheelRight.setGearedRPM(speed1, speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
break;
}
case LEFT_MOTOR: {
_2WD.wheelRightSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
break;
}
default: {
@ -47,17 +48,22 @@ void UCommands::setPIDParameter (uint8_t* d, U_Component c)
d[COMMAND_POS + 10],
d[COMMAND_POS + 11],
d[COMMAND_POS + 12]);
int T = Utils::bytesToInt(d[COMMAND_POS + 9],
d[COMMAND_POS + 10],
d[COMMAND_POS + 11],
d[COMMAND_POS + 12]);
switch (c) {
case ALL_MOTORS: {
_2WD.PIDTune(P,I,D,10);
wheelLeft.PIDSetup(P,I,D,T);
wheelRight.PIDSetup(P,I,D,T);
break;
}
case RIGHT_MOTOR: {
_2WD.wheelLeftPIDTune(P,I,D,10);
wheelLeft.PIDSetup(P,I,D,T);
break;
}
case LEFT_MOTOR: {
_2WD.wheelRightPIDTune(P,I,D,10);
wheelRight.PIDSetup(P,I,D,T);
break;
}
default: {

9
src/dcaitirobot/embedded/src/main.cpp
View File

@ -10,7 +10,6 @@ int speed = 0;
#include <PinChangeIntConfig.h>
#include <PID_Beta6.h>
#include <MotorWheel.h>
#include <R2WD.h>
#ifndef MICROS_PER_SEC
#define MICROS_PER_SEC 1000000
#endif
@ -18,14 +17,13 @@ irqISR(irq1, isr1);
irqISR(irq2, isr2);
MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
MotorWheel wheelRight(10, 11, 6, 7, &irq2, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
R2WD _2WD(&wheelLeft, &wheelRight, WHEELSPAN);
void setup(void)
{
TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz, Silent PWM
_2WD.PIDEnable(0.26,0.02,0.02,10);// Enable PID
wheelLeft.PIDEnable(0.26,0.02,0.02,10);// Enable PID
wheelRight.PIDEnable(0.26,0.02,0.02,10);// Enable PID
//_2WD.PIDEnable(KC, TAUI, TAUD, 10);
UARTCom::init(UARTCom::data);
controlTimer = millis();
uartTransmitTimer = millis();
@ -38,6 +36,7 @@ void setup(void)
void loop(void)
{
_2WD.PIDRegulate();
wheelLeft.PIDRegulate();
wheelRight.PIDRegulate();
UARTCom::read(UARTCom::data);
}