/
UART_VESC.ino
132 lines (113 loc) · 4.57 KB
/
UART_VESC.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
//Library for VESC UART
#include <VescUartControl.h>
// global variables
#import "global_variables.h"
#define USE_PATCHED_VESC_FW_2_18
unsigned long count;
bool VESC_setup()
{
Vesc_Right.begin(&Serial2);
Vesc_Right.UartSetCurrent(0.0);
Vesc_Right.UartSetCurrentBrake(0.0);
Serial.println("VESC_setup_right_begin");
delay(200);
#if defined (USE_PATCHED_VESC_FW_2_18)
if (Vesc_Right.UartGetLimits(limitsVescRight))
{
Serial.print("currentMax_Vesc_Right: "); Serial.println(limitsVescRight.currentMax);
Serial.print("currentMin_Vesc_Right: "); Serial.println(limitsVescRight.currentMin);
Serial.print("inCurrentMax_Vesc_Right: "); Serial.println(limitsVescRight.inCurrentMax);
Serial.print("inCurrentMin_Vesc_Right: "); Serial.println(limitsVescRight.inCurrentMin);
Serial.print("absCurrentMax_Vesc_Right: "); Serial.println(limitsVescRight.absCurrentMax);
Serial.print("minVin_Vesc_Right: "); Serial.println(limitsVescRight.minVin);
Serial.print("maxVin_Vesc_Right: "); Serial.println(limitsVescRight.maxVin);
Serial.print("batteryCutStart_Vesc_Right: "); Serial.println(limitsVescRight.batteryCutStart);
Serial.print("batteryCutEnd_Vesc_Right: "); Serial.println(limitsVescRight.batteryCutEnd);
Serial.print("tempFetStart_Vesc_Right: "); Serial.println(limitsVescRight.tempFetStart);
Serial.print("tempFetEnd_Vesc_Right: "); Serial.println(limitsVescRight.tempFetEnd);
Serial.print("tempMotStart_Vesc_Right: "); Serial.println(limitsVescRight.tempMotorStart);
Serial.print("tempMotEnd_Vesc_Right: "); Serial.println(limitsVescRight.tempMotorEnd);
Serial.print("max_Duty_Vesc_Right: "); Serial.println(limitsVescRight.max_Duty);
VESC_right_limits_ok = 1;
}
else
{
Serial.println("could not get limits from Vesc_Right");
VESC_right_limits_ok = 0;
}
#else
limitsVescRight.currentMax=MAX_AMP_MOTOR;
limitsVescRight.currentMin=-MAX_AMP_MOTOR;
#endif //#if defined (USE_PATCHED_VESC_FW_2_18)
Serial.println("VESC_setup_right_end");
delay(100);
Vesc_Left.begin(&Serial3);
Vesc_Left.UartSetCurrent(0.0);
Vesc_Left.UartSetCurrentBrake(0.0);
Serial.println("VESC_setup_left_begin");
delay(200);
#if defined (USE_PATCHED_VESC_FW_2_18)
if (Vesc_Left.UartGetLimits(limitsVescLeft))
{
Serial.print("currentMax_Vesc_Left: "); Serial.println(limitsVescLeft.currentMax);
Serial.print("currentMin_Vesc_Left: "); Serial.println(limitsVescLeft.currentMin);
Serial.print("inCurrentMax_Vesc_Left: "); Serial.println(limitsVescLeft.inCurrentMax);
Serial.print("inCurrentMin_Vesc_Left: "); Serial.println(limitsVescLeft.inCurrentMin);
Serial.print("absCurrentMax_Vesc_Left: "); Serial.println(limitsVescLeft.absCurrentMax);
Serial.print("minVin_Vesc_Left: "); Serial.println(limitsVescLeft.minVin);
Serial.print("maxVin_Vesc_Left: "); Serial.println(limitsVescLeft.maxVin);
Serial.print("batteryCutStart_Vesc_Left: "); Serial.println(limitsVescLeft.batteryCutStart);
Serial.print("batteryCutEnd_Vesc_Left: "); Serial.println(limitsVescLeft.batteryCutEnd);
Serial.print("tempFetStart_Vesc_Left: "); Serial.println(limitsVescLeft.tempFetStart);
Serial.print("tempFetEnd_Vesc_Left: "); Serial.println(limitsVescLeft.tempFetEnd);
Serial.print("tempMotStart_Vesc_Left: "); Serial.println(limitsVescLeft.tempMotorStart);
Serial.print("tempMotEnd_Vesc_Left: "); Serial.println(limitsVescLeft.tempMotorEnd);
Serial.print("max_Duty_Vesc_Left: "); Serial.println(limitsVescLeft.max_Duty);
VESC_left_limits_ok = 1;
}
else
{
Serial.println("could not get limits from Vesc_Left");
VESC_left_limits_ok = 0;
}
#else
limitsVescLeft.currentMax=MAX_AMP_MOTOR;
limitsVescLeft.currentMin=-MAX_AMP_MOTOR;
#endif //#if defined (USE_PATCHED_VESC_FW_2_18)
Serial.println("VESC_setup_left_end");
delay(100);
if (VESC_right_limits_ok==1 & VESC_left_limits_ok==1) {return true;}
else {return false;}
}
void VESC_update()
{
Vesc_Right.UartSetCurrent(mapfloat(power_right, -MAX_PID, MAX_PID, limitsVescRight.currentMin, limitsVescRight.currentMax));
Vesc_Left.UartSetCurrent(mapfloat(power_left, -MAX_PID, MAX_PID, limitsVescLeft.currentMin, limitsVescLeft.currentMax));
}
bool VESC_read()
{
if (Vesc_Right.UartGetValue(measuredVescRight))
{
VESC_right_measurements_ok=1;
}
else
{
VESC_right_measurements_ok=0;
}
if (Vesc_Left.UartGetValue(measuredVescLeft))
{
VESC_left_measurements_ok=1;
}
else
{
VESC_left_measurements_ok=0;
}
if(VESC_right_measurements_ok ==1 || VESC_left_measurements_ok == 1)
{
return true;
}
else
{
return false;
}
}