29 void servo360_monitorRun(
void)
31 #ifdef _servo360_monitor_
38 servo360_monitor_start();
39 suppressFbDisplay = 0;
49 void servo360_monitorEnd(
void)
51 #ifdef _servo360_monitor_
52 suppressFbDisplay = 1;
53 servo360_monitor_stop();
58 #ifdef _servo360_monitor_
61 volatile int suppressFbDisplay = 0;
62 static char bufcons[20];
63 static volatile int val;
65 static volatile int operation;
66 static volatile int pvStart;
67 static volatile int angleFixedPrev;
69 volatile int tElapsed;
72 servo360_t fbt[S360_DEVS_MAX];
80 void servo360_monitor_start()
84 _fb360c.cntPrev =
CNT;
85 consoleCog =
cog_run(console, 512);
90 void servo360_monitor_stop()
134 servo360_waitServoCtrllEdgeNeg(_fb360c.devCount - 1);
136 tElapsed = cnt - _fb360c.cntPrev;
137 _fb360c.cntPrev = cnt;
139 ready = terminal_checkForValue(term, &value);
143 suppressFbDisplay = !suppressFbDisplay;
145 else if(ready ==
'a' || ready ==
'A')
147 operation = S360_POSITION;
148 dprint(term,
"\rEnter angle: ");
150 else if(ready ==
's' || ready ==
'S')
152 operation = S360_SPEED;
153 dprint(term,
"\rEnter speed: ");
155 else if(ready ==
'g' || ready ==
'G')
157 operation = S360_GOTO;
158 _fs[0].csop = S360_GOTO;
159 dprint(term,
"\rGoto Mode: ");
161 else if(ready ==
'm' || ready ==
'M')
163 operation = S360_MONITOR;
164 _fs[0].csop = S360_MONITOR;
165 operation = S360_MONITOR;
166 dprint(term,
"\rMonitor Mode: ");
171 dprint(term,
"\rvalue = %d\r", value);
172 dprint(term,
"\rEnter value: ");
173 if(operation == S360_POSITION)
177 else if(operation == S360_SPEED)
181 else if(operation == S360_GOTO)
185 else if(operation == S360_MONITOR)
189 dprint(term,
"sp = %d\r", _fs[0].sp);
190 dprint(term,
"operation = %d\r", operation);
194 if(!suppressFbDisplay)
202 while(
lockset(_fb360c.lock360));
203 for(
int p = 0; p < _fb360c.devCount; p++)
210 for(
int p = 0; p < _fb360c.devCount; p++)
222 dprint(term,
"pc: %d, id: %d, csop: %d\r", _fb360c.pulseCount, p, fbt[p].csop);
224 if(fbt[p].csop == S360_POSITION)
226 dprint(term,
"sp: %d, pv: %d, op: %d, e: %d, "\
227 "p: %d, i: %d, d: %d th: %d\r",
228 fbt[p].sp, fbt[p].angle, fbt[p].op, fbt[p].er,
229 fbt[p].p, fbt[p].i, fbt[p].d, fbt[p].theta);
232 else if(fbt[p].csop == S360_SPEED)
258 dprint(term,
"spR: %d, spT: %d, spTT: %d, spM: %d\r",
259 fbt[p].speedReq, fbt[p].speedTarget, _fs[p].speedTargetTemp, fbt[p].speedMeasured);
260 dprint(term,
"aC: %d, aM: %d, aE: %d\r",
261 fbt[p].angleCalc, fbt[p].angle, fbt[p].angleError);
262 dprint(term,
"pV: %d, iV: %d, dV: %d\r",
263 fbt[p].pV, fbt[p].iV, fbt[p].dV);
264 dprint(term,
"sd: %d, lag: %d, spdO: %d\r",
265 fbt[p].stepDir, fbt[p].lag, fbt[p].speedOut);
266 dprint(term,
"tf: %d, pidV: %d, tf+pidV: %d",
267 fbt[p].drive, fbt[p].opV, fbt[p].opPidV);
277 else if(fbt[p].csop == S360_GOTO)
280 "spR: %d, spT: %d, spM: %d, "\
281 "csop: %d, aT: %d, a: %d, "\
282 "tG: %d, tD: %d, af: %d\r",
283 fbt[p].speedReq, fbt[p].speedTarget, fbt[p].speedMeasured,
284 fbt[p].csop, fbt[p].angleTarget, fbt[p].angle,
285 fbt[p].ticksGuard, fbt[p].ticksDiff, fbt[p].approachFlag
289 else if(fbt[p].csop == S360_MONITOR)
295 dprint(term,
"dc: %d, theta: %d, turns: %d, angle: %d\r",
296 fbt[p].dc, fbt[p].theta, fbt[p].turns, fbt[p].angle);
306 int terminal_checkForValue(
fdserial *connection,
int *value)
312 if((bufcons[n] >=
' ') && (bufcons[n] <=
'z'))
314 dprint(connection,
"%c", bufcons[n]);
318 dprint(connection,
"[%d]", bufcons[n]);
334 int temp = bufcons[n];
338 else if((bufcons[n] ==
'\r') || (bufcons[n] ==
'\n'))
340 sscan(bufcons,
"%d", &val);
343 dprint(connection,
"\rval = %d\r", val);
344 dprint(connection,
"Enter value: ");
353 #endif //_servo360_monitor_