-
Notifications
You must be signed in to change notification settings - Fork 3
/
mtrmgr.c
143 lines (129 loc) · 4.56 KB
/
mtrmgr.c
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
132
133
134
135
136
137
138
139
140
141
142
143
/**
* @file PROS Smart Motor Library (SML)
* @brief Provides additional functionality for interfacing with the Vex motors.
* Allows for linear scaling to account for the MC29s (trueSpeed), slewing,
* and easier configuration for motor characteristics (reversal, etc.)
*
* @author Elliot Berman, Jonathan Bayless, Brian Hanford
*
*/
#include "mtrmgr.h"
static Motor motor[10];
static Mutex mutex[10];
static TaskHandle motorManagerTaskHandle;
/**
* @brief The default recalculate function for RecalculateCommanded (takes input and returns it)
* This method is only accessible to this file for organizational purposes and may be opened to other files.
* @param in
* Input value
* @returns Returns in
*/
static int _defaultRecalculate(int in)
{
return in;
}
/**
* @brief The motor manager task processes all the motors and determines if a change
* needs to be made to the motor speed and executes the change if necessary
* This task is initialized by the InitializeMotorManager method.
* Do not manually create this task.
*/
static void _motorManagerTask(void *none)
{
unsigned long int now;
while (true)
{
now = millis();
for (int i = 0; i < NUM_MOTORS; i++)
{
// the motorGet function gets a motor between channels 1-10. motor[index] goes from 0-9
if (motorGet(i+1) != motor[i].cmd) // Motor has not been set to target
{
int current = motorGet(i+1);
int commanded = motor[i].cmd;
float slew = motor[i].slewrate;
int out = 0;
if (slew == 0) //setting a slew rate of zero prevents output
continue;
if (commanded != current) {
//Slewing
if (commanded > current) {
out = current + (int)(slew * (millis() - motor[i]._lastUpdate)); //extrapolate largest allowable acceleration
if (out > commanded) //requested change in output is lower than maximum possible
out = commanded;
}
else if (commanded < current) {
out = current - (int)(slew * (millis() - motor[i]._lastUpdate)); //extrapolate largest allowable acceleration
if (out < commanded) //requested change in output is lower than maximum possible
out = commanded;
}
out = motor[i].recalculate(out);
}
// Grab mutex if possible, if it's not available (being changed by MotorSet()), skip the motor check.
if (!mutexTake(mutex[i], 5))
continue;
motorSet(i+1, out);
mutexGive(mutex[i]);
}
motor[i]._lastUpdate = millis();
}
taskDelayUntil(&now, SLEW_DELTA_T);
}
}
void motorManagerInit()
{
for (int i = 0; i < NUM_MOTORS; i++)
{
mutex[i] = mutexCreate();
motor[i].recalculate = &_defaultRecalculate;
}
motorManagerTaskHandle = taskCreate(_motorManagerTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_HIGHEST-1);
}
void blrsMotorInit(int port, bool inverted, float slewrate, int (*recalculate)(int))
{
if (port < 1 || port > 10)
return;
port--;
motor[port].port = port + 1;
motor[port].inverted = inverted ? -1 : 1;
motor[port].slewrate = slewrate;
if(!recalculate) {
motor[port].recalculate = &_defaultRecalculate;
}
else {
motor[port].recalculate = recalculate;
}
}
void motorManagerStop()
{
if (motorManagerTaskHandle != NULL) // passing NULL kills current thread, so don't allow that to happen
taskDelete(motorManagerTaskHandle);
}
bool blrsMotorSet(int port, int commanded, bool immediate)
{
if (port > 10 || port < 1)
return false;
if (commanded > 127)
commanded = 127;
else if (commanded < -127)
commanded = -127;
port--;
if (immediate)
{
// if not able to take control in time then exit and unsuccessful
if (!mutexTake(mutex[port], MUTEX_TAKE_TIMEOUT)) {
return false;
}
motorSet(port + 1, commanded * motor[port].inverted);
mutexGive(mutex[port]);
}
motor[port].cmd = commanded * motor[port].inverted;
return true;
}
int blrsMotorGet(int port)
{
if (port > 10 || port < 1)
return 0;
port--;
return motor[port].cmd * motor[port].inverted;
}