...
 
......@@ -4,6 +4,7 @@
#include "stdint.h"
#include "uart.h"
#include "arm_math.h"
#include "custom_type.h"
class Etape;
......
......@@ -9,7 +9,7 @@
#define INC_TYPE_ETAPES_H_
#include "GenericHeader.h"
#include "arm_math.h"
//#include "arm_math.h"
#include "custom_type.h"
class Robot;
......
......@@ -105,7 +105,7 @@ void Routine::updateEtape(){
case Calibration_oDrive:
//TODO Read oDrive State
break;
case Iinit_Actionneur:
case Init_Actionneur:
if (robot_->InitPositionActionneurs() == 1) {
robot_->setMatchPhase(Init_Position);
}
......
......@@ -24,7 +24,7 @@ class Robot;
enum MatchPhase{Idle = 0,
Calibration_oDrive,
Init_Position,
Iinit_Actionneur,
Init_Actionneur,
Match,
End,
};
......
......@@ -166,7 +166,6 @@ uint16_t Deplacement::generateLine(point_t start, point_t end){
k = 1;
} else {
j = i;
uint16_t plop = (2*((distance/2) - endRamp));
topSpeed = (2*((distance/2) - endRamp))/(float32_t)k;
}
}
......
......@@ -16,6 +16,7 @@ ODriveVit::ODriveVit(uint8_t motorId, Uart* uart, uint8_t inversion) {
_motorId = motorId;
_uart = uart;
_inversion = inversion;
readBufferIndex_ = 0;
_buffer[0] = 'v';
_buffer[1] = ' ';
......@@ -50,6 +51,48 @@ void ODriveVit::update(){}
void ODriveVit::disableMotor(){}
void ODriveVit::enableMotor(){}
void ODriveVit::RequestState(){
uint8_t dummyBuffer[50];
_uart->receiveMessage((uint8_t*)&dummyBuffer);
if (_motorId == 1) {
message_t message = {((uint8_t*)&readState1_),sizeof(readState1_)};
_uart->sendMessage(message);
} else {
message_t message = {((uint8_t*)&readState0_),sizeof(readState0_)};
_uart->sendMessage(message);
}
}
bool ODriveVit::isReady(){
uint8_t buffer[50];
uint8_t readLength;
readLength = _uart->receiveMessage((uint8_t*)&buffer);
if (readBufferIndex_ + readLength < ODRIVE_READBUFFER_SIZE){
for (int var = 0; var < readLength; ++var) {
readBuffer_[readBufferIndex_ + var] = buffer[var];
}
readBufferIndex_ += readLength;
if (readBufferIndex_>= 4){
if (readBuffer_[0] == 't' && readBuffer_[1] == 'r' && readBuffer_[2] == 'u' && readBuffer_[3] == 'e'){
readBufferIndex_ = 0;
return true;
}
}
if (readBufferIndex_>= 5){
if (readBuffer_[0] == 'f' && readBuffer_[1] == 'a' && readBuffer_[2] == 'l' && readBuffer_[3] == 's' && readBuffer_[4] == 'e'){
readBufferIndex_ = 0;
return false;
}
}
}
for (int var = 0; var < readBufferIndex_; ++var) {
if (readBuffer_[var] == '\n'){
readBufferIndex_ = 0;
}
}
return false;
}
void ODriveVit::send_odrive(int32_t speed){
uint32_t conv[3];
conv[0] = abs(speed);
......
......@@ -15,6 +15,8 @@
/* Defines -------------------------------------------------------------------*/
#define ODRIVE_READBUFFER_SIZE 10
/* Class -------------------------------------------------------------------*/
/**
......@@ -40,12 +42,19 @@ public:
void disableMotor();
void enableMotor();
void RequestState();
bool isReady();
private:
float32_t _consigneVitesse;
uint8_t _motorId;
Uart *_uart;
uint8_t _inversion;
uint8_t _buffer[12];
uint8_t readBuffer_[ODRIVE_READBUFFER_SIZE];
uint8_t readBufferIndex_;
char readState0_[26]= "r axis0.encoder.is_ready\n";
char readState1_[26]= "r axis1.encoder.is_ready\n";
char _intToString[201] = "00010203040506070809101112131415161718192021222324252627282930313233343536373839401424344445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899";
void send_odrive(int32_t speed);
......