com/robotCom.h

@@ -4,6 +4,7 @@
 
 #include "stdint.h"
 #include "uart.h"
+#include "arm_math.h"
 #include "custom_type.h"
 
 class Etape;

routine_slave/Etapes.h

@@ -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;

routine_slave/routine.cpp

@@ -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);
 				}

routine_slave/routine.h

@@ -24,7 +24,7 @@ class Robot;
 enum MatchPhase{Idle = 0,
 				Calibration_oDrive,
 				Init_Position,
-				Iinit_Actionneur,
+				Init_Actionneur,
 				Match,
 				End,
 				};

trajectoire/deplacement.cpp

@@ -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;
 			}
 		}

trajectoire/oDriveVit.cpp

@@ -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);

trajectoire/oDriveVit.h

@@ -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);