Skip to content

Commit 9fce8e1

Browse files
SolidGeekSolidGeek
committed
First commit
1 parent 3b46681 commit 9fce8e1

File tree

10 files changed

+1361
-0
lines changed

10 files changed

+1361
-0
lines changed

‎examples/getVescValues/getVescValues.ino

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
/*
2+
Name: getVescValues.ino
3+
Created: 19-08-2018
4+
Author: SolidGeek
5+
Description: This example is made using a Arduino Micro (Atmega32u4) that has a HardwareSerial port (Serial1) seperated from the Serial port.
6+
A Arduino Nano or Uno that only has one Serial port will not be able to display the data returned.
7+
*/
8+
9+
#include <VescUart.h>
10+
11+
/** Initiate VescUart class */
12+
VescUart UART;
13+
14+
void setup() {
15+
16+
/** Setup Serial port to display data */
17+
Serial.begin(9600);
18+
19+
/** Setup UART port (Serial1 on Atmega32u4) */
20+
Serial1.begin(19200);
21+
22+
while (!Serial) {;}
23+
24+
/** Define which ports to use as UART */
25+
UART.setSerialPort(&Serial1);
26+
}
27+
28+
void loop() {
29+
30+
/** Call the function getVescValues() to acquire data from VESC */
31+
if ( UART.getVescValues() ) {
32+
33+
Serial.println(UART.data.rpm);
34+
Serial.println(UART.data.inpVoltage);
35+
Serial.println(UART.data.ampHours);
36+
Serial.println(UART.data.tachometerAbs);
37+
38+
}
39+
else
40+
{
41+
Serial.println("Failed to get data!");
42+
}
43+
44+
delay(50);
45+
}

‎keywords.txt

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#######################################
2+
# Syntax Coloring Map For vescUart
3+
#######################################
4+
5+
#######################################
6+
# Datatypes (KEYWORD1)
7+
#######################################
8+
9+
VescUart KEYWORD1
10+
11+
#######################################
12+
# Methods and Functions (KEYWORD2)
13+
#######################################
14+
setSerialPort
15+
setDebugPort
16+
getVescValues KEYWORD2
17+
printVescValues KEYWORD2
18+
setNunchuckValues
19+
printVescValues

‎library.properties

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
name=VescUart
2+
version=1.0.0
3+
author=SolidGeek
4+
maintainer=SolidGeek
5+
sentence=Library offering UART communication for VESC
6+
paragraph=VESC UART library
7+
category=Device Control
8+
url=https://solidgeek.dk
9+
architectures=*

‎src/VescUart.cpp

+280
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,280 @@
1+
#include "VescUart.h"
2+
#include <HardwareSerial.h>
3+
4+
5+
VescUart::VescUart(void){
6+
nunchuck.valXJoy = 127;
7+
nunchuck.valYJoy = 127;
8+
nunchuck.valLowerButton = false;
9+
nunchuck.valLowerButton = false;
10+
}
11+
12+
void VescUart::setSerialPort(HardwareSerial* _serialPort)
13+
{
14+
serialPort = _serialPort;
15+
}
16+
17+
void VescUart::setDebugPort(Stream* _debugPort)
18+
{
19+
debugPort = _debugPort;
20+
}
21+
22+
int VescUart::receiveUartMessage(uint8_t * payloadReceived) {
23+
24+
// Messages <= 255 starts with "2", 2nd byte is length
25+
// Messages > 255 starts with "3" 2nd and 3rd byte is length combined with 1st >>8 and then &0xFF
26+
27+
unsigned int counter = 0;
28+
unsigned int endMessage = 256;
29+
bool messageRead = false;
30+
uint8_t messageReceived[256];
31+
unsigned int lenPayload = 0;
32+
33+
unsigned long timeout = 0;
34+
timeout = millis() + 100;
35+
36+
while ( millis() < timeout && messageRead == false) {
37+
38+
while (serialPort->available()) {
39+
40+
messageReceived[counter++] = serialPort->read();
41+
42+
if (counter == 2) {
43+
44+
switch (messageReceived[0])
45+
{
46+
case 2:
47+
endMessage = messageReceived[1] + 5; //Payload size + 2 for sice + 3 for SRC and End.
48+
lenPayload = messageReceived[1];
49+
break;
50+
51+
case 3:
52+
// ToDo: Add Message Handling > 255 (starting with 3)
53+
break;
54+
55+
default:
56+
if( debugPort != NULL ){
57+
debugPort->println("No start bit");
58+
}
59+
break;
60+
}
61+
}
62+
63+
if (counter >= sizeof(messageReceived)) {
64+
break;
65+
}
66+
67+
if (counter == endMessage && messageReceived[endMessage - 1] == 3) {
68+
messageReceived[endMessage] = 0;
69+
if (debugPort != NULL) {
70+
debugPort->println("End of message reached!");
71+
}
72+
messageRead = true;
73+
break; //Exit if end of message is reached, even if there is still more data in buffer.
74+
}
75+
}
76+
}
77+
78+
if(messageRead == false && debugPort != NULL ) {
79+
debugPort->println("Timeout");
80+
}
81+
82+
bool unpacked = false;
83+
if (messageRead) {
84+
unpacked = unpackPayload(messageReceived, endMessage, payloadReceived);
85+
}
86+
87+
if (unpacked) {
88+
// Message was read
89+
return lenPayload;
90+
}
91+
else {
92+
// No Message Read
93+
return 0;
94+
}
95+
}
96+
97+
98+
bool VescUart::unpackPayload(uint8_t * message, int lenMes, uint8_t * payload) {
99+
100+
uint16_t crcMessage = 0;
101+
uint16_t crcPayload = 0;
102+
103+
// Rebuild crc:
104+
crcMessage = message[lenMes - 3] << 8;
105+
crcMessage &= 0xFF00;
106+
crcMessage += message[lenMes - 2];
107+
108+
if(debugPort!=NULL){
109+
debugPort->print("SRC received: "); debugPort->println(crcMessage);
110+
}
111+
112+
// Extract payload:
113+
memcpy(payload, &message[2], message[1]);
114+
115+
crcPayload = crc16(payload, message[1]);
116+
117+
if( debugPort != NULL ){
118+
debugPort->print("SRC calc: "); debugPort->println(crcPayload);
119+
}
120+
121+
if (crcPayload == crcMessage) {
122+
if( debugPort != NULL ) {
123+
debugPort->print("Received: ");
124+
serialPrint(message, lenMes); debugPort->println();
125+
126+
debugPort->print("Payload : ");
127+
serialPrint(payload, message[1] - 1); debugPort->println();
128+
}
129+
130+
return true;
131+
}else{
132+
return false;
133+
}
134+
}
135+
136+
137+
int VescUart::packSendPayload(uint8_t * payload, int lenPay) {
138+
139+
uint16_t crcPayload = crc16(payload, lenPay);
140+
int count = 0;
141+
uint8_t messageSend[256];
142+
143+
if (lenPay <= 256)
144+
{
145+
messageSend[count++] = 2;
146+
messageSend[count++] = lenPay;
147+
}
148+
else
149+
{
150+
messageSend[count++] = 3;
151+
messageSend[count++] = (uint8_t)(lenPay >> 8);
152+
messageSend[count++] = (uint8_t)(lenPay & 0xFF);
153+
}
154+
155+
memcpy(&messageSend[count], payload, lenPay);
156+
157+
count += lenPay;
158+
messageSend[count++] = (uint8_t)(crcPayload >> 8);
159+
messageSend[count++] = (uint8_t)(crcPayload & 0xFF);
160+
messageSend[count++] = 3;
161+
messageSend[count] = '\0';
162+
163+
if(debugPort!=NULL){
164+
debugPort->print("UART package send: "); serialPrint(messageSend, count);
165+
}
166+
167+
// Sending package
168+
serialPort->write(messageSend, count);
169+
170+
// Returns number of send bytes
171+
return count;
172+
}
173+
174+
175+
bool VescUart::processReadPacket(uint8_t * message) {
176+
177+
COMM_PACKET_ID packetId;
178+
int32_t ind = 0;
179+
180+
packetId = (COMM_PACKET_ID)message[0];
181+
message++; // Removes the packetId from the actual message (payload)
182+
183+
switch (packetId){
184+
case COMM_GET_VALUES:
185+
186+
ind = 4; // Skip the first 4 bytes
187+
data.avgMotorCurrent = buffer_get_float32(message, 100.0, &ind);
188+
data.avgInputCurrent = buffer_get_float32(message, 100.0, &ind);
189+
ind += 8; // Skip the next 8 bytes
190+
data.dutyCycleNow = buffer_get_float16(message, 1000.0, &ind);
191+
data.rpm = buffer_get_int32(message, &ind);
192+
data.inpVoltage = buffer_get_float16(message, 10.0, &ind);
193+
data.ampHours = buffer_get_float32(message, 10000.0, &ind);
194+
data.ampHoursCharged = buffer_get_float32(message, 10000.0, &ind);
195+
ind += 8; // Skip the next 8 bytes
196+
data.tachometer = buffer_get_int32(message, &ind);
197+
data.tachometerAbs = buffer_get_int32(message, &ind);
198+
return true;
199+
200+
break;
201+
202+
default:
203+
return false;
204+
break;
205+
}
206+
}
207+
208+
bool VescUart::getVescValues(void) {
209+
210+
uint8_t command[1] = { COMM_GET_VALUES };
211+
uint8_t payload[256];
212+
213+
packSendPayload(command, 1);
214+
// delay(1); //needed, otherwise data is not read
215+
216+
int lenPayload = receiveUartMessage(payload);
217+
218+
if (lenPayload > 55) {
219+
bool read = processReadPacket(payload); //returns true if sucessful
220+
return read;
221+
}
222+
else
223+
{
224+
return false;
225+
}
226+
}
227+
228+
void VescUart::setNunchuckValues() {
229+
int32_t ind = 0;
230+
uint8_t payload[11];
231+
232+
payload[ind++] = COMM_SET_CHUCK_DATA;
233+
payload[ind++] = nunchuck.valXJoy;
234+
payload[ind++] = nunchuck.valYJoy;
235+
buffer_append_bool(payload, nunchuck.valLowerButton, &ind);
236+
buffer_append_bool(payload, nunchuck.valUpperButton, &ind);
237+
238+
// Acceleration Data. Not used, Int16 (2 byte)
239+
payload[ind++] = 0;
240+
payload[ind++] = 0;
241+
payload[ind++] = 0;
242+
payload[ind++] = 0;
243+
payload[ind++] = 0;
244+
payload[ind++] = 0;
245+
246+
if(debugPort != NULL){
247+
debugPort->println("Data reached at setNunchuckValues:");
248+
debugPort->print("valXJoy = "); debugPort->print(nunchuck.valXJoy); debugPort->print(" valYJoy = "); debugPort->println(nunchuck.valYJoy);
249+
debugPort->print("LowerButton = "); debugPort->print(nunchuck.valLowerButton); debugPort->print(" UpperButton = "); debugPort->println(nunchuck.valUpperButton);
250+
}
251+
252+
packSendPayload(payload, 11);
253+
}
254+
255+
void VescUart::serialPrint(uint8_t * data, int len) {
256+
if(debugPort != NULL){
257+
for (int i = 0; i <= len; i++)
258+
{
259+
debugPort->print(data[i]);
260+
debugPort->print(" ");
261+
}
262+
263+
debugPort->println("");
264+
}
265+
}
266+
267+
268+
void VescUart::printVescValues() {
269+
if(debugPort != NULL){
270+
debugPort->print("avgMotorCurrent: "); debugPort->println(data.avgMotorCurrent);
271+
debugPort->print("avgInputCurrent: "); debugPort->println(data.avgInputCurrent);
272+
debugPort->print("dutyCycleNow: "); debugPort->println(data.dutyCycleNow);
273+
debugPort->print("rpm: "); debugPort->println(data.rpm);
274+
debugPort->print("inputVoltage: "); debugPort->println(data.inpVoltage);
275+
debugPort->print("ampHours: "); debugPort->println(data.ampHours);
276+
debugPort->print("ampHoursCharges: "); debugPort->println(data.ampHoursCharged);
277+
debugPort->print("tachometer: "); debugPort->println(data.tachometer);
278+
debugPort->print("tachometerAbs: "); debugPort->println(data.tachometerAbs);
279+
}
280+
}

0 commit comments

Comments
 (0)