/
MotorController.cpp
212 lines (170 loc) · 5.63 KB
/
MotorController.cpp
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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#include "MotorController.h"
MotorController g_motorCtrl;
MotorController::MotorController(void)
{
;
}
/** \brief Initialize the motor controller class
*
* \param void
* \return void
*
*/
void MotorController::init(void)
{
printf("MotorController::init\n");
// register the motor handlers
regHanlders();
// init the two motorsfloat
motorLeft.init( INDEX_MOTOR_LEFT, &motorCtrlHandle, false);
motorRight.init(INDEX_MOTOR_RIGHT, &motorCtrlHandle, true);
printf("MotorController::init - DONE~!\n");
}
/** \brief Finalize and clear everything, shut down the motors, clear the registered handles
*
* \param void
* \return void
*
*/
void MotorController::fin(void)
{
printf("MotorController::fin\n");
// execute the fin functions of the two motors to stop them
motorLeft.fin();
motorRight.fin();
// clean all the rest registered handlers
clrHandlers();
printf("MotorController::fin - DONE~!\n");
}
//******************************* Handlers ********************************
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorAttachHandler(CPhidgetHandle MC, void *userptr)
{
int serialNo;
const char *name;
CPhidget_getDeviceName (MC, &name);
CPhidget_getSerialNumber(MC, &serialNo);
printf("%s %10d attached!\n", name, serialNo);
return 0;
}
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorDetachHandler(CPhidgetHandle MC, void *userptr)
{
int serialNo;
const char *name;
CPhidget_getDeviceName (MC, &name);
CPhidget_getSerialNumber(MC, &serialNo);
printf("%s %10d detached!\n", name, serialNo);
return 0;
}
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorErrorHandler(CPhidgetHandle MC, void *userptr, int ErrorCode, const char *Description)
{
printf("Error handled. %d - %s\n", ErrorCode, Description);
return 0;
}
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorInputChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int index, int state)
{
#if (DEBUG_MODE_LOW)
printf("Input %d > state: %d\n", index, state);
#endif
return 0;
}
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorVelocityChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int index, double value)
{
#if (DEBUG_MODE_LOW)
printf("Motor %d > Current Speed: %f\n", index, value);
#endif
return 0;
}
int
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
__stdcall
#endif
MotorCurrentChangeHandler(CPhidgetMotorControlHandle MC, void *usrptr, int index, double value)
{
#if (DEBUG_MODE_LOW)
printf("Motor: %d > Current Draw: %f\n", index, value);
#endif
return 0;
}
int MotorDisplayProperties(CPhidgetMotorControlHandle phid)
{
int serialNo, version, numInputs, numMotors;
const char* ptr;
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
CPhidgetMotorControl_getInputCount(phid, &numInputs);
CPhidgetMotorControl_getMotorCount(phid, &numMotors);
#if (DEBUG_MODE_LOW)
printf("%s\n", ptr);
printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
printf("# Inputs: %d\n# Motors: %d\n", numInputs, numMotors);
#endif
return 0;
}
void MotorController::clrHandlers(void)
{
printf("MotorController::clrHandlers\n");
CPhidget_close( (CPhidgetHandle)motorCtrlHandle);
CPhidget_delete((CPhidgetHandle)motorCtrlHandle);
}
int MotorController::regHanlders(void)
{
printf("MotorController::regHandlers\n");
#if (!DEBUG_MODE_PC)
int result;
const char *err;
#endif
//Declare a motor control handle
motorCtrlHandle = 0;
//create the motor control object
CPhidgetMotorControl_create(&motorCtrlHandle);
//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motorCtrlHandle, MotorAttachHandler, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motorCtrlHandle, MotorDetachHandler, NULL);
CPhidget_set_OnError_Handler( (CPhidgetHandle)motorCtrlHandle, MotorErrorHandler, NULL);
//Registers a callback that will run if an input changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnInputChange_Handler(motorCtrlHandle, MotorInputChangeHandler, NULL);
//Registers a callback that will run if a motor changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnVelocityChange_Handler(motorCtrlHandle, MotorVelocityChangeHandler, NULL);
//Registers a callback that will run if the current draw changes.
//Requires the handle for the Phidget, the function that will be called, and a arbitrary pointer that will be supplied to the callback function (may be NULL).
CPhidgetMotorControl_set_OnCurrentChange_Handler(motorCtrlHandle, MotorCurrentChangeHandler, NULL);
//open the motor control for device connections
CPhidget_open((CPhidgetHandle)motorCtrlHandle, -1);
//get the program to wait for a motor control device to be attached
printf("Waiting for MotorControl to be attached....\n");
#if (!DEBUG_MODE_PC)
if ((result = CPhidget_waitForAttachment((CPhidgetHandle)motorCtrlHandle, 10000)))
{
CPhidget_getErrorDescription(result, &err);
printf("Problem waiting for attachment: %s\n", err);
return 0;
}
#endif
//Display the properties of the attached motor control device
MotorDisplayProperties(motorCtrlHandle);
//all done, exit
return 0;
}