LMS 2012
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
d_pwm.c
Go to the documentation of this file.
1 /*
2  * LEGO® MINDSTORMS EV3
3  *
4  * Copyright (C) 2010-2013 The LEGO Group
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  */
20 
25 #define HW_ID_SUPPORT
26 
27 #include "../../lms2012/source/lms2012.h"
28 #include "../../lms2012/source/am1808.h"
29 
30 int Hw = 0;
31 int HwInvBits = 0;
32 
33 #define MODULE_NAME "pwm_module"
34 #define DEVICE1_NAME PWM_DEVICE
35 #define DEVICE2_NAME MOTOR_DEVICE
36 
37 
38 #define SOFT_TIMER_MS 2
39 #define SOFT_TIMER_SETUP (SOFT_TIMER_MS * 1000000)
40 
41 /*
42  * NO_OF_TACHO_SAMPLES holds the number of recent tacho samples
43  */
44 #define NO_OF_TACHO_SAMPLES 128
45 #define NO_OF_OUTPUT_PORTS 4
46 
47 #define MAX_PWM_CNT (10000)
48 #define MAX_SPEED (100)
49 #define SPEED_PWMCNT_REL (100) //(MAX_PWM_CNT/MAX_SPEED)
50 #define RAMP_FACTOR (1000)
51 #define MAX_SYNC_MOTORS (2)
52 
53 //#define COUNTS_PER_PULSE_LM 25600L
54 //#define COUNTS_PER_PULSE_MM 16200L
55 
56 #define COUNTS_PER_PULSE_LM 12800L
57 #define COUNTS_PER_PULSE_MM 8100L
58 
59 enum
60 {
61  SAMPLES_BELOW_SPEED_25 = 0,
62  SAMPLES_SPEED_25_50 = 1,
63  SAMPLES_SPEED_50_75 = 2,
64  SAMPLES_ABOVE_SPEED_75 = 3,
65  NO_OF_SAMPLE_STEPS = 4
66 };
67 
68 
69 /*
70  * Defines related to hardware PWM output
71  */
72 // TBCTL (Time-Base Control)
73 // = = = = = = = = = = = = = = = = = = = = = = = = = =
74 #define TB_COUNT_UP 0x0 // TBCNT MODE bits
75 #define TB_COUNT_FREEZE 0x3 // TBCNT MODE bits
76 #define TB_DISABLE 0x0 // PHSEN bit
77 #define TB_ENABLE 0x4
78 #define TB_SHADOW 0x0 // PRDLD bit
79 #define TB_IMMEDIATE 0x8
80 #define TB_SYNC_DISABLE 0x30 // SYNCOSEL bits
81 #define TB_HDIV1 0x0 // HSPCLKDIV bits
82 #define TB_DIV1 0x0 // CLKDIV bits
83 #define TB_UP 0x2000 // PHSDIR bit
84 
85 // CMPCTL (Compare Control)
86 // = = = = = = = = = = = = = = = = = = = = = = = = = =
87 #define CC_CTR_A_ZERO 0x0 // LOADAMODE bits
88 #define CC_CTR_B_ZERO 0x0
89 #define CC_A_SHADOW 0x00 // SHDWAMODE and SHDWBMODE bits
90 #define CC_B_SHADOW 0x00
91 
92 #define TBCTL 0x0
93 #define TBPHS 0x3
94 #define TBCNT 0x4
95 #define TBPRD 0x5
96 #define CMPCTL 0x7
97 #define CMPA 0x9
98 #define CMPB 0xA
99 #define AQCTLA 0xB
100 #define AQCTLB 0xC
101 
102 
103 /* SYS config register configuration and bits*/
104 enum
105 {
106  CFGCHIP1 = 0x60,
107 };
108 
109 enum
110 {
111  TBCLKSYNC = 0x00001000,
112 };
113 
114 
115 /* TIMER64 register configuration */
116 enum
117 {
118  REVID = 0,
119  EMUMGT = 1,
120  GPINTGPEN = 2,
121  GPDATGPDIR = 3,
122  TIM12 = 4,
123  TIM34 = 5,
124  PRD12 = 6,
125  PRD34 = 7,
126  TCR = 8,
127  TGCR = 9,
128  WDTCR = 10,
129  NOTUSED1 = 11,
130  NOTUSED2 = 12,
131  REL12 = 13,
132  REL34 = 14,
133  CAP12 = 15,
134  CAP34 = 16,
135  NOTUSED3 = 17,
136  NOTUSED4 = 18,
137  NOTUSED5 = 19,
138  NOTUSED6 = 20,
139  NOTUSED7 = 21,
140  NOTUSED8 = 22,
141  INTCTLSTAT = 23,
142  CMP0 = 24,
143  CMP1 = 25,
144  CMP2 = 26,
145  CMP3 = 27,
146  CMP4 = 28,
147  CMP5 = 39,
148  CMP6 = 30,
149  CMP7 = 31,
150 };
151 
152 /* eCAP Register configuration */
153 enum
154 {
155  TSCTR = 0,
156  CTRPHS = 2,
157  CAP1 = 4,
158  CAP2 = 6,
159  CAP3 = 8,
160  CAP4 = 10,
161  ECCTL1 = 20,
162  ECCTL2 = 21,
163  ECEINT = 22,
164  ECFLG = 23,
165  ECCLR = 24,
166  ECFRC = 25,
167  ECAP_REVID = 46,
168 };
169 
170 
171 #define NON_INV 1
172 #define INV -1
173 
174 
175 enum
176 {
177  FORWARD,
178  BACKWARD,
179  BRAKE,
180  COAST,
181 };
182 
183 enum
184 {
185  UNLIMITED_UNREG,
186  UNLIMITED_REG,
187  LIMITED_REG_STEPUP,
188  LIMITED_REG_STEPCONST,
189  LIMITED_REG_STEPDOWN,
190  LIMITED_UNREG_STEPUP,
191  LIMITED_UNREG_STEPCONST,
192  LIMITED_UNREG_STEPDOWN,
193  LIMITED_REG_TIMEUP,
194  LIMITED_REG_TIMECONST,
195  LIMITED_REG_TIMEDOWN,
196  LIMITED_UNREG_TIMEUP,
197  LIMITED_UNREG_TIMECONST,
198  LIMITED_UNREG_TIMEDOWN,
199  LIMITED_STEP_SYNC,
200  LIMITED_TURN_SYNC,
201  LIMITED_DIFF_TURN_SYNC,
202  SYNCED_SLAVE,
203  RAMP_DOWN_SYNC,
204  HOLD,
205  BRAKED,
206  STOP_MOTOR,
207  IDLE,
208 };
209 
210 enum
211 {
212  UNUSED_SYNC_MOTOR = 0xFF
213 };
214 
215 
216 typedef struct
217 {
218  SLONG IrqTacho;
219  SLONG TachoCnt;
220  SLONG TachoSensor;
221  SLONG TimeCnt;
222  SLONG TimeInc;
223  SLONG OldTachoCnt;
224  SLONG TachoCntUp;
225  SLONG TachoCntConst;
226  SLONG TachoCntDown;
227  SLONG RampUpFactor;
228  SLONG RampUpOffset;
229  SLONG RampDownFactor;
230  SLONG PVal;
231  SLONG IVal;
232  SLONG DVal;
233  SLONG OldSpeedErr;
234  SLONG Power;
235  SLONG TargetPower;
236  SLONG Dir;
237  SBYTE Speed;
238  SBYTE TargetSpeed;
239  SBYTE TargetBrake;
240  UBYTE BrakeAfter;
241  UBYTE LockRampDown;
242  SBYTE Pol;
243  UBYTE Direction;
244  UBYTE Type;
245  UBYTE State;
246  UBYTE TargetState;
247  UBYTE Owner;
248  UBYTE Mutex;
249  UBYTE DirChgPtr;
250  SWORD TurnRatio;
251 }MOTOR;
252 
253 
254 typedef struct
255 {
256  ULONG TachoArray[NO_OF_TACHO_SAMPLES];
257  UBYTE ArrayPtr;
258  UBYTE ArrayPtrOld;
259 }TACHOSAMPLES;
260 
261 
262 static int ModuleInit(void);
263 static void ModuleExit(void);
264 
265 static irqreturn_t IntA (int irq, void * dev);
266 static irqreturn_t IntB (int irq, void * dev);
267 static irqreturn_t IntC (int irq, void * dev);
268 static irqreturn_t IntD (int irq, void * dev);
269 
270 UBYTE dCalculateSpeed(UBYTE No, SBYTE *pSpeed);
271 void SetGpioRisingIrq(UBYTE PinNo, irqreturn_t (*IntFuncPtr)(int, void *));
272 void GetSyncDurationCnt(SLONG *pCount0, SLONG *pCount1);
273 void CheckforEndOfSync(void);
274 
275 
276 void SetDutyMA(ULONG Duty);
277 void SetDutyMB(ULONG Duty);
278 void SetDutyMC(ULONG Duty);
279 void SetDutyMD(ULONG Duty);
280 
281 #include <linux/kernel.h>
282 #include <linux/fs.h>
283 #include <linux/signal.h>
284 #include <linux/sched.h>
285 
286 #ifndef PCASM
287 #include <linux/ioport.h>
288 #include <asm/gpio.h>
289 #include <asm/uaccess.h>
290 #include <linux/module.h>
291 #include <linux/miscdevice.h>
292 
293 #include <linux/mm.h>
294 #include <linux/hrtimer.h>
295 
296 #include <linux/init.h>
297 #include <asm/siginfo.h> //siginfo
298 #include <linux/rcupdate.h> //rcu_read_lock
299 #include <linux/uaccess.h>
300 #include <linux/debugfs.h>
301 
302 #include <asm/io.h>
303 #include <asm/uaccess.h>
304 
305 #include <linux/irq.h>
306 #include <linux/interrupt.h>
307 
308 MODULE_LICENSE("GPL");
309 MODULE_AUTHOR("The LEGO Group");
310 MODULE_DESCRIPTION(MODULE_NAME);
311 MODULE_SUPPORTED_DEVICE(DEVICE1_NAME);
312 
313 module_init(ModuleInit);
314 module_exit(ModuleExit);
315 
316 #include <mach/mux.h>
317 
318 #else
319 // Keep Eclipse happy
320 #endif
321 
322 
323 enum OutputPortPins
324 {
325  PWM,
326  DIR0,
327  DIR1,
328  INT,
329  DIR,
330  SLEEP,
331  FAULT,
332  OUTPUT_PORT_PINS
333 };
334 
335 
336 #define IRQA_PINNO ((pOutputPortPin[Hw])[(0 * OUTPUT_PORT_PINS) + INT].Pin)
337 #define IRQB_PINNO ((pOutputPortPin[Hw])[(1 * OUTPUT_PORT_PINS) + INT].Pin)
338 #define IRQC_PINNO ((pOutputPortPin[Hw])[(2 * OUTPUT_PORT_PINS) + INT].Pin)
339 #define IRQD_PINNO ((pOutputPortPin[Hw])[(3 * OUTPUT_PORT_PINS) + INT].Pin)
340 
341 
342 /* EP2 hardware have sanyo motor driver */
343 INPIN EP2_OutputPortPin[][OUTPUT_PORT_PINS] =
344 {
345  { // Output port A
346  { EPWM1B , NULL, 0 }, // PWM motor A (GPIO 2_14)
347  { GP3_15 , NULL, 0 }, // DIR0 A
348  { GP3_6 , NULL, 0 }, // DIR1 A
349  { GP5_11 , NULL, 0 }, // INT A
350  { GP0_4 , NULL, 0 }, // DIR A
351  { -1 , NULL, 0 }, // Sleep AB
352  { -1 , NULL, 0 }, // Fault AB
353  },
354  { // Output port B
355  { EPWM1A , NULL, 0 }, // PWM motor B (GPIO 2_15)
356  { GP2_1 , NULL, 0 }, // DIR0 B
357  { GP0_3 , NULL, 0 }, // DIR1 B
358  { GP5_8 , NULL, 0 }, // INT B
359  { GP2_9 , NULL, 0 }, // DIR B
360  { -1 , NULL, 0 }, // Sleep AB
361  { -1 , NULL, 0 }, // Fault AB
362  },
363  { // Output port C
364  { APWM0 , NULL, 0 }, // PWM motor C (GPIO 8_7)
365  { GP6_8 , NULL, 0 }, // DIR0 C
366  { GP5_9 , NULL, 0 }, // DIR1 C
367  { GP5_13 , NULL, 0 }, // INT C
368  { GP3_14 , NULL, 0 }, // DIR C
369  { -1 , NULL, 0 }, // Sleep AB
370  { -1 , NULL, 0 }, // Fault AB
371  },
372  { // Output port D
373  { APWM1 , NULL, 0 }, // PWM MOTOR D (GPIO 0_0)
374  { GP5_3 , NULL, 0 }, // DIR0 D
375  { GP5_10 , NULL, 0 }, // DIR1 D
376  { GP6_9 , NULL, 0 }, // INT D
377  { GP2_8 , NULL, 0 }, // DIR D
378  { -1 , NULL, 0 }, // Sleep AB
379  { -1 , NULL, 0 }, // Fault AB
380  },
381 };
382 
383 
384 /* FINALB have TI motor driver */
385 INPIN FINALB_OutputPortPin[][OUTPUT_PORT_PINS] =
386 {
387  { // Output port A
388  { EPWM1A , NULL, 0 }, // PWM motor A (GPIO 2_15)
389  { GP0_3 , NULL, 0 }, // DIR0 A
390  { GP4_12 , NULL, 0 }, // DIR1 A
391  { GP5_11 , NULL, 0 }, // INT A
392  { GP0_4 , NULL, 0 }, // DIR A
393  { GP3_10 , NULL, 0 }, // Sleep AB
394  { GP2_0 , NULL, 0 }, // Fault AB
395  },
396  { // Output port B
397  { EPWM1B , NULL, 0 }, // PWM motor B (GPIO 2_14)
398  { GP3_15 , NULL, 0 }, // DIR0 B
399  { GP3_6 , NULL, 0 }, // DIR1 B
400  { GP5_8 , NULL, 0 }, // INT B
401  { GP2_9 , NULL, 0 }, // DIR B
402  { GP3_10 , NULL, 0 }, // Sleep AB - Same as above
403  { GP2_0 , NULL, 0 }, // Fault AB - Same as above
404  },
405  { // Output port C
406  { APWM1 , NULL, 0 }, // PWM motor C (GPIO 0_0)
407  { GP5_10 , NULL, 0 }, // DIR0 C
408  { GP5_3 , NULL, 0 }, // DIR1 C
409  { GP5_13 , NULL, 0 }, // INT C
410  { GP3_14 , NULL, 0 }, // DIR C
411  { GP2_3 , NULL, 0 }, // Sleep CD
412  { GP6_0 , NULL, 0 }, // Fault CD
413  },
414  { // Output port D
415  { APWM0 , NULL, 0 }, // PWM MOTOR D (GPIO 8_7)
416  { GP6_8 , NULL, 0 }, // DIR0 D
417  { GP5_9 , NULL, 0 }, // DIR1 D
418  { GP6_9 , NULL, 0 }, // INT D
419  { GP2_8 , NULL, 0 }, // DIR D
420  { GP2_3 , NULL, 0 }, // Sleep CD - Same as above
421  { GP6_0 , NULL, 0 }, // Fault CD - Same as above
422  },
423 };
424 
425 
426 INPIN FINAL_OutputPortPin[][OUTPUT_PORT_PINS] =
427 {
428  { // Output port A
429  { EPWM1A , NULL, 0 }, // PWM motor A (GPIO 2_15)
430  { GP0_3 , NULL, 0 }, // DIR0 A
431  { GP4_12 , NULL, 0 }, // DIR1 A
432  { GP5_11 , NULL, 0 }, // INT A
433  { GP0_4 , NULL, 0 }, // DIR A
434  { GP3_10 , NULL, 0 }, // Sleep AB
435  { GP2_0 , NULL, 0 }, // Fault AB
436  },
437  { // Output port B
438  { EPWM1B , NULL, 0 }, // PWM motor B (GPIO 2_14)
439  { GP3_15 , NULL, 0 }, // DIR0 B
440  { GP3_6 , NULL, 0 }, // DIR1 B
441  { GP5_8 , NULL, 0 }, // INT B
442  { GP2_9 , NULL, 0 }, // DIR B
443  { GP3_10 , NULL, 0 }, // Sleep AB - Same as above
444  { GP2_0 , NULL, 0 }, // Fault AB - Same as above
445  },
446  { // Output port C
447  { APWM1 , NULL, 0 }, // PWM motor C (GPIO 0_0)
448  { GP5_10 , NULL, 0 }, // DIR0 C
449  { GP5_3 , NULL, 0 }, // DIR1 C
450  { GP5_13 , NULL, 0 }, // INT C
451  { GP3_14 , NULL, 0 }, // DIR C
452  { GP2_3 , NULL, 0 }, // Sleep CD
453  { GP6_0 , NULL, 0 }, // Fault CD
454  },
455  { // Output port D
456  { APWM0 , NULL, 0 }, // PWM MOTOR D (GPIO 8_7)
457  { GP6_8 , NULL, 0 }, // DIR0 D
458  { GP5_9 , NULL, 0 }, // DIR1 D
459  { GP6_9 , NULL, 0 }, // INT D
460  { GP2_8 , NULL, 0 }, // DIR D
461  { GP2_3 , NULL, 0 }, // Sleep CD - Same as above
462  { GP6_0 , NULL, 0 }, // Fault CD - Same as above
463  },
464 };
465 
466 
467 INPIN *pOutputPortPin[] =
468 {
469  [FINAL] = (INPIN*)&FINAL_OutputPortPin[0], // FINAL platform
470  [FINALB] = (INPIN*)&FINALB_OutputPortPin[0], // FINALB platform
471  [EP2] = (INPIN*)&EP2_OutputPortPin[0], // EP2 platform
472 };
473 
474 
475 #define OutputReadDir(port,pin) ((HwInvBits ^ (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).in_data) & pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].Mask)
476 #define READDirA OutputReadDir(0,DIR)
477 #define READDirB OutputReadDir(1,DIR)
478 #define READDirC OutputReadDir(2,DIR)
479 #define READDirD OutputReadDir(3,DIR)
480 
481 /*
482  * Variables
483  */
484 static ULONG *GPIO;
485 static ULONG *SYSCFG0;
486 static ULONG *SYSCFG1;
487 static ULONG *PLLC1;
488 static ULONG *PSC1;
489 static UWORD *eHRPWM1;
490 static UWORD *eCAP0;
491 static UWORD *eCAP1;
492 static ULONG *TIMER64P3;
493 
494 static MOTOR Motor[NO_OF_OUTPUT_PORTS];
495 static SLONG *(StepPowerSteps[NO_OF_OUTPUT_PORTS]);
496 static SLONG *(TachoSteps[NO_OF_OUTPUT_PORTS]) = {&(Motor[0].TachoCnt),&(Motor[1].TachoCnt),&(Motor[2].TachoCnt),&(Motor[3].TachoCnt)};
497 static SLONG *(TimerSteps[NO_OF_OUTPUT_PORTS]) = {&(Motor[0].TimeCnt),&(Motor[1].TimeCnt),&(Motor[2].TimeCnt),&(Motor[3].TimeCnt)};
498 
499 static void (*SetDuty[NO_OF_OUTPUT_PORTS])(ULONG) = {SetDutyMA,SetDutyMB,SetDutyMC,SetDutyMD};
500 
501 static TACHOSAMPLES TachoSamples[NO_OF_OUTPUT_PORTS];
502 static UBYTE ReadyStatus = 0;
503 static UBYTE TestStatus = 0;
504 
505 static MOTORDATA MotorData[NO_OF_OUTPUT_PORTS];
506 static MOTORDATA *pMotor = MotorData;
507 
508 static ULONG TimeOutSpeed0[NO_OF_OUTPUT_PORTS];
509 static UBYTE MinRegEnabled[NO_OF_OUTPUT_PORTS];
510 
511 static UBYTE SyncMNos[MAX_SYNC_MOTORS];
512 static SBYTE MaxSyncSpeed;
513 
514 static struct hrtimer Device1Timer;
515 static ktime_t Device1Time;
516 
517 static UBYTE PrgStopTimer[NO_OF_OUTPUT_PORTS];
518 
519 
520 static ULONG CountsPerPulse[4] = {COUNTS_PER_PULSE_LM, COUNTS_PER_PULSE_LM, COUNTS_PER_PULSE_LM, COUNTS_PER_PULSE_LM};
521 
540 static UBYTE SamplesMediumMotor[NO_OF_SAMPLE_STEPS] = {2, 4, 8, 16};
541 static UBYTE SamplesLargeMotor[NO_OF_SAMPLE_STEPS] = {4, 16, 32, 64};
542 //static UBYTE SamplesMediumMotor[NO_OF_SAMPLE_STEPS] = {1, 2, 4, 8};
543 //static UBYTE SamplesLargeMotor[NO_OF_SAMPLE_STEPS] = {2, 8, 16, 32};
544 static UBYTE *SamplesPerSpeed[NO_OF_OUTPUT_PORTS] = {SamplesLargeMotor, SamplesLargeMotor, SamplesLargeMotor, SamplesLargeMotor};
545 
546 static UBYTE AVG_TACHO_COUNTS[NO_OF_OUTPUT_PORTS] = {2,2,2,2};
547 static ULONG AVG_COUNTS[NO_OF_OUTPUT_PORTS] = {(2 * COUNTS_PER_PULSE_LM),(2 * COUNTS_PER_PULSE_LM),(2 * COUNTS_PER_PULSE_LM),(2 * COUNTS_PER_PULSE_LM)};
548 
549 
550 /*
551  * Macros
552  */
553 #define FLOATFaultPins {\
554  if (((pOutputPortPin[Hw])[(0 * OUTPUT_PORT_PINS) + FAULT].Pin) != -1)\
555  {\
556  (*pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + FAULT].pGpio).dir |= pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + FAULT].Mask;\
557  }\
558  if (((pOutputPortPin[Hw])[(2 * OUTPUT_PORT_PINS) + FAULT].Pin) != -1)\
559  {\
560  (*pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + FAULT].pGpio).dir |= pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + FAULT].Mask;\
561  }\
562  }
563 
564 #define SETSleepPins {\
565  if (((pOutputPortPin[Hw])[(0 * OUTPUT_PORT_PINS) + SLEEP].Pin) != -1)\
566  {\
567  (*pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + SLEEP].pGpio).set_data = pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + SLEEP].Mask;\
568  (*pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + SLEEP].pGpio).dir &= ~pOutputPortPin[Hw][(0 * OUTPUT_PORT_PINS) + SLEEP].Mask;\
569  }\
570  if (((pOutputPortPin[Hw])[(2 * OUTPUT_PORT_PINS) + SLEEP].Pin) != -1)\
571  {\
572  (*pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + SLEEP].pGpio).set_data = pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + SLEEP].Mask;\
573  (*pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + SLEEP].pGpio).dir &= ~pOutputPortPin[Hw][(2 * OUTPUT_PORT_PINS) + SLEEP].Mask;\
574  }\
575  }
576 
577 #define SETMotorType(Port, NewType) {\
578  Motor[Port].Type = NewType;\
579  if (TYPE_MINITACHO == NewType)\
580  {\
581  CountsPerPulse[Port] = COUNTS_PER_PULSE_MM;\
582  SamplesPerSpeed[Port] = SamplesMediumMotor;\
583  }\
584  else\
585  {\
586  CountsPerPulse[Port] = COUNTS_PER_PULSE_LM;\
587  SamplesPerSpeed[Port] = SamplesLargeMotor;\
588  }\
589  }
590 
591 #define SETAvgTachoCount(Port, Speed) {\
592  if (Speed > 80)\
593  {\
594  AVG_TACHO_COUNTS[Port] = SamplesPerSpeed[Port][3];\
595  AVG_COUNTS[Port] = SamplesPerSpeed[Port][3] * CountsPerPulse[Port];\
596  }\
597  else\
598  {\
599  if (Speed > 60)\
600  {\
601  AVG_TACHO_COUNTS[Port] = SamplesPerSpeed[Port][2];\
602  AVG_COUNTS[Port] = SamplesPerSpeed[Port][2] * CountsPerPulse[Port];\
603  }\
604  else\
605  {\
606  if (Speed > 40)\
607  {\
608  AVG_TACHO_COUNTS[Port] = SamplesPerSpeed[Port][1];\
609  AVG_COUNTS[Port] = SamplesPerSpeed[Port][1] * CountsPerPulse[Port];\
610  }\
611  else\
612  {\
613  AVG_TACHO_COUNTS[Port] = SamplesPerSpeed[Port][0];\
614  AVG_COUNTS[Port] = SamplesPerSpeed[Port][0] * CountsPerPulse[Port];\
615  }\
616  }\
617  }\
618  }
619 
620 #define FREERunning24bittimer ((ULONG)((((ULONG*)(TIMER64P3))[TIM34])>>8))
621 
622 #define CLEARTachoArray(No) {\
623  Motor[No].DirChgPtr = 0;\
624  memset(&TachoSamples[No], 0, sizeof(TACHOSAMPLES));\
625  }
626 
627 #define OutputFloat(port,pin) {\
628  (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).dir |= pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].Mask;\
629  }
630 
631 #define OutputRead(port,pin) ((*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).in_data & pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].Mask)
632 
633 #define OutputHigh(port,pin) {\
634  (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).set_data = pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].Mask;\
635  (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).dir &= ~pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].Mask;\
636  }
637 
638 #define OutputLow(port,pin) {\
639  (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).clr_data = pOutputPortPin[Hw][(OUTPUT_PORT_PINS * port) + pin].Mask;\
640  (*pOutputPortPin[Hw][(port * OUTPUT_PORT_PINS) + pin].pGpio).dir &= ~pOutputPortPin[Hw][(OUTPUT_PORT_PINS * port) + pin].Mask;\
641  }
642 
643 
644 #define EHRPWMClkDis {\
645  eHRPWM1[TBCTL] = (TB_UP | TB_DISABLE | TB_SHADOW | TB_SYNC_DISABLE | TB_HDIV1 | TB_DIV1 | TB_COUNT_FREEZE);\
646  }
647 
648 #define EHRPWMClkEna {\
649  eHRPWM1[TBCTL] = (TB_UP | TB_DISABLE | TB_SHADOW | TB_SYNC_DISABLE | TB_HDIV1 | TB_DIV1 | TB_COUNT_UP);\
650  REGUnlock;\
651  iowrite32((ioread32(&SYSCFG0[CFGCHIP1]) | TBCLKSYNC),&SYSCFG0[CFGCHIP1]); /*This is the clock to all eHRPWM's*/\
652  REGLock;\
653  }
654 
655 #define STOPPwm {\
656  iowrite16(0x00, &eCAP0[0x15]);\
657  iowrite16(0x00, &eCAP1[0x15]);\
658  TIMER64P3[TGCR] = 0x00000000;\
659  iowrite16(0x00, &eHRPWM1[TBCTL]);\
660  iowrite16(0x00, &eHRPWM1[CMPCTL]);\
661  EHRPWMClkDis;\
662  }
663 
664 #define SETPwmFreqKHz(KHz) {\
665  eHRPWM1[TBPRD] = KHz; /* For Motor A and Motor B */\
666  eCAP0[CAP1] = KHz; /* For Motor C */\
667  eCAP1[CAP1] = KHz; /* For Motor D */\
668  }
669 
670 #define SETUPPwmModules { \
671  \
672  /* eHRPWM Module */\
673  EHRPWMClkDis;\
674  eHRPWM1[TBPHS] = 0;\
675  eHRPWM1[TBCNT] = 0;\
676  eHRPWM1[CMPCTL] = (CC_A_SHADOW | CC_B_SHADOW | CC_CTR_A_ZERO | CC_CTR_B_ZERO);\
677  eHRPWM1[AQCTLA] = 0x00000021;\
678  eHRPWM1[AQCTLB] = 0x00000201;\
679  EHRPWMClkEna;\
680  \
681  /* eCAP modules - APWM */\
682  (eCAP0)[TSCTR] = 0;\
683  (eCAP1)[TSCTR] = 0;\
684  (eCAP0)[CTRPHS] = 0;\
685  (eCAP1)[CTRPHS] = 0;\
686  eCAP0[ECCTL2] = 0x0690;\
687  eCAP1[ECCTL2] = 0x0690;\
688  TIMER64P3[TGCR] = 0x00003304;\
689  TIMER64P3[TGCR] |= 0x00000002;\
690  TIMER64P3[PRD34] = 0xFFFFFFFF;\
691  TIMER64P3[TCR] = 0x00800000;\
692  \
693  /* Setup PWM */\
694  SetDutyMA(0);\
695  SetDutyMB(0);\
696  SetDutyMC(0);\
697  SetDutyMD(0);\
698  SETPwmFreqKHz(MAX_PWM_CNT-1);\
699  FLOATFaultPins;\
700  SETSleepPins;\
701  }
702 
703 #define READIntA OutputRead(0,INT)
704 #define READIntB OutputRead(1,INT)
705 #define READIntC OutputRead(2,INT)
706 #define READIntD OutputRead(3,INT)
707 
708 
709 /*
710  * Functions
711  */
712 void CheckSpeedPowerLimits(SBYTE *pCheckVal)
713 {
714  if (MAX_SPEED < *pCheckVal)
715  {
716  *pCheckVal = MAX_SPEED;
717  }
718  if (-MAX_SPEED > *pCheckVal)
719  {
720  *pCheckVal = -MAX_SPEED;
721  }
722 }
723 
724 
725 UBYTE CheckLessThanSpecial(SLONG Param1, SLONG Param2, SLONG Inv)
726 {
727  UBYTE RtnVal = FALSE;
728 
729  if ((Param1 * Inv) < (Param2 * Inv))
730  {
731  RtnVal = TRUE;
732  }
733  return(RtnVal);
734 }
735 
736 
737 void IncSpeed(SBYTE Dir, SBYTE *pSpeed)
738 {
739  if ((NON_INV == Dir) && (MAX_SPEED > *pSpeed))
740  {
741  (*pSpeed)++;
742  }
743  else
744  {
745  if ((INV == Dir) && (-MAX_SPEED < *pSpeed))
746  {
747  (*pSpeed)--;
748  }
749  }
750 }
751 
752 
753 void DecSpeed(SBYTE Dir, SBYTE *pSpeed)
754 {
755  if ((NON_INV == Dir) && (0 < *pSpeed))
756  {
757  (*pSpeed)--;
758  }
759  else
760  {
761  if ((INV == Dir) && (0 > *pSpeed))
762  {
763  (*pSpeed)++;
764  }
765  }
766 }
767 
768 
769 void SetDirRwd(UBYTE Port)
770 {
771  OutputFloat(Port,DIR0);
772  OutputHigh(Port,DIR1);
773 }
774 
775 void SetDirFwd(UBYTE Port)
776 {
777  OutputHigh(Port,DIR0);
778  OutputFloat(Port,DIR1);
779 }
780 
781 void SetBrake(UBYTE Port)
782 {
783  OutputHigh(Port,DIR0);
784  OutputHigh(Port,DIR1);
785 }
786 
787 void SetCoast(UBYTE Port)
788 {
789  OutputLow(Port,DIR0);
790  OutputLow(Port,DIR1);
791 }
792 
793 
794 UWORD GetTachoDir(UBYTE Port)
795 {
796  return(OutputRead(Port,DIR));
797 }
798 
799 
800 UWORD GetTachoInt(UBYTE Port)
801 {
802  return(OutputRead(Port,INT));
803 }
804 
805 
806 void SetPower(UBYTE Port, SLONG Power)
807 {
808  if (MAX_PWM_CNT < Power)
809  {
810  Power = MAX_PWM_CNT;
811  Motor[Port].Power = Power;
812  }
813  if (-MAX_PWM_CNT > Power)
814  {
815  Power = -MAX_PWM_CNT;
816  Motor[Port].Power = Power;
817  }
818 
819  if (TYPE_MINITACHO == Motor[Port].Type)
820  {
821  // Medium Motor
822  if (0 != Power)
823  {
824  if (0 < Power)
825  {
826  SetDirFwd(Port);
827  }
828  else
829  {
830  SetDirRwd(Port);
831  Power = 0 - Power;
832  }
833  Power = ((Power * 8000)/10000) + 2000;
834  }
835  }
836  else
837  {
838  // Large motor
839  if (0 != Power)
840  {
841  if (0 < Power)
842  {
843  SetDirFwd(Port);
844  }
845  else
846  {
847  SetDirRwd(Port);
848  Power = 0 - Power;
849  }
850  Power = ((Power * 9500)/10000) + 500;
851  }
852  }
853  SetDuty[Port](Power);
854 }
855 
856 
857 void SetRegulationPower(UBYTE Port, SLONG Power)
858 {
859  if (MAX_PWM_CNT < Power)
860  {
861  Power = MAX_PWM_CNT;
862  Motor[Port].Power = Power;
863  }
864  if (-MAX_PWM_CNT > Power)
865  {
866  Power = -MAX_PWM_CNT;
867  Motor[Port].Power = Power;
868  }
869 
870  if (TYPE_MINITACHO == Motor[Port].Type)
871  {
872  // Medium Motor
873  if (0 != Power)
874  {
875  if (0 < Power)
876  {
877  SetDirFwd(Port);
878  }
879  else
880  {
881  SetDirRwd(Port);
882  Power = 0 - Power;
883  }
884  Power = ((Power * 9000)/10000) + 1000;
885  }
886  }
887  else
888  {
889  // Large motor
890  if (0 != Power)
891  {
892  if (0 < Power)
893  {
894  SetDirFwd(Port);
895  }
896  else
897  {
898  SetDirRwd(Port);
899  Power = 0 - Power;
900  }
901  Power = ((Power * 10000)/10000) + 0;
902  }
903  }
904  SetDuty[Port](Power);
905 }
906 
907 
908 void SetDutyMA(ULONG Duty)
909 {
910  eHRPWM1[CMPA] = (UWORD)Duty;
911 }
912 void SetDutyMB(ULONG Duty)
913 {
914  eHRPWM1[CMPB] = (UWORD)Duty;
915 }
916 void SetDutyMC(ULONG Duty)
917 {
918  eCAP1[CAP2] = Duty;
919 }
920 void SetDutyMD(ULONG Duty)
921 {
922  eCAP0[CAP2] = Duty;
923 }
924 
925 
926 void ClearPIDParameters(UBYTE No)
927 {
928  Motor[No].PVal = 0;
929  Motor[No].IVal = 0;
930  Motor[No].DVal = 0;
931  Motor[No].OldSpeedErr = 0;
932 }
933 
934 
951 void StepPowerStopMotor(UBYTE No, SLONG AdjustTachoValue)
952 {
953  *StepPowerSteps[No] -= AdjustTachoValue;
954  SetPower(No, 0); // Don't destroy power level if next command needs to use it
955  if (Motor[No].TargetBrake)
956  {
957  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
958  {
959  // To be able to brake at tacho = 0 if this was timed command
960  Motor[No].TachoCnt = 0;
961  }
962  Motor[No].State = BRAKED;
963  }
964  else
965  {
966  Motor[No].State = IDLE;
967  SetCoast(No);
968  }
969  Motor[No].TargetState = UNLIMITED_UNREG; // Default motor startup
970 
971  // If this was a synced motor then clear it
972  if (SyncMNos[0] == No)
973  {
974  SyncMNos[0] = UNUSED_SYNC_MOTOR;
975  }
976  if (SyncMNos[1] == No)
977  {
978  SyncMNos[1] = UNUSED_SYNC_MOTOR;
979  }
980  ReadyStatus &= ~(0x01 << No); // Clear ready flag
981  TestStatus &= ~(0x01 << No); // Clear ready flag
982 }
983 
984 
1000 UBYTE StepSpeedCheckTachoCntUp(UBYTE No)
1001 {
1002  UBYTE Return;
1003  SLONG CorrPower;
1004 
1005  Return = FALSE;
1006  if (Motor[No].TachoCntUp)
1007  { // RampUp enabled
1008 
1009  Motor[No].State = LIMITED_REG_STEPUP;
1010 
1011  if (0 != Motor[No].Speed)
1012  {
1013  CorrPower = Motor[No].TargetPower - (SLONG)(Motor[No].Speed);
1014  }
1015  else
1016  {
1017  CorrPower = Motor[No].TargetPower;
1018  }
1019 
1020  // Number of powersteps per tacho count
1021  Motor[No].RampUpFactor = ((CorrPower * RAMP_FACTOR) + (Motor[No].TachoCntUp/2))/Motor[No].TachoCntUp;
1022 
1023  if (0 == Motor[No].RampUpFactor)
1024  {
1025  Motor[No].RampUpFactor = 1;
1026  }
1027  Motor[No].RampUpOffset = Motor[No].Speed;
1028 
1029  if (0 == Motor[No].Speed)
1030  {
1031  ClearPIDParameters(No);
1032  }
1033 
1034  if((0 == Motor[No].TachoCntConst) && (0 == Motor[No].TachoCntDown) && (1 == Motor[No].TargetBrake))
1035  {
1036  Motor[No].BrakeAfter = TRUE;
1037  }
1038  else
1039  {
1040  Motor[No].BrakeAfter = FALSE;
1041  }
1042  Motor[No].LockRampDown = FALSE;
1043  Return = TRUE;
1044  }
1045  return(Return);
1046 }
1047 
1048 
1065 UBYTE StepSpeedCheckTachoCntConst(UBYTE No, SLONG AdjustTachoValue)
1066 {
1067  UBYTE ReturnState;
1068 
1069  ReturnState = FALSE;
1070 
1071  if (Motor[No].TachoCntConst)
1072  {
1073  *StepPowerSteps[No] -= AdjustTachoValue;
1074  Motor[No].TargetSpeed = Motor[No].TargetPower; //(Motor[No].TargetPower / SPEED_PWMCNT_REL);
1075  Motor[No].State = LIMITED_REG_STEPCONST;
1076  ClearPIDParameters(No);
1077 
1078  if((0 == Motor[No].TachoCntDown) && (1 == Motor[No].TargetBrake))
1079  {
1080  Motor[No].BrakeAfter = TRUE;
1081  }
1082  else
1083  {
1084  Motor[No].BrakeAfter = FALSE;
1085  }
1086  Motor[No].LockRampDown = FALSE;
1087  ReturnState = TRUE;
1088  }
1089  return(ReturnState);
1090 }
1091 
1092 
1109 void StepSpeedCheckTachoCntDown(UBYTE No, SLONG AdjustTachoValue)
1110 {
1111  if (Motor[No].TachoCntDown)
1112  {
1113  Motor[No].RampDownFactor = ((Motor[No].TargetPower * RAMP_FACTOR) + (Motor[No].TachoCntDown/2))/Motor[No].TachoCntDown;
1114 
1115  *StepPowerSteps[No] -= AdjustTachoValue;
1116  Motor[No].State = LIMITED_REG_STEPDOWN;
1117  MinRegEnabled[No] = FALSE;
1118  ClearPIDParameters(No);
1119  }
1120  else
1121  {
1122  StepPowerStopMotor(No, AdjustTachoValue);
1123  }
1124 }
1125 
1126 
1142 UBYTE StepPowerCheckTachoCntUp(UBYTE No)
1143 {
1144  UBYTE Return;
1145 
1146  Return = FALSE;
1147 
1148  if (Motor[No].TachoCntUp)
1149  { // RampUp enabled
1150 
1151  Motor[No].State = LIMITED_UNREG_STEPUP;
1152  Motor[No].RampUpFactor = ((Motor[No].TargetPower * RAMP_FACTOR) + (Motor[No].TachoCntUp/2))/Motor[No].TachoCntUp;
1153  ClearPIDParameters(No);
1154 
1155  if((0 == Motor[No].TachoCntConst) && (0 == Motor[No].TachoCntDown) && (1 == Motor[No].TargetBrake))
1156  {
1157  Motor[No].BrakeAfter = TRUE;
1158  }
1159  else
1160  {
1161  Motor[No].BrakeAfter = FALSE;
1162  }
1163  Motor[No].LockRampDown = FALSE;
1164  Return = TRUE;
1165  }
1166  return(Return);
1167 }
1168 
1169 
1186 UBYTE StepPowerCheckTachoCntConst(UBYTE No, SLONG AdjustTachoValue)
1187 {
1188  UBYTE ReturnState;
1189 
1190  ReturnState = FALSE;
1191 
1192  if (Motor[No].TachoCntConst)
1193  {
1194  *StepPowerSteps[No] -= AdjustTachoValue;
1195  Motor[No].Power = Motor[No].TargetPower;
1196  Motor[No].State = LIMITED_UNREG_STEPCONST;
1197  SetPower(No,Motor[No].Power);
1198  ClearPIDParameters(No);
1199 
1200  if((0 == Motor[No].TachoCntDown) && (1 == Motor[No].TargetBrake))
1201  {
1202  Motor[No].BrakeAfter = TRUE;
1203  }
1204  else
1205  {
1206  Motor[No].BrakeAfter = FALSE;
1207  }
1208  Motor[No].LockRampDown = FALSE;
1209  ReturnState = TRUE;
1210  }
1211  return(ReturnState);
1212 }
1213 
1214 
1231 void StepPowerCheckTachoCntDown(UBYTE No, SLONG AdjustTachoValue)
1232 {
1233  if (Motor[No].TachoCntDown)
1234  {
1235 
1236  Motor[No].RampDownFactor = ((Motor[No].TargetPower * RAMP_FACTOR) + (Motor[No].TachoCntDown/2))/Motor[No].TachoCntDown;
1237  *StepPowerSteps[No] -= AdjustTachoValue;
1238  Motor[No].State = LIMITED_UNREG_STEPDOWN;
1239  MinRegEnabled[No] = FALSE;
1240  ClearPIDParameters(No);
1241  }
1242  else
1243  {
1244  StepPowerStopMotor(No, AdjustTachoValue);
1245  }
1246 }
1247 
1248 
1270 void dRegulateSpeed(UBYTE No)
1271 {
1272 
1273  SLONG SpeedErr;
1274 
1275  if (TYPE_MINITACHO == Motor[No].Type)
1276  {
1277  SpeedErr = ((SLONG)(Motor[No].TargetSpeed) - (SLONG)(Motor[No].Speed));
1278  Motor[No].PVal = SpeedErr * 4;
1279  Motor[No].IVal = ((Motor[No].IVal * 9)/10) + (SpeedErr / 3);
1280  Motor[No].DVal = (((SpeedErr - (Motor[No].OldSpeedErr)) * 4)/2) * 40;
1281  }
1282  else
1283  {
1284  SpeedErr = ((SLONG)(Motor[No].TargetSpeed) - (SLONG)(Motor[No].Speed));
1285  Motor[No].PVal = SpeedErr * 2;
1286  Motor[No].IVal = ((Motor[No].IVal * 9)/10) + (SpeedErr / 4);
1287  Motor[No].DVal = (((SpeedErr - (Motor[No].OldSpeedErr)) * 4)/2) * 40;
1288  }
1289 
1290  Motor[No].Power = Motor[No].Power + ((Motor[No].PVal + Motor[No].IVal + Motor[No].DVal));
1291 
1292  if(Motor[No].Power > MAX_PWM_CNT)
1293  {
1294  Motor[No].Power = MAX_PWM_CNT;
1295  }
1296 
1297  if(Motor[No].Power < -MAX_PWM_CNT)
1298  {
1299  Motor[No].Power = -MAX_PWM_CNT;
1300  }
1301  Motor[No].OldSpeedErr = SpeedErr;
1302 
1303 
1304  if (((Motor[No].TargetSpeed) > 0) && (Motor[No].Power < 0))
1305  {
1306  Motor[No].Power = 0;
1307  }
1308 
1309  if (((Motor[No].TargetSpeed) < 0) && (Motor[No].Power > 0))
1310  {
1311  Motor[No].Power = 0;
1312  }
1313 
1314  SetRegulationPower(No, Motor[No].Power);
1315 }
1316 
1317 
1318 void BrakeMotor(UBYTE No, SLONG TachoCnt)
1319 {
1320  SLONG TmpTacho;
1321 
1322  TmpTacho = TachoCnt * 100;
1323 
1324  TmpTacho <<= 2;
1325 
1326  if (TmpTacho > MAX_PWM_CNT)
1327  {
1328  TmpTacho = MAX_PWM_CNT;
1329  }
1330  if (TmpTacho < -MAX_PWM_CNT)
1331  {
1332  TmpTacho = -MAX_PWM_CNT;
1333  }
1334 
1335  Motor[No].Power = 0 - TmpTacho;
1336  SetRegulationPower(No,Motor[No].Power);
1337 }
1338 
1339 
1340 UBYTE RampDownToBrake(UBYTE No, SLONG CurrentCnt, SLONG TargetCnt, SLONG Dir)
1341 {
1342  UBYTE Status;
1343 
1344  Status = FALSE;
1345 
1346  if (TRUE == CheckLessThanSpecial(CurrentCnt, TargetCnt, Dir))
1347  {
1348 
1349 // Motor[No].TargetSpeed = (SBYTE)((Motor[No].TachoCntConst - CurrentCnt));
1350  Motor[No].TargetSpeed = (SBYTE)(TargetCnt - CurrentCnt);
1351 
1352  if ((Motor[No].TargetSpeed > 5) || (Motor[No].TargetSpeed < -5))
1353  {
1354  if (TRUE == CheckLessThanSpecial(Motor[No].TargetSpeed, Motor[No].Speed, Dir))
1355  {
1356  Motor[No].TargetSpeed = 1 * Dir;
1357  }
1358  }
1359 
1360  dRegulateSpeed(No);
1361  }
1362  else
1363  {
1364  // Done ramping down to brake
1365  Status = TRUE;
1366  }
1367  return(Status);
1368 }
1369 
1370 
1371 void CalcRampDownFactor(UBYTE No, ULONG CurrentPower, ULONG Counts)
1372 {
1373 
1374  if (Counts != 0)
1375  {
1376  Motor[No].RampDownFactor = ((CurrentPower * RAMP_FACTOR) + (Counts/2))/Counts;
1377  }
1378 }
1379 
1380 
1381 void GetCompareCounts(UBYTE No, SLONG *Counts, SLONG *CompareCounts)
1382 {
1383  *Counts = *StepPowerSteps[No];
1384 
1385  if (TRUE == Motor[No].BrakeAfter)
1386  {
1387  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1388  {
1389  // Run for time
1390  *CompareCounts = *Counts + (Motor[No].Speed);
1391  }
1392  else
1393  {
1394  // Run for tacho
1395  if (TYPE_TACHO == Motor[No].Type)
1396  {
1397  *CompareCounts = *Counts + ((Motor[No].Speed)/2);
1398  }
1399  else
1400  {
1401  *CompareCounts = *Counts + ((Motor[No].Speed/3));
1402  }
1403  }
1404  }
1405  else
1406  {
1407  *CompareCounts = *Counts;
1408  }
1409 }
1410 
1411 
1412 void StopAndBrakeMotor(UBYTE MotorNo)
1413 {
1414  ReadyStatus &= ~(0x01 << MotorNo);
1415  TestStatus &= ~(0x01 << MotorNo);
1416  Motor[MotorNo].Power = 0;
1417  Motor[MotorNo].Speed = 0;
1418  Motor[MotorNo].TachoCnt = 0;
1419  Motor[MotorNo].State = BRAKED;
1420  CLEARTachoArray(MotorNo);
1421  SetPower(MotorNo, Motor[MotorNo].Power);
1422  SetBrake(MotorNo);
1423 }
1424 
1425 
1426 void StopAndFloatMotor(UBYTE MotorNo)
1427 {
1428  ReadyStatus &= ~(0x01 << MotorNo);
1429  TestStatus &= ~(0x01 << MotorNo);
1430  Motor[MotorNo].Power = 0;
1431  Motor[MotorNo].Speed = 0;
1432  Motor[MotorNo].TachoCnt = 0;
1433  Motor[MotorNo].State = IDLE;
1434  CLEARTachoArray(MotorNo);
1435  SetPower(MotorNo, Motor[MotorNo].Power);
1436  SetCoast(MotorNo);
1437 }
1438 
1439 
1440 void FloatSyncedMotors(void)
1441 {
1442  UBYTE TmpNo0, TmpNo1;
1443 
1444  TmpNo0 = SyncMNos[0];
1445  TmpNo1 = SyncMNos[1];
1446 
1447  Motor[TmpNo0].Mutex = TRUE;
1448  Motor[TmpNo1].Mutex = TRUE;
1449 
1450  StopAndFloatMotor(TmpNo0);
1451  StopAndFloatMotor(TmpNo1);
1452  SyncMNos[0] = UNUSED_SYNC_MOTOR;
1453  SyncMNos[1] = UNUSED_SYNC_MOTOR;
1454 
1455  MaxSyncSpeed = 0;
1456  Motor[TmpNo0].TargetSpeed = 0;
1457  Motor[TmpNo1].TargetSpeed = 0;
1458 
1459  Motor[TmpNo0].Mutex = FALSE;
1460  Motor[TmpNo1].Mutex = FALSE;
1461 }
1462 
1463 
1464 void TestAndFloatSyncedMotors(UBYTE MotorBitField, UBYTE SyncCmd)
1465 {
1466 // UBYTE TmpNo0, TmpNo1;
1467 
1468  // Only if motors are already sync'ed
1469  if ((SyncMNos[0] != UNUSED_SYNC_MOTOR) && (SyncMNos[1] != UNUSED_SYNC_MOTOR))
1470  {
1471  if (TRUE == SyncCmd)
1472  {
1473  //This is new sync'ed motor command
1474  if (((MotorBitField & (0x01 << SyncMNos[0])) && (MotorBitField & (0x01 << SyncMNos[1]))))
1475  {
1476  // Same motors used -> don't stop motors
1477  }
1478  else
1479  {
1480  // Only 2 motors can be sync'ed at a time -> stop sync'ed command
1481  FloatSyncedMotors();
1482  }
1483  }
1484  else
1485  {
1486  if ((MotorBitField & (0x01 << SyncMNos[0])) || (MotorBitField & (0x01 << SyncMNos[1])))
1487  {
1488  // Same motors used -> stop sync'ed command
1489  FloatSyncedMotors();
1490  }
1491  }
1492 
1493 
1494 
1495 /*
1496 
1497  // Check if new sync'ed motor command uses same motors as previous sync cmd
1498  if ((MotorBitField & (0x01 << SyncMNos[0])) && (MotorBitField & (0x01 << SyncMNos[1])))
1499  {
1500  // Check if only one of the sync'ed motors are affected by the new motor cmd
1501  if (((MotorBitField & (0x01 << SyncMNos[0])) || (MotorBitField & (0x01 << SyncMNos[1]))) || (TRUE == SyncCmd))
1502  {
1503  // The motor is in sync'ed mode -> float both sync'ed motors
1504  TmpNo0 = SyncMNos[0];
1505  TmpNo1 = SyncMNos[1];
1506 
1507  Motor[TmpNo0].Mutex = TRUE;
1508  Motor[TmpNo1].Mutex = TRUE;
1509 
1510  StopAndFloatMotor(TmpNo0);
1511  StopAndFloatMotor(TmpNo1);
1512  SyncMNos[0] = UNUSED_SYNC_MOTOR;
1513  SyncMNos[1] = UNUSED_SYNC_MOTOR;
1514 
1515  MaxSyncSpeed = 0;
1516  Motor[TmpNo0].TargetSpeed = 0;
1517  Motor[TmpNo1].TargetSpeed = 0;
1518 
1519  Motor[TmpNo0].Mutex = FALSE;
1520  Motor[TmpNo1].Mutex = FALSE;
1521  }
1522  }*/
1523  }
1524 }
1525 
1526 
1540 static enum hrtimer_restart Device1TimerInterrupt1(struct hrtimer *pTimer)
1541 {
1542  UBYTE No;
1543  UBYTE Test;
1544 
1545  static SLONG volatile TmpTacho;
1546  static SLONG volatile Tmp;
1547 
1548  hrtimer_forward_now(pTimer,Device1Time);
1549  for (No = 0; No < NO_OF_OUTPUT_PORTS; No++)
1550  {
1551  TmpTacho = Motor[No].IrqTacho;
1552  Tmp = (TmpTacho - Motor[No].OldTachoCnt);
1553 
1554  Motor[No].TachoCnt += Tmp;
1555  Motor[No].TachoSensor += Tmp;
1556  Motor[No].OldTachoCnt = TmpTacho;
1557 
1558  /* Update shared memory */
1559  pMotor[No].TachoCounts = Motor[No].TachoCnt;
1560  pMotor[No].Speed = Motor[No].Speed;
1561  pMotor[No].TachoSensor = Motor[No].TachoSensor;
1562 
1563  Motor[No].TimeCnt += Motor[No].TimeInc; // Add or sub so that TimerCnt is 1 mS resolution
1564 
1565  if (FALSE == Motor[No].Mutex)
1566  {
1567 
1568  Test = dCalculateSpeed(No, &(Motor[No].Speed));
1569  switch(Motor[No].State)
1570  {
1571  case UNLIMITED_UNREG:
1572  {
1573  if (Motor[No].TargetPower != Motor[No].Power)
1574  {
1575  Motor[No].Power = Motor[No].TargetPower;
1576  SetPower(No,Motor[No].Power);
1577  }
1578  Motor[No].TachoCnt = 0;
1579  Motor[No].TimeCnt = 0;
1580 
1581  }
1582  break;
1583 
1584  case UNLIMITED_REG:
1585  {
1586  dRegulateSpeed(No);
1587  Motor[No].TachoCnt = 0;
1588  Motor[No].TimeCnt = 0;
1589  }
1590  break;
1591 
1592  case LIMITED_REG_STEPUP:
1593  {
1594  UBYTE Status;
1595  SLONG StepCnt;
1596  SLONG StepCntTst;
1597 
1598  // Status used to check if ramp up has completed
1599  Status = FALSE;
1600 
1601  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1602  {
1603  Motor[No].TachoCnt = 0;
1604  }
1605  else
1606  {
1607  Motor[No].TimeCnt = 0;
1608  }
1609 
1610  GetCompareCounts(No, &StepCnt, &StepCntTst);
1611 
1612  if ((TRUE == CheckLessThanSpecial(StepCntTst, Motor[No].TachoCntUp, Motor[No].Dir)) && (FALSE == Motor[No].LockRampDown))
1613  {
1614  Motor[No].TargetSpeed = ((StepCnt * (Motor[No].RampUpFactor))/ RAMP_FACTOR) + Motor[No].RampUpOffset;
1615  if (TRUE == CheckLessThanSpecial((SLONG)Motor[No].TargetSpeed,((SLONG)6 * Motor[No].Dir), Motor[No].Dir))
1616  {
1617  //Ensure minimum speed
1618  Motor[No].TargetSpeed = 6 * Motor[No].Dir;
1619  }
1620  dRegulateSpeed(No);
1621  }
1622  else
1623  {
1624  if (TRUE == Motor[No].BrakeAfter)
1625  {
1626 
1627  Motor[No].LockRampDown = TRUE;
1628  Status = RampDownToBrake(No, StepCnt, Motor[No].TachoCntUp, Motor[No].Dir);
1629  }
1630  else
1631  {
1632  Status = TRUE;
1633  }
1634  }
1635 
1636  if (TRUE == Status)
1637  {
1638  // Ramp up completed check for next step
1639  if (FALSE == StepSpeedCheckTachoCntConst(No, Motor[No].TachoCntUp))
1640  {
1641  StepSpeedCheckTachoCntDown(No, Motor[No].TachoCntUp);
1642  }
1643  }
1644  }
1645  break;
1646 
1647  case LIMITED_REG_STEPCONST:
1648  {
1649  SLONG StepCnt, StepCntTst;
1650 
1651  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1652  {
1653  Motor[No].TachoCnt = 0;
1654  }
1655  else
1656  {
1657  Motor[No].TimeCnt = 0;
1658  }
1659 
1660  GetCompareCounts(No, &StepCnt, &StepCntTst);
1661 
1662  if ((TRUE == CheckLessThanSpecial(StepCntTst, Motor[No].TachoCntConst, Motor[No].Dir)) && (FALSE == Motor[No].LockRampDown))
1663  {
1664  dRegulateSpeed(No);
1665  }
1666  else
1667  {
1668  if (TRUE == Motor[No].BrakeAfter)
1669  {
1670  Motor[No].LockRampDown = TRUE;
1671  if (TRUE == RampDownToBrake(No, StepCnt, Motor[No].TachoCntConst, Motor[No].Dir))
1672  {
1673  StepSpeedCheckTachoCntDown(No, Motor[No].TachoCntConst);
1674  }
1675  }
1676  else
1677  {
1678  StepSpeedCheckTachoCntDown(No, Motor[No].TachoCntConst);
1679  }
1680  }
1681  }
1682  break;
1683 
1684  case LIMITED_REG_STEPDOWN:
1685  {
1686  SLONG StepCnt;
1687  SBYTE NewSpeed;
1688 
1689  StepCnt = *StepPowerSteps[No];
1690 
1691  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1692  {
1693  Motor[No].TachoCnt = 0;
1694  }
1695  else
1696  {
1697  Motor[No].TimeCnt = 0;
1698  }
1699 
1700  if (TRUE == CheckLessThanSpecial(StepCnt, Motor[No].TachoCntDown, Motor[No].Dir))
1701  {
1702  NewSpeed = Motor[No].TargetPower - ((StepCnt * (Motor[No].RampDownFactor))/ RAMP_FACTOR);
1703  if (TRUE == CheckLessThanSpecial((4 * Motor[No].Dir), NewSpeed, Motor[No].Dir))
1704  {
1705  Motor[No].TargetSpeed = NewSpeed;
1706  }
1707  dRegulateSpeed(No);
1708  }
1709  else
1710  {
1711  StepPowerStopMotor(No, Motor[No].TachoCntDown);
1712  }
1713  }
1714  break;
1715 
1716  case LIMITED_UNREG_STEPUP:
1717  {
1718 
1719  UBYTE Status;
1720  SLONG StepCnt;
1721  SLONG StepCntTst;
1722 
1723  // Status used to check if ramp up has completed
1724  Status = FALSE;
1725 
1726  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1727  {
1728  Motor[No].TachoCnt = 0;
1729  }
1730  else
1731  {
1732  Motor[No].TimeCnt = 0;
1733  }
1734 
1735  GetCompareCounts(No, &StepCnt, &StepCntTst);
1736 
1737  if ((TRUE == CheckLessThanSpecial(StepCntTst, Motor[No].TachoCntUp, Motor[No].Dir)) && (FALSE == Motor[No].LockRampDown))
1738  {
1739  if (0 != Motor[No].Speed)
1740  {
1741  if (TRUE == CheckLessThanSpecial(Motor[No].Power, (StepCnt * (Motor[No].RampUpFactor))/*/RAMP_FACTOR*/, Motor[No].Dir))
1742  {
1743  // if very slow ramp up then power could be calculated as 0 for a while
1744  // avoid starting and stopping
1745  Motor[No].Power = (StepCnt * (Motor[No].RampUpFactor))/RAMP_FACTOR;
1746 
1747  }
1748 
1749  if (TRUE == CheckLessThanSpecial(Motor[No].Power, 0, Motor[No].Dir))
1750  {
1751  // Avoid motor turning the wrong way
1752  Motor[No].Power = Motor[No].Power * -1;
1753  }
1754  }
1755  else
1756  {
1757  Motor[No].Power += (20 * Motor[No].Dir);
1758  }
1759  SetPower(No,Motor[No].Power);
1760  }
1761  else
1762  {
1763  // Done Stepping up
1764  Motor[No].LockRampDown = TRUE;
1765  if (TRUE == Motor[No].BrakeAfter)
1766  {
1767  Status = RampDownToBrake(No, StepCnt, Motor[No].TachoCntUp, Motor[No].Dir);
1768  }
1769  else
1770  {
1771  Status = TRUE;
1772  }
1773  }
1774 
1775  if (TRUE == Status)
1776  {
1777  // Ramp up completed check for next step
1778  if (FALSE == StepPowerCheckTachoCntConst(No, Motor[No].TachoCntUp))
1779  {
1780  StepPowerCheckTachoCntDown(No, Motor[No].TachoCntUp);
1781  }
1782  }
1783  }
1784  break;
1785 
1786  case LIMITED_UNREG_STEPCONST:
1787  {
1788  SLONG StepCnt, StepCntTst;
1789 
1790  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1791  {
1792  Motor[No].TachoCnt = 0;
1793  }
1794  else
1795  {
1796  Motor[No].TimeCnt = 0;
1797  }
1798 
1799  GetCompareCounts(No, &StepCnt, &StepCntTst);
1800 
1801  if ((TRUE == CheckLessThanSpecial(StepCntTst, Motor[No].TachoCntConst, Motor[No].Dir)) && (FALSE == Motor[No].LockRampDown))
1802  {
1803  }
1804  else
1805  {
1806  if (TRUE == Motor[No].BrakeAfter)
1807  {
1808 
1809  Motor[No].LockRampDown = TRUE;
1810  if (TRUE == RampDownToBrake(No, StepCnt, Motor[No].TachoCntConst, Motor[No].Dir))
1811  {
1812  StepPowerCheckTachoCntDown(No, Motor[No].TachoCntConst);
1813  }
1814  }
1815  else
1816  {
1817  StepPowerCheckTachoCntDown(No, Motor[No].TachoCntConst);
1818  }
1819  }
1820  }
1821  break;
1822 
1823  case LIMITED_UNREG_STEPDOWN:
1824  {
1825  SLONG StepCnt;
1826 
1827  StepCnt = *StepPowerSteps[No];
1828 
1829  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1830  {
1831  Motor[No].TachoCnt = 0;
1832  }
1833  else
1834  {
1835  Motor[No].TimeCnt = 0;
1836  }
1837 
1838  if (TRUE == CheckLessThanSpecial(StepCnt, Motor[No].TachoCntDown, Motor[No].Dir))
1839  {
1840  if ((TRUE == CheckLessThanSpecial((2 * Motor[No].Dir), Motor[No].Speed, Motor[No].Dir)) && (FALSE == MinRegEnabled[No]))
1841  {
1842  if (TRUE == CheckLessThanSpecial((4 * Motor[No].Dir), (Motor[No].TargetPower - ((StepCnt * (Motor[No].RampDownFactor))/ RAMP_FACTOR)), Motor[No].Dir))
1843  {
1844  Motor[No].Power = Motor[No].TargetPower - ((StepCnt * (Motor[No].RampDownFactor))/ RAMP_FACTOR);
1845  }
1846  SetPower(No,Motor[No].Power);
1847  }
1848  else
1849  {
1850 
1851  MinRegEnabled[No] = TRUE;
1852  Motor[No].TargetSpeed = (2 * Motor[No].Dir);
1853  dRegulateSpeed(No);
1854  }
1855  }
1856  else
1857  {
1858  StepPowerStopMotor(No, Motor[No].TachoCntDown);
1859  }
1860  }
1861  break;
1862 
1863  case LIMITED_STEP_SYNC:
1864  {
1865  // Here motor are syncronized and supposed to drive straight
1866 
1867  UBYTE Cnt;
1868  UBYTE Status;
1869  SLONG StepCnt;
1870 
1871  StepCnt = *StepPowerSteps[No];
1872 
1873  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1874  {
1875  Motor[No].TachoCnt = 0;
1876  }
1877  else
1878  {
1879  Motor[No].TimeCnt = 0;
1880  }
1881 
1882  Status = FALSE;
1883 
1884  if ((Motor[SyncMNos[0]].Power > (MAX_PWM_CNT - 100)) || (Motor[SyncMNos[0]].Power < (-MAX_PWM_CNT + 100)))
1885  {
1886  // Regulation is stretched to the limit....... Checked in both directions
1887  Status = TRUE;
1888  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[0]].Speed), (SLONG)(Motor[SyncMNos[0]].TargetSpeed), Motor[SyncMNos[0]].Dir))
1889  {
1890  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[0]].Speed), (SLONG)(MaxSyncSpeed), Motor[SyncMNos[0]].Dir))
1891  {
1892  // Check for running the same direction as target speed
1893  if (((Motor[SyncMNos[0]].Speed <= 0) && (Motor[SyncMNos[0]].TargetSpeed <= 0)) ||
1894  ((Motor[SyncMNos[0]].Speed >= 0) && (Motor[SyncMNos[0]].TargetSpeed >= 0)))
1895  {
1896  MaxSyncSpeed = Motor[SyncMNos[0]].Speed;
1897  }
1898  }
1899  }
1900  }
1901  if ((Motor[SyncMNos[1]].Power > (MAX_PWM_CNT - 100)) || (Motor[SyncMNos[1]].Power < (-MAX_PWM_CNT + 100)))
1902  {
1903  // Regulation is stretched to the limit....... Checked in both directions
1904  Status = TRUE;
1905  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[1]].Speed), (SLONG)(Motor[SyncMNos[1]].TargetSpeed), (Motor[SyncMNos[0]].Dir * Motor[SyncMNos[1]].Dir)))
1906  {
1907  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[1]].Speed), (SLONG)(MaxSyncSpeed), (Motor[SyncMNos[0]].Dir * Motor[SyncMNos[1]].Dir)))
1908  {
1909  // Check for running the same direction as target speed
1910  if (((Motor[SyncMNos[1]].Speed <= 0) && (Motor[SyncMNos[1]].TargetSpeed <= 0)) ||
1911  ((Motor[SyncMNos[1]].Speed >= 0) && (Motor[SyncMNos[1]].TargetSpeed >= 0)))
1912  {
1913  MaxSyncSpeed = Motor[SyncMNos[1]].Speed;
1914  }
1915  }
1916  }
1917  }
1918 
1919  if (FALSE == Status)
1920  {
1921  if (TRUE == CheckLessThanSpecial((SLONG)(MaxSyncSpeed), Motor[SyncMNos[0]].TargetPower, Motor[SyncMNos[0]].Dir))
1922  {
1923  // TargetSpeed has been reduced but now there is room to increase
1924  IncSpeed(Motor[SyncMNos[0]].Dir, &MaxSyncSpeed);
1925  }
1926  }
1927 
1928  for (Cnt = 0; Cnt < MAX_SYNC_MOTORS; Cnt++)
1929  {
1930  //Update all motors to the max sync speed
1931  Motor[SyncMNos[Cnt]].TargetSpeed = MaxSyncSpeed;
1932  }
1933 
1934  if (TRUE == CheckLessThanSpecial(Motor[SyncMNos[1]].TachoCnt, Motor[SyncMNos[0]].TachoCnt, Motor[SyncMNos[0]].Dir))
1935  {
1936  DecSpeed(Motor[SyncMNos[0]].Dir, &(Motor[SyncMNos[0]].TargetSpeed));
1937  }
1938 
1939  if (TRUE == CheckLessThanSpecial(Motor[SyncMNos[0]].TachoCnt, Motor[SyncMNos[1]].TachoCnt, Motor[SyncMNos[0]].Dir))
1940  {
1941  DecSpeed(Motor[SyncMNos[0]].Dir, &(Motor[SyncMNos[1]].TargetSpeed));
1942  }
1943 
1944  dRegulateSpeed(SyncMNos[0]);
1945  dRegulateSpeed(SyncMNos[1]);
1946 
1947  CheckforEndOfSync();
1948 
1949  }
1950  break;
1951  case SYNCED_SLAVE:
1952  {
1953  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1954  {
1955  Motor[No].TachoCnt = 0;
1956  }
1957  else
1958  {
1959  Motor[No].TimeCnt = 0;
1960  }
1961  }
1962  break;
1963  case LIMITED_DIFF_TURN_SYNC:
1964  {
1965  UBYTE Status;
1966  SLONG StepCnt;
1967 
1968  StepCnt = *StepPowerSteps[No];
1969 
1970  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
1971  {
1972  Motor[No].TachoCnt = 0;
1973  }
1974  else
1975  {
1976  Motor[No].TimeCnt = 0;
1977  }
1978 
1979  Status = FALSE;
1980 
1981  if ((Motor[SyncMNos[0]].Power > (MAX_PWM_CNT - 100)) || (Motor[SyncMNos[0]].Power < (-MAX_PWM_CNT + 100)))
1982  {
1983  // Regulation is stretched to the limit....... in both directions
1984  Status = TRUE;
1985  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[0]].Speed), (SLONG)(Motor[SyncMNos[0]].TargetSpeed), Motor[SyncMNos[0]].Dir))
1986  {
1987  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[0]].Speed), (SLONG)(MaxSyncSpeed), Motor[SyncMNos[0]].Dir))
1988  {
1989  // Check for running the same direction as target speed
1990  if (((Motor[SyncMNos[0]].Speed <= 0) && (Motor[SyncMNos[0]].TargetSpeed <= 0)) ||
1991  ((Motor[SyncMNos[0]].Speed >= 0) && (Motor[SyncMNos[0]].TargetSpeed >= 0)))
1992  {
1993  MaxSyncSpeed = Motor[SyncMNos[0]].Speed;
1994  }
1995  }
1996  }
1997  }
1998 
1999  if ((Motor[SyncMNos[1]].Power > (MAX_PWM_CNT - 100)) || (Motor[SyncMNos[1]].Power < (-MAX_PWM_CNT + 100)))
2000  {
2001  // Regulation is stretched to the limit....... in both directions
2002  Status = TRUE;
2003  if (TRUE == CheckLessThanSpecial((SLONG)(Motor[SyncMNos[1]].Speed), (SLONG)(Motor[SyncMNos[1]].TargetSpeed), (Motor[SyncMNos[1]].Dir * Motor[SyncMNos[0]].Dir)))
2004  {
2005  if (TRUE == CheckLessThanSpecial((SLONG)((Motor[SyncMNos[1]].Speed * 100)/(Motor[SyncMNos[1]].TurnRatio * Motor[SyncMNos[1]].Dir)), (SLONG)(MaxSyncSpeed), Motor[SyncMNos[0]].Dir))
2006  {
2007  if ((0 == Motor[SyncMNos[1]].TurnRatio) || (0 == Motor[SyncMNos[1]].Speed))
2008  {
2009  MaxSyncSpeed = 0;
2010  }
2011  else
2012  {
2013  // Check for running the same direction as target speed
2014  if (((Motor[SyncMNos[1]].Speed <= 0) && (Motor[SyncMNos[1]].TargetSpeed <= 0)) ||
2015  ((Motor[SyncMNos[1]].Speed >= 0) && (Motor[SyncMNos[1]].TargetSpeed >= 0)))
2016  {
2017  MaxSyncSpeed = ((Motor[SyncMNos[1]].Speed * 100)/Motor[SyncMNos[1]].TurnRatio) * Motor[SyncMNos[1]].Dir;
2018  }
2019  }
2020  }
2021  }
2022  }
2023 
2024  if (FALSE == Status)
2025  {
2026  if (TRUE == CheckLessThanSpecial((SLONG)(MaxSyncSpeed), Motor[SyncMNos[0]].TargetPower, Motor[SyncMNos[0]].Dir))
2027  {
2028  // TargetSpeed has been reduced but now there is room to increase
2029  IncSpeed(Motor[SyncMNos[0]].Dir, &MaxSyncSpeed);
2030  }
2031  }
2032 
2033  // Set the new
2034  Motor[SyncMNos[0]].TargetSpeed = MaxSyncSpeed;
2035  if ((0 == Motor[SyncMNos[1]].TurnRatio) || (0 == MaxSyncSpeed))
2036  {
2037  Motor[SyncMNos[1]].TargetSpeed = 0;
2038  }
2039  else
2040  {
2041  Motor[SyncMNos[1]].TargetSpeed = ((MaxSyncSpeed * Motor[SyncMNos[1]].TurnRatio)/100) * Motor[SyncMNos[1]].Dir;
2042  }
2043 
2044  if(0 != Motor[SyncMNos[1]].TurnRatio)
2045  {
2046  if (TRUE == CheckLessThanSpecial((((Motor[SyncMNos[1]].TachoCnt * 100)/Motor[SyncMNos[1]].TurnRatio) * Motor[SyncMNos[1]].Dir), Motor[SyncMNos[0]].TachoCnt, Motor[SyncMNos[0]].Dir))
2047  {
2048  DecSpeed(Motor[SyncMNos[0]].Dir, &(Motor[SyncMNos[0]].TargetSpeed));
2049  }
2050  }
2051 
2052  if(0 != Motor[SyncMNos[1]].TurnRatio)
2053  {
2054  if (TRUE == CheckLessThanSpecial(Motor[SyncMNos[0]].TachoCnt, ((Motor[SyncMNos[1]].TachoCnt * 100)/(Motor[SyncMNos[1]].TurnRatio) * Motor[SyncMNos[1]].Dir), Motor[SyncMNos[0]].Dir))
2055  {
2056  DecSpeed(Motor[SyncMNos[1]].Dir * Motor[SyncMNos[0]].Dir, &(Motor[SyncMNos[1]].TargetSpeed));
2057  }
2058  }
2059 
2060  dRegulateSpeed(SyncMNos[0]);
2061  dRegulateSpeed(SyncMNos[1]);
2062 
2063  CheckforEndOfSync();
2064 
2065  }
2066  break;
2067 
2068  case RAMP_DOWN_SYNC:
2069  {
2070 
2071  SLONG Count0, Count1;
2072 
2073  if (StepPowerSteps[No] != &(Motor[No].TachoCnt))
2074  {
2075  Motor[No].TachoCnt = 0;
2076  }
2077  else
2078  {
2079  Motor[No].TimeCnt = 0;
2080  }
2081 
2082  // Duration is either dependent on timer ticks or tacho counts
2083  GetSyncDurationCnt(&Count0, &Count1);
2084 
2085  if (TRUE == CheckLessThanSpecial(Count0, Motor[SyncMNos[0]].TachoCntConst, Motor[SyncMNos[0]].Dir))
2086  {
2087 
2088  RampDownToBrake(SyncMNos[0], Count0, Motor[SyncMNos[0]].TachoCntConst, Motor[SyncMNos[0]].Dir);
2089 
2090  if (StepPowerSteps[SyncMNos[0]] == TimerSteps[SyncMNos[0]])
2091  {
2092  // Needs to adjust second synchronised in time mode - both motor needs to run for the same
2093  // amount of time but not at the same speed
2094  RampDownToBrake(SyncMNos[1], ((Count1 * Motor[SyncMNos[1]].TurnRatio)/100),(( Motor[SyncMNos[1]].TachoCntConst * Motor[SyncMNos[1]].TurnRatio)/100), (Motor[SyncMNos[0]].Dir * Motor[SyncMNos[1]].Dir));
2095  }
2096  else
2097  {
2098  RampDownToBrake(SyncMNos[1], Count1, Motor[SyncMNos[1]].TachoCntConst, (Motor[SyncMNos[0]].Dir * Motor[SyncMNos[1]].Dir));
2099  }
2100  }
2101  else
2102  {
2103  MaxSyncSpeed = 0;
2104  Motor[SyncMNos[0]].TargetSpeed = 0;
2105  Motor[SyncMNos[1]].TargetSpeed = 0;
2106  StepPowerStopMotor(SyncMNos[0], Motor[SyncMNos[0]].TachoCntConst);
2107  StepPowerStopMotor(SyncMNos[1], Motor[SyncMNos[1]].TachoCntConst);
2108  }
2109  }
2110  break;
2111 
2112  case STOP_MOTOR:
2113  {
2114  if (PrgStopTimer[No])
2115  {
2116  PrgStopTimer[No]--;
2117  }
2118  else
2119  {
2120  Motor[No].State = IDLE;
2121  Motor[No].TargetState = UNLIMITED_UNREG;
2122  SetCoast(No);
2123  }
2124  }
2125  break;
2126 
2127  case BRAKED:
2128  {
2129  BrakeMotor(No, Motor[No].TachoCnt);
2130  }
2131  break;
2132 
2133  case IDLE:
2134  { /* Intentionally left empty */
2135  }
2136  break;
2137  default:
2138  { /* Intentionally left empty */
2139  }
2140  break;
2141  }
2142  }
2143  }
2144  return (HRTIMER_RESTART);
2145 }
2146 
2147 
2148 void GetSyncDurationCnt(SLONG *pCount0, SLONG *pCount1)
2149 {
2150  if (StepPowerSteps[SyncMNos[0]] == TimerSteps[SyncMNos[0]])
2151  {
2152  *pCount0 = *TimerSteps[SyncMNos[0]];
2153  *pCount1 = *TimerSteps[SyncMNos[1]];
2154  }
2155  else
2156  {
2157  *pCount0 = *TachoSteps[SyncMNos[0]];
2158  *pCount1 = *TachoSteps[SyncMNos[1]];
2159  }
2160 }
2161 
2162 
2163 void CheckforEndOfSync(void)
2164 {
2165  SLONG Count0, Count1;
2166  SBYTE Speed;
2167 
2168  // Duration is either dependent on timer ticks or tacho counts
2169  GetSyncDurationCnt(&Count0, &Count1);
2170 
2171  if (StepPowerSteps[SyncMNos[0]] == TimerSteps[SyncMNos[0]])
2172  {
2173  Speed = (Motor[SyncMNos[0]].Speed);
2174  }
2175  else
2176  {
2177  Speed = (Motor[SyncMNos[0]].Speed/2);
2178  }
2179 
2180  if ((TRUE == CheckLessThanSpecial(Count0 + (SLONG)(Speed), Motor[SyncMNos[0]].TachoCntConst,Motor[SyncMNos[0]].Dir)) ||
2181  (0 == Motor[SyncMNos[0]].TachoCntConst))
2182  {
2183  }
2184  else
2185  {
2186  Motor[SyncMNos[0]].State = RAMP_DOWN_SYNC;
2187  }
2188 }
2189 
2190 
2226 static ssize_t Device1Write(struct file *File,const char *Buffer,size_t Count,loff_t *Data)
2227 {
2228 
2229  SBYTE Buf[20];
2230  int Lng = 0;
2231 
2232  copy_from_user(Buf,Buffer,Count);
2233 
2234  switch((UBYTE)(Buf[0]))
2235  {
2236 
2237  case opPROGRAM_STOP:
2238  {
2239  UBYTE Tmp;
2240 
2241  for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
2242  {
2243 
2244  Motor[Tmp].Mutex = TRUE;
2245  ReadyStatus &= ~(0x01 << Tmp); // Clear Ready flag
2246  TestStatus &= ~(0x01 << Tmp); // Clear Test flag
2247 
2248  Motor[Tmp].Power = 0;
2249  Motor[Tmp].Speed = 0;
2250  MaxSyncSpeed = 0;
2251  Motor[Tmp].TargetSpeed = 0;
2252 
2253  if (((IDLE == Motor[Tmp].State) && ((OutputRead(Tmp,DIR0)) && (OutputRead(Tmp,DIR1)))) || (BRAKED == Motor[Tmp].State))
2254  {
2255  PrgStopTimer[Tmp] = 100/SOFT_TIMER_MS;
2256  Motor[Tmp].State = STOP_MOTOR;
2257  Motor[Tmp].TargetState = UNLIMITED_UNREG;
2258  SetBrake(Tmp);
2259  }
2260  else
2261  {
2262  Motor[Tmp].State = IDLE;
2263  Motor[Tmp].TargetState = UNLIMITED_UNREG;
2264  SetCoast(Tmp);
2265  }
2266  Motor[Tmp].Mutex = FALSE;
2267  }
2268  }
2269 
2270  case opPROGRAM_START:
2271  {
2272  UBYTE Tmp;
2273 
2274  for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
2275  {
2276  Motor[Tmp].Pol = 1;
2277  }
2278  SyncMNos[0] = UNUSED_SYNC_MOTOR;
2279  SyncMNos[1] = UNUSED_SYNC_MOTOR;
2280  }
2281  break;
2282 
2283  case opOUTPUT_SET_TYPE:
2284  {
2285  UBYTE Tmp;
2286 
2287  for (Tmp = 0; Tmp < OUTPUTS; Tmp++)
2288  {
2289  if (Buf[Tmp + 1] != Motor[Tmp].Type)
2290  {
2291  Motor[Tmp].Mutex = TRUE;
2292  if ((TYPE_TACHO == Buf[Tmp + 1]) || (TYPE_MINITACHO == Buf[Tmp + 1]))
2293  {
2294  // There is a motor attached the port
2295  CLEARTachoArray(Tmp);
2296  SETMotorType(Tmp, Buf[Tmp + 1]); // Motor types can be: TYPE_TACHO, TYPE_NONE, TYPE_MINITACHO
2297  SETAvgTachoCount(Tmp, 0); // Switching motor => speed = 0
2298  }
2299  else
2300  {
2301  // No motor is connected
2302  Motor[Tmp].State = IDLE;
2303  SetCoast(Tmp);
2304  }
2305  Motor[Tmp].Type = Buf[Tmp + 1];
2306 
2307  // All counts are reset when motor type changes
2308  Motor[Tmp].TachoCnt = 0;
2309  pMotor[Tmp].TachoCounts = 0;
2310  Motor[Tmp].TimeCnt = 0;
2311  pMotor[Tmp].TachoSensor = 0;
2312  Motor[Tmp].TachoSensor = 0;
2313  Motor[Tmp].Mutex = FALSE;
2314  }
2315  }
2316  }
2317  break;
2318 
2319  case opOUTPUT_RESET:
2320  {
2321  UBYTE Tmp;
2322 
2323  TestAndFloatSyncedMotors(Buf[1], FALSE);
2324 
2325  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2326  {
2327  if (Buf[1] & (1 << Tmp))
2328  {
2329  Motor[Tmp].Mutex = TRUE;
2330  Motor[Tmp].TachoCnt = 0;
2331  pMotor[Tmp].TachoCounts = 0;
2332  Motor[Tmp].TimeCnt = 0;
2333  Motor[Tmp].Mutex = FALSE;
2334  }
2335  }
2336  }
2337  break;
2338 
2339  case opOUTPUT_CLR_COUNT:
2340  {
2341  UBYTE Tmp;
2342  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2343  {
2344  if (Buf[1] & (1 << Tmp))
2345  {
2346  Motor[Tmp].Mutex = TRUE;
2347  pMotor[Tmp].TachoSensor = 0;
2348  Motor[Tmp].TachoSensor = 0;
2349  Motor[Tmp].Mutex = FALSE;
2350  }
2351  }
2352  }
2353  break;
2354 
2355  case opOUTPUT_STOP:
2356  {
2357  UBYTE Tmp;
2358 
2359  TestAndFloatSyncedMotors(Buf[1], FALSE);
2360 
2361  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2362  {
2363  if (Buf[1] & (1 << Tmp))
2364  {
2365  Motor[Tmp].Mutex = TRUE;
2366 
2367  if (Buf[2])
2368  {
2369  StopAndBrakeMotor(Tmp);
2370  }
2371  else
2372  {
2373  StopAndFloatMotor(Tmp);
2374  }
2375 
2376  Motor[Tmp].Mutex = FALSE;
2377  }
2378  }
2379  }
2380  break;
2381 
2382  case opOUTPUT_POWER:
2383  {
2384  UBYTE Tmp;
2385 
2386  TestAndFloatSyncedMotors(Buf[1], FALSE);
2387 
2388  CheckSpeedPowerLimits(&(Buf[2]));
2389 
2390  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2391  {
2392  if (Buf[1] & (1 << Tmp))
2393  {
2394  Motor[Tmp].Mutex = TRUE;
2395  Motor[Tmp].TargetPower = (SLONG)(Buf[2]) * (SLONG)(Motor[Tmp].Pol) * (SLONG)SPEED_PWMCNT_REL;
2396 
2397  if ((IDLE == Motor[Tmp].State) || (BRAKED == Motor[Tmp].State))
2398  {
2399  Motor[Tmp].TargetState = UNLIMITED_UNREG;
2400  }
2401  Motor[Tmp].Mutex = FALSE;
2402  }
2403  }
2404  }
2405  break;
2406 
2407  case opOUTPUT_SPEED:
2408  {
2409  UBYTE Tmp;
2410 
2411  TestAndFloatSyncedMotors(Buf[1], FALSE);
2412 
2413  CheckSpeedPowerLimits(&(Buf[2]));
2414 
2415  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2416  {
2417  if (Buf[1] & (1 << Tmp))
2418  {
2419  Motor[Tmp].Mutex = TRUE;
2420  Motor[Tmp].TargetSpeed = (Buf[2]) * (Motor[Tmp].Pol);
2421  if ((IDLE == Motor[Tmp].State) || (BRAKED == Motor[Tmp].State))
2422  {
2423  Motor[Tmp].TargetState = UNLIMITED_REG;
2424  }
2425  Motor[Tmp].Mutex = FALSE;
2426  }
2427  }
2428  }
2429  break;
2430 
2431  case opOUTPUT_START:
2432  {
2433  UBYTE Tmp;
2434  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2435  {
2436  if (Buf[1] & (1 << Tmp))
2437  {
2438  Motor[Tmp].Mutex = TRUE;
2439  if ((IDLE == Motor[Tmp].State) || (BRAKED == Motor[Tmp].State))
2440  {
2441  Motor[Tmp].State = Motor[Tmp].TargetState;
2442  }
2443  Motor[Tmp].Mutex = FALSE;
2444  }
2445  }
2446  }
2447  break;
2448 
2449  case opOUTPUT_POLARITY:
2450  {
2451  UBYTE Tmp;
2452 
2453  TestAndFloatSyncedMotors(Buf[1], FALSE);
2454 
2455  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2456  {
2457  if (Buf[1] & (1 << Tmp))
2458  {
2459  Motor[Tmp].Mutex = TRUE;
2460  if (0 == (SBYTE)Buf[2])
2461  {
2462  /* 0 == Toggle */
2463  Motor[Tmp].TargetPower = (Motor[Tmp].TargetPower) * -1;
2464  Motor[Tmp].TargetSpeed = (Motor[Tmp].TargetSpeed) * -1;
2465 
2466  if (1 == Motor[Tmp].Pol)
2467  {
2468  Motor[Tmp].Pol = -1;
2469  }
2470  else
2471  {
2472  Motor[Tmp].Pol = 1;
2473  }
2474  }
2475  else
2476  {
2477 
2478  if (Motor[Tmp].Pol != (SBYTE)Buf[2])
2479  {
2480  Motor[Tmp].TargetPower = (Motor[Tmp].TargetPower) * -1;
2481  Motor[Tmp].TargetSpeed = (Motor[Tmp].TargetSpeed) * -1;
2482  Motor[Tmp].Pol = Buf[2];
2483  }
2484  }
2485  SetPower(Tmp, Motor[Tmp].TargetPower);
2486  Motor[Tmp].Mutex = FALSE;
2487  }
2488  }
2489  }
2490  break;
2491 
2492  case opOUTPUT_POSITION:
2493  {
2494  }
2495  break;
2496 
2497  case opOUTPUT_STEP_POWER:
2498  {
2499  UBYTE Tmp;
2500  STEPPOWER StepPower;
2501 
2502  memcpy((UBYTE*)(&(StepPower.Cmd)), &Buf[0], sizeof(StepPower));
2503 
2504  TestAndFloatSyncedMotors(StepPower.Nos, FALSE);
2505 
2506  CheckSpeedPowerLimits(&(StepPower.Power));
2507 
2508  // Adjust if there is inconsistency between power and steps
2509  if (((StepPower.Power < 0) && ((StepPower.Step1 > 0) || (StepPower.Step2 > 0) || (StepPower.Step3 > 0))) ||
2510  ((StepPower.Power > 0) && ((StepPower.Step1 < 0) || (StepPower.Step2 < 0) || (StepPower.Step3 < 0))))
2511  {
2512  StepPower.Step1 *= -1;
2513  StepPower.Step2 *= -1;
2514  StepPower.Step3 *= -1;
2515  }
2516 
2517  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2518  {
2519  if (StepPower.Nos & (1 << Tmp))
2520  {
2521  Motor[Tmp].Mutex = TRUE;
2522 
2523  StepPowerSteps[Tmp] = TachoSteps[Tmp];
2524 
2525  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2526  TestStatus |= (0x01 << Tmp); // Set Test flag
2527 
2528  Motor[Tmp].TargetPower = StepPower.Power * SPEED_PWMCNT_REL * (Motor[Tmp].Pol);
2529  Motor[Tmp].TachoCntUp = StepPower.Step1 * (Motor[Tmp].Pol);
2530  Motor[Tmp].TachoCntConst = StepPower.Step2 * (Motor[Tmp].Pol);
2531  Motor[Tmp].TachoCntDown = StepPower.Step3 * (Motor[Tmp].Pol);
2532  Motor[Tmp].TargetBrake = StepPower.Brake;
2533  if (Motor[Tmp].TargetPower >= 0)
2534  {
2535  Motor[Tmp].Dir = 1;
2536  }
2537  else
2538  {
2539  Motor[Tmp].Dir = -1;
2540  }
2541 
2542  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2543 
2544  if (FALSE == StepPowerCheckTachoCntUp(Tmp))
2545  {
2546  SLONG AdjustTachoValue = 0;
2547  if (FALSE == StepPowerCheckTachoCntConst(Tmp, AdjustTachoValue))
2548  {
2549  StepPowerCheckTachoCntDown(Tmp, AdjustTachoValue);
2550  }
2551  }
2552  Motor[Tmp].Mutex = FALSE;
2553  }
2554  }
2555  }
2556  break;
2557 
2558  case opOUTPUT_TIME_POWER:
2559  {
2560  UBYTE Tmp;
2561  TIMEPOWER TimePower;
2562  SLONG Inc;
2563 
2564  memcpy((UBYTE*)(&(TimePower.Cmd)), &Buf[0], sizeof(TimePower));
2565 
2566  TestAndFloatSyncedMotors(TimePower.Nos, FALSE);
2567 
2568  // Adjust if there is inconsistency between power and Time
2569  if (((TimePower.Power < 0) && ((TimePower.Time1 > 0) || (TimePower.Time2 > 0) || (TimePower.Time3 > 0))) ||
2570  ((TimePower.Power > 0) && ((TimePower.Time1 < 0) || (TimePower.Time2 < 0) || (TimePower.Time3 < 0))))
2571  {
2572  TimePower.Time1 *= -1;
2573  TimePower.Time2 *= -1;
2574  TimePower.Time3 *= -1;
2575  }
2576 
2577  if ((TimePower.Time1 > 0) || (TimePower.Time2 > 0) || (TimePower.Time3 > 0))
2578  {
2579  Inc = SOFT_TIMER_MS;
2580  }
2581  else
2582  {
2583  Inc = -SOFT_TIMER_MS;
2584  }
2585 
2586  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2587  {
2588  if (TimePower.Nos & (1 << Tmp))
2589  {
2590  Motor[Tmp].Mutex = TRUE;
2591 
2592  StepPowerSteps[Tmp] = TimerSteps[Tmp];
2593  Motor[Tmp].TimeInc = Inc * (Motor[Tmp].Pol);
2594  *StepPowerSteps[Tmp] = 0;
2595 
2596  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2597  TestStatus |= (0x01 << Tmp); // Set Test flag
2598 
2599  Motor[Tmp].TargetPower = TimePower.Power * SPEED_PWMCNT_REL * (Motor[Tmp].Pol);
2600  Motor[Tmp].TachoCntUp = TimePower.Time1 * (Motor[Tmp].Pol);
2601  Motor[Tmp].TachoCntConst = TimePower.Time2 * (Motor[Tmp].Pol);
2602  Motor[Tmp].TachoCntDown = TimePower.Time3 * (Motor[Tmp].Pol);
2603  Motor[Tmp].TargetBrake = TimePower.Brake;
2604 
2605  if (Motor[Tmp].TargetPower >= 0)
2606  {
2607  Motor[Tmp].Dir = 1;
2608  }
2609  else
2610  {
2611  Motor[Tmp].Dir = -1;
2612  }
2613 
2614  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2615 
2616  if (FALSE == StepPowerCheckTachoCntUp(Tmp))
2617  {
2618  SLONG AdjustTachoValue = 0;
2619  if (FALSE == StepPowerCheckTachoCntConst(Tmp, AdjustTachoValue))
2620  {
2621  StepPowerCheckTachoCntDown(Tmp, AdjustTachoValue);
2622  }
2623  }
2624  Motor[Tmp].Mutex = FALSE;
2625  }
2626  }
2627  }
2628  break;
2629 
2630  case opOUTPUT_STEP_SPEED:
2631  {
2632  UBYTE Tmp;
2633  STEPSPEED StepSpeed;
2634 
2635  memcpy((UBYTE*)(&(StepSpeed.Cmd)), &Buf[0], sizeof(StepSpeed));
2636 
2637  TestAndFloatSyncedMotors(StepSpeed.Nos, FALSE);
2638 
2639  CheckSpeedPowerLimits(&(StepSpeed.Speed));
2640 
2641  // Adjust if there is inconsistency between Speed and Steps
2642  if (((StepSpeed.Speed < 0) && ((StepSpeed.Step1 > 0) || (StepSpeed.Step2 > 0) || (StepSpeed.Step3 > 0))) ||
2643  ((StepSpeed.Speed > 0) && ((StepSpeed.Step1 < 0) || (StepSpeed.Step2 < 0) || (StepSpeed.Step3 < 0))))
2644  {
2645  StepSpeed.Step1 *= -1;
2646  StepSpeed.Step2 *= -1;
2647  StepSpeed.Step3 *= -1;
2648  }
2649 
2650  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2651  {
2652  if (StepSpeed.Nos & (1 << Tmp))
2653  {
2654  Motor[Tmp].Mutex = TRUE;
2655 
2656  StepPowerSteps[Tmp] = TachoSteps[Tmp];
2657 
2658  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2659  TestStatus |= (0x01 << Tmp); // Set Test flag
2660 
2661  Motor[Tmp].TargetPower = StepSpeed.Speed * (Motor[Tmp].Pol);
2662  Motor[Tmp].TachoCntUp = StepSpeed.Step1 * (Motor[Tmp].Pol);
2663  Motor[Tmp].TachoCntConst = StepSpeed.Step2 * (Motor[Tmp].Pol);
2664  Motor[Tmp].TachoCntDown = StepSpeed.Step3 * (Motor[Tmp].Pol);
2665  Motor[Tmp].TargetBrake = StepSpeed.Brake;
2666 
2667  if (Motor[Tmp].TargetPower >= 0)
2668  {
2669  Motor[Tmp].Dir = 1;
2670  }
2671  else
2672  {
2673  Motor[Tmp].Dir = -1;
2674  }
2675 
2676  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2677 
2678  if (FALSE == StepSpeedCheckTachoCntUp(Tmp))
2679  {
2680  SLONG AdjustTachoValue = 0;
2681  if (FALSE == StepSpeedCheckTachoCntConst(Tmp, AdjustTachoValue))
2682  {
2683  StepSpeedCheckTachoCntDown(Tmp, AdjustTachoValue);
2684  }
2685  }
2686  Motor[Tmp].TargetSpeed = Motor[Tmp].TargetPower;
2687  Motor[Tmp].Mutex = FALSE;
2688  }
2689  }
2690  }
2691  break;
2692 
2693  case opOUTPUT_TIME_SPEED:
2694  {
2695  UBYTE Tmp;
2696  SLONG Inc;
2697  TIMESPEED TimeSpeed;
2698 
2699 
2700  memcpy((UBYTE*)(&(TimeSpeed.Cmd)), &Buf[0], sizeof(TimeSpeed));
2701 
2702  TestAndFloatSyncedMotors(TimeSpeed.Nos, FALSE);
2703 
2704  CheckSpeedPowerLimits(&(TimeSpeed.Speed));
2705 
2706  // Adjust if there is inconsistency between Speed and Time
2707  if (((TimeSpeed.Speed < 0) && ((TimeSpeed.Time1 > 0) || (TimeSpeed.Time2 > 0) || (TimeSpeed.Time3 > 0))) ||
2708  ((TimeSpeed.Speed > 0) && ((TimeSpeed.Time1 < 0) || (TimeSpeed.Time2 < 0) || (TimeSpeed.Time3 < 0))))
2709  {
2710  TimeSpeed.Time1 *= -1;
2711  TimeSpeed.Time2 *= -1;
2712  TimeSpeed.Time3 *= -1;
2713  }
2714 
2715  if ((TimeSpeed.Time1 > 0) || (TimeSpeed.Time2 > 0) || (TimeSpeed.Time3 > 0))
2716  {
2717  Inc = SOFT_TIMER_MS;
2718  }
2719  else
2720  {
2721  Inc = -SOFT_TIMER_MS;
2722  }
2723 
2724  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2725  {
2726  if (TimeSpeed.Nos & (1 << Tmp))
2727  {
2728  Motor[Tmp].Mutex = TRUE;
2729 
2730  StepPowerSteps[Tmp] = TimerSteps[Tmp];
2731  Motor[Tmp].TimeInc = Inc * (Motor[Tmp].Pol);
2732  *StepPowerSteps[Tmp] = 0;
2733 
2734  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2735  TestStatus |= (0x01 << Tmp); // Set Test flag
2736 
2737  Motor[Tmp].TargetPower = TimeSpeed.Speed * (Motor[Tmp].Pol);
2738  Motor[Tmp].TachoCntUp = TimeSpeed.Time1 * (Motor[Tmp].Pol);
2739  Motor[Tmp].TachoCntConst = TimeSpeed.Time2 * (Motor[Tmp].Pol);
2740  Motor[Tmp].TachoCntDown = TimeSpeed.Time3 * (Motor[Tmp].Pol);
2741  Motor[Tmp].TargetBrake = TimeSpeed.Brake;
2742 
2743  if (Motor[Tmp].TargetPower >= 0)
2744  {
2745  Motor[Tmp].Dir = 1;
2746  }
2747  else
2748  {
2749  Motor[Tmp].Dir = -1;
2750  }
2751 
2752  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2753 
2754  if (FALSE == StepSpeedCheckTachoCntUp(Tmp))
2755  {
2756  SLONG AdjustTachoValue = 0;
2757  if (FALSE == StepSpeedCheckTachoCntConst(Tmp, AdjustTachoValue))
2758  {
2759  StepSpeedCheckTachoCntDown(Tmp, AdjustTachoValue);
2760  }
2761  }
2762  Motor[Tmp].TargetSpeed = Motor[Tmp].TargetPower;
2763  Motor[Tmp].Mutex = FALSE;
2764  }
2765  }
2766  }
2767  break;
2768 
2769  case opOUTPUT_STEP_SYNC:
2770  {
2771  UBYTE No = 0;
2772  UBYTE Tmp;
2773  STEPSYNC StepSync;
2774 
2775  memcpy((UBYTE*)(&(StepSync.Cmd)), &Buf[0], sizeof(StepSync));
2776 
2777  TestAndFloatSyncedMotors(StepSync.Nos, TRUE);
2778 
2779  //Check if exceeding speed limits
2780  CheckSpeedPowerLimits(&(StepSync.Speed));
2781 
2782  if (((StepSync.Speed < 0) && (StepSync.Step > 0)) ||
2783  ((StepSync.Speed > 0) && (StepSync.Step < 0)))
2784  {
2785  StepSync.Step *= -1;
2786  }
2787 
2788  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2789  {
2790 
2791  if (StepSync.Nos & (1 << Tmp))
2792  {
2793  Motor[Tmp].Mutex = TRUE;
2794 
2795  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2796  TestStatus |= (0x01 << Tmp); // Set Test flag
2797  StepPowerSteps[Tmp] = TachoSteps[Tmp];
2798  Motor[Tmp].TargetBrake = StepSync.Brake;
2799 
2800  if (0 == StepSync.Step)
2801  {
2802  // If run forever
2803  *StepPowerSteps[Tmp] = 0;
2804  }
2805 
2806  if (0 == StepSync.Turn)
2807  {
2808  // Synced motors are going straight
2809  Motor[Tmp].TargetPower = StepSync.Speed;
2810  Motor[Tmp].TurnRatio = 100;
2811  Motor[Tmp].TachoCntConst = StepSync.Step;
2812  SyncMNos[No] = Tmp;
2813  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2814  MaxSyncSpeed = StepSync.Speed;
2815 
2816  // Find the direction the main motor will drive
2817  if (0 <= StepSync.Speed)
2818  {
2819  Motor[Tmp].Dir = NON_INV;
2820  }
2821  else
2822  {
2823  Motor[Tmp].Dir = INV;
2824  }
2825 
2826  if (0 == No)
2827  {
2828  Motor[Tmp].State = LIMITED_STEP_SYNC;
2829  }
2830  else
2831  {
2832  Motor[Tmp].State = SYNCED_SLAVE;
2833  Motor[Tmp].Dir = NON_INV;
2834  }
2835  }
2836  else
2837  {
2838  // Turning
2839  UBYTE MotorIndex;
2840 
2841  if (0 < StepSync.Turn)
2842  {
2843  MotorIndex = No;
2844  }
2845  else
2846  {
2847  // Invert if turning left (right motor runs fastest)
2848  MotorIndex = 1 - No;
2849  }
2850 
2851  if (0 == MotorIndex)
2852  {
2853  if (StepSync.Speed > 0)
2854  {
2855  Motor[Tmp].Dir = NON_INV;
2856  }
2857  else
2858  {
2859  Motor[Tmp].Dir = INV;
2860  }
2861  Motor[Tmp].TurnRatio = StepSync.Turn;
2862  }
2863  else
2864  {
2865  if ((StepSync.Turn < 100) && (StepSync.Turn > -100))
2866  {
2867  Motor[Tmp].Dir = NON_INV;
2868 
2869  if (StepSync.Turn > 0) // Invert the ratio in the first quarter
2870  {
2871  Motor[Tmp].TurnRatio = 100 - StepSync.Turn;
2872  }
2873  else
2874  {
2875  Motor[Tmp].TurnRatio = -100 - StepSync.Turn;
2876  }
2877  }
2878  else
2879  {
2880  if ((StepSync.Turn >= 100) || (StepSync.Turn <= -100))
2881  {
2882  // Both motors are turning
2883  Motor[Tmp].Dir = INV;
2884  if (StepSync.Turn <= -100)
2885  {
2886  Motor[Tmp].TurnRatio = (StepSync.Turn + 100);
2887  }
2888  else
2889  {
2890  Motor[Tmp].TurnRatio = (StepSync.Turn - 100);
2891  }
2892  }
2893  }
2894  }
2895 
2896  SyncMNos[MotorIndex] = Tmp;
2897 
2898  if (0 > StepSync.Turn)
2899  {
2900  Motor[Tmp].TurnRatio *= -1;
2901  }
2902 
2903  if (0 == MotorIndex)
2904  {
2905  Motor[Tmp].TargetPower = StepSync.Speed;
2906  Motor[Tmp].TachoCntConst = StepSync.Step;
2907  MaxSyncSpeed = StepSync.Speed;
2908  Motor[Tmp].State = LIMITED_DIFF_TURN_SYNC;
2909  }
2910  else
2911  {
2912  Motor[Tmp].TargetPower = (SLONG)(((SLONG)(StepSync.Speed) * (SLONG)(Motor[Tmp].TurnRatio))/((SLONG)100 * Motor[Tmp].Dir));
2913  Motor[Tmp].TachoCntConst = ((StepSync.Step * (SLONG)(Motor[Tmp].TurnRatio))/100) * Motor[Tmp].Dir;
2914  Motor[Tmp].State = SYNCED_SLAVE;
2915  }
2916 
2917  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2918  }
2919  No++;
2920  }
2921  }
2922  Motor[SyncMNos[0]].Mutex = FALSE;
2923  Motor[SyncMNos[1]].Mutex = FALSE;
2924  }
2925  break;
2926 
2927  case opOUTPUT_TIME_SYNC:
2928  {
2929  SLONG Inc;
2930  UBYTE No = 0;
2931  UBYTE Tmp;
2932  TIMESYNC TimeSync;
2933 
2934  memcpy((UBYTE*)(&(TimeSync.Cmd)), &Buf[0], sizeof(TimeSync));
2935 
2936  TestAndFloatSyncedMotors(TimeSync.Nos, TRUE);
2937 
2938  //Check if exceeding speed limits
2939  CheckSpeedPowerLimits(&(TimeSync.Speed));
2940 
2941  if (((TimeSync.Speed < 0) && (TimeSync.Time > 0)) ||
2942  ((TimeSync.Speed > 0) && (TimeSync.Time < 0)))
2943  {
2944  TimeSync.Time *= -1;
2945  }
2946 
2947  if (TimeSync.Time > 0)
2948  {
2949  Inc = SOFT_TIMER_MS;
2950  }
2951  else
2952  {
2953  Inc = -SOFT_TIMER_MS;
2954  }
2955 
2956  for (Tmp = 0;Tmp < OUTPUTS;Tmp++)
2957  {
2958  if (TimeSync.Nos & (1 << Tmp))
2959  {
2960  Motor[Tmp].Mutex = TRUE;
2961 
2962  ReadyStatus |= (0x01 << Tmp); // Set Ready flag
2963  TestStatus |= (0x01 << Tmp); // Set Test flag
2964 
2965  StepPowerSteps[Tmp] = TimerSteps[Tmp];
2966  Motor[Tmp].TimeInc = Inc;
2967  *TimerSteps[Tmp] = 0;
2968 
2969  Motor[Tmp].TargetBrake = TimeSync.Brake;
2970 
2971  if (0 == TimeSync.Turn)
2972  {
2973  // Synced motors are going straight
2974  Motor[Tmp].TargetPower = TimeSync.Speed;
2975  Motor[Tmp].TurnRatio = 100;
2976  Motor[Tmp].TachoCntConst = TimeSync.Time;
2977  SyncMNos[No] = Tmp;
2978  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
2979  MaxSyncSpeed = TimeSync.Speed;
2980 
2981  // Find the direction the main motor will drive
2982  if (0 <= TimeSync.Speed)
2983  {
2984  Motor[Tmp].Dir = NON_INV;
2985  }
2986  else
2987  {
2988  Motor[Tmp].Dir = INV;
2989  }
2990 
2991  if (0 == No)
2992  {
2993  Motor[Tmp].State = LIMITED_STEP_SYNC;
2994  }
2995  else
2996  {
2997  Motor[Tmp].State = SYNCED_SLAVE;
2998  Motor[Tmp].Dir = NON_INV;
2999  }
3000  }
3001  else
3002  {
3003  // Turning
3004  UBYTE MotorIndex;
3005 
3006  if (0 < TimeSync.Turn)
3007  {
3008  MotorIndex = No;
3009  }
3010  else
3011  {
3012  // Invert if turning left (right motor runs fastest)
3013  MotorIndex = 1 - No;
3014  }
3015 
3016  if (0 == MotorIndex)
3017  {
3018  if (TimeSync.Speed > 0)
3019  {
3020  Motor[Tmp].Dir = NON_INV;
3021  }
3022  else
3023  {
3024  Motor[Tmp].Dir = INV;
3025  }
3026  Motor[Tmp].TurnRatio = TimeSync.Turn;
3027  }
3028  else
3029  {
3030  if ((TimeSync.Turn < 100) && (TimeSync.Turn > -100))
3031  {
3032  Motor[Tmp].Dir = NON_INV;
3033 
3034  if (TimeSync.Turn > 0) // Invert the ratio in the first quarter
3035  {
3036  Motor[Tmp].TurnRatio = 100 - TimeSync.Turn;
3037  }
3038  else
3039  {
3040  Motor[Tmp].TurnRatio = -100 - TimeSync.Turn;
3041  }
3042  }
3043  else
3044  {
3045  if ((TimeSync.Turn >= 100) || (TimeSync.Turn <= -100))
3046  {
3047  // Both motors are turning
3048  Motor[Tmp].Dir = INV;
3049  if (TimeSync.Turn <= -100)
3050  {
3051  Motor[Tmp].TurnRatio = (TimeSync.Turn + 100);
3052  }
3053  else
3054  {
3055  Motor[Tmp].TurnRatio = (TimeSync.Turn - 100);
3056  }
3057  }
3058  }
3059  }
3060 
3061  SyncMNos[MotorIndex] = Tmp;
3062 
3063  if (0 > TimeSync.Turn)
3064  {
3065  Motor[Tmp].TurnRatio *= -1;
3066  }
3067 
3068  if (0 == MotorIndex)
3069  {
3070  Motor[Tmp].TargetPower = TimeSync.Speed;
3071  Motor[Tmp].TachoCntConst = TimeSync.Time;
3072  MaxSyncSpeed = TimeSync.Speed;
3073  Motor[Tmp].State = LIMITED_DIFF_TURN_SYNC;
3074  }
3075  else
3076  {
3077  Motor[Tmp].TargetPower = (SLONG)(((SLONG)(TimeSync.Speed) * (SLONG)(Motor[Tmp].TurnRatio))/((SLONG)100 * Motor[Tmp].Dir));
3078  Motor[Tmp].TachoCntConst = TimeSync.Time * Motor[Tmp].Dir;
3079  Motor[Tmp].TimeInc *= Motor[Tmp].Dir;
3080  Motor[Tmp].State = SYNCED_SLAVE;
3081  }
3082  TimeOutSpeed0[Tmp] = FREERunning24bittimer;
3083  }
3084  No++;
3085  }
3086  }
3087  Motor[SyncMNos[0]].Mutex = FALSE;
3088  Motor[SyncMNos[1]].Mutex = FALSE;
3089  }
3090  break;
3091 
3092  default:
3093  {
3094  }
3095  break;
3096  }
3097 
3098  return (Lng);
3099 }
3100 
3101 
3102 static ssize_t Device1Read(struct file *File,char *Buffer,size_t Count,loff_t *Offset)
3103 {
3104  int Lng = 0;
3105 
3106  Lng = snprintf(&Buffer[0],Count,"%01u ",ReadyStatus);
3107  Lng += snprintf(&Buffer[Lng],Count - Lng,"%01u ",TestStatus);
3108 
3109  return (Lng);
3110 }
3111 
3112 
3113 static const struct file_operations Device1Entries =
3114 {
3115  .owner = THIS_MODULE,
3116  .read = Device1Read,
3117  .write = Device1Write
3118 };
3119 
3120 
3121 static struct miscdevice Device1 =
3122 {
3123  MISC_DYNAMIC_MINOR,
3124  DEVICE1_NAME,
3125  &Device1Entries
3126 };
3127 
3128 
3140 void GetPeriphealBasePtr(ULONG Address, ULONG Size, ULONG **Ptr)
3141 {
3142  /* eCAP0 pointer */
3143  if (request_mem_region(Address,Size,MODULE_NAME) >= 0)
3144  {
3145 
3146  *Ptr = (ULONG*)ioremap(Address,Size);
3147 
3148  if (*Ptr != NULL)
3149  {
3150 #ifdef DEBUG
3151  printk("%s memory Remapped from 0x%08lX\n",DEVICE1_NAME,(unsigned long)*Ptr);
3152 #endif
3153  }
3154  else
3155  {
3156  printk("Memory remap ERROR");
3157  }
3158  }
3159  else
3160  {
3161  printk("Region request error");
3162  }
3163 }
3164 
3165 
3175 static int Device1Init(void)
3176 {
3177  int Result = -1;
3178  UBYTE Tmp;
3179 
3180  GetPeriphealBasePtr(0x01C14000, 0x190, (ULONG**)&SYSCFG0); /* SYSCFG0 pointer */
3181  GetPeriphealBasePtr(0x01E2C000, 0x1C, (ULONG**)&SYSCFG1); /* SYSCFG1 pointer */
3182  GetPeriphealBasePtr(0x01F02000, 0x2854,(ULONG**)&eHRPWM1); /* eHRPWM Pointer */
3183  GetPeriphealBasePtr(0x01F06000, 0x60, (ULONG**)&eCAP0); /* eCAP0 pointer */
3184  GetPeriphealBasePtr(0x01F07000, 0x60, (ULONG**)&eCAP1); /* eCAP1 pointer */
3185  GetPeriphealBasePtr(0x01F0D000, 0x80, (ULONG**)&TIMER64P3);/* TIMER64P3 pointer */
3186  GetPeriphealBasePtr(0x01E26000, 0xD4, (ULONG**)&GPIO); /* GPIO pointer */
3187  GetPeriphealBasePtr(0x01E1A000, 0x1F8, (ULONG**)&PLLC1); /* PLLC1 pointer */
3188  GetPeriphealBasePtr(0x01E27000, 0xA80, (ULONG**)&PSC1); /* PSC1 pointer */
3189 
3190  Result = misc_register(&Device1);
3191  if (Result)
3192  {
3193  printk(" %s device register failed\n",DEVICE1_NAME);
3194  }
3195  else
3196  {
3197 #ifdef DEBUG
3198  printk(" %s device register succes\n",DEVICE1_NAME);
3199 #endif
3200 
3201  iowrite32(0x00000003, &PSC1[0x291]); /* Setup ePWM module power on */
3202  iowrite32(0x00000003, &PSC1[0x48]); /* Eval the NEXT field */
3203 
3204  iowrite32((ioread32(&PSC1[0x294]) | 0x00000003), &PSC1[0x294]);/* Turn PSC on for the eCAP module */
3205  iowrite32((ioread32(&PSC1[0x48]) | 0x00000003), &PSC1[0x48]); /* Execute the next step */
3206 
3207  for(Tmp = 0; Tmp < NO_OF_OUTPUT_PORTS; Tmp++)
3208  {
3209  memset(&Motor[Tmp], 0, sizeof(MOTOR));
3210 
3211  Motor[Tmp].TargetBrake = COAST;
3212  Motor[Tmp].Pol = 1;
3213  Motor[Tmp].Direction = FORWARD;
3214  Motor[Tmp].Type = TYPE_NONE;
3215  Motor[Tmp].State = IDLE;
3216  Motor[Tmp].TargetState = UNLIMITED_UNREG; //default startup state
3217  Motor[Tmp].Mutex = FALSE;
3218  Motor[Tmp].BrakeAfter = FALSE;
3219 
3220  CLEARTachoArray(Tmp);
3221  SETMotorType(Tmp, TYPE_NONE); // Motor types can be: TYPE_TACHO, TYPE_NONE, TYPE_MINITACHO
3222  SETAvgTachoCount(Tmp, 0); // At initialisation speed is assumed to be zero
3223  }
3224 
3225  /* Float the tacho inputs */
3226  for(Tmp = 0; Tmp < NO_OF_OUTPUT_PORTS; Tmp++)
3227  {
3228  OutputFloat(Tmp,INT);
3229  OutputFloat(Tmp,DIR);
3230  SetCoast(Tmp);
3231  }
3232 
3233  SyncMNos[0] = UNUSED_SYNC_MOTOR;
3234  SyncMNos[1] = UNUSED_SYNC_MOTOR;
3235 
3236  /* Setup the PWM peripheals */
3237  SETUPPwmModules;
3238 
3239  /* Setup interrupt for the tacho int pins */
3240  SetGpioRisingIrq(IRQA_PINNO, IntA);
3241  SetGpioRisingIrq(IRQB_PINNO, IntB);
3242  SetGpioRisingIrq(IRQC_PINNO, IntC);
3243  SetGpioRisingIrq(IRQD_PINNO, IntD);
3244  }
3245  return (Result);
3246 }
3247 
3248 
3249 static void Device1Exit(void)
3250 {
3251  hrtimer_cancel(&Device1Timer);
3252  misc_deregister(&Device1);
3253 #ifdef DEBUG
3254  printk(" %s device unregistered\n",DEVICE1_NAME);
3255 #endif
3256  iounmap(SYSCFG0);
3257  iounmap(SYSCFG1);
3258  iounmap(GPIO);
3259  iounmap(eCAP0);
3260  iounmap(eCAP1);
3261  iounmap(TIMER64P3);
3262  iounmap(eHRPWM1);
3263  iounmap(PLLC1);
3264  iounmap(PSC1);
3265 #ifdef DEBUG
3266  printk(" %s memory unmapped\n",DEVICE1_NAME);
3267 #endif
3268 }
3269 
3270 
3271 static void __iomem *GpioBase;
3272 
3273 void SetGpio(int Pin)
3274 {
3275  int Tmp = 0;
3276  void __iomem *Reg;
3277 
3278  if (Pin >= 0)
3279  {
3280  while ((MuxRegMap[Tmp].Pin != -1) && (MuxRegMap[Tmp].Pin != Pin))
3281  {
3282  Tmp++;
3283  }
3284  if (MuxRegMap[Tmp].Pin == Pin)
3285  {
3286  Reg = da8xx_syscfg0_base + 0x120 + (MuxRegMap[Tmp].MuxReg << 2);
3287 
3288  *(u32*)Reg &= MuxRegMap[Tmp].Mask;
3289  *(u32*)Reg |= MuxRegMap[Tmp].Mode;
3290 
3291  if (Pin < NO_OF_GPIOS)
3292  {
3293 #ifdef DEBUG
3294  printk(" GP%d_%-2d 0x%08X and 0x%08X or 0x%08X\n",(Pin >> 4),(Pin & 0x0F),(u32)Reg, MuxRegMap[Tmp].Mask, MuxRegMap[Tmp].Mode);
3295 #endif
3296  }
3297  else
3298  {
3299 #ifdef DEBUG
3300  printk(" OUTPUT FUNCTION 0x%08X and 0x%08X or 0x%08X\n",(u32)Reg, MuxRegMap[Tmp].Mask, MuxRegMap[Tmp].Mode);
3301 #endif
3302  }
3303  }
3304  else
3305  {
3306  printk(" GP%d_%-2d Not found (Const no. %d, Tmp = %d)\n",(Pin >> 4),(Pin & 0x0F), Pin, Tmp);
3307  }
3308  }
3309 }
3310 
3311 
3312 void InitGpio(void)
3313 {
3314  int Port;
3315  int Pin;
3316 
3317  // unlock
3318  REGUnlock;
3319 
3320  for (Port = 0;Port < NO_OF_OUTPUT_PORTS;Port++)
3321  {
3322 #ifdef DEBUG
3323  printk(" Output port %d\n",Port + 1);
3324 #endif
3325  for (Pin = 0;Pin < OUTPUT_PORT_PINS;Pin++)
3326  {
3327  if ((pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].Pin) >= 0)
3328  {
3329  pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].pGpio = (struct gpio_controller *__iomem)(GpioBase + ((pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].Pin >> 5) * 0x28) + 0x10);
3330  pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].Mask = (1 << (pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].Pin & 0x1F));
3331 
3332  SetGpio(pOutputPortPin[Hw][(Port * OUTPUT_PORT_PINS) + Pin].Pin);
3333  }
3334  }
3335  }
3336 
3337  // lock
3338  REGLock;
3339 }
3340 
3341 
3342 void SetGpioRisingIrq(UBYTE PinNo, irqreturn_t (*IntFuncPtr)(int, void *))
3343 {
3344  UWORD Status;
3345 
3346  Status = request_irq(gpio_to_irq(PinNo), IntFuncPtr, 0, "PWM_DEVICE", NULL);
3347  if(Status < 0)
3348  {
3349  printk("error %d requesting GPIO IRQ %d\n", Status, PinNo);
3350  }
3351  set_irq_type(gpio_to_irq(PinNo), IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING);
3352 #ifdef DEBUG
3353  printk(".. done\n");
3354 #endif
3355 }
3356 
3357 
3378 static irqreturn_t IntA (int irq, void * dev)
3379 {
3380  UBYTE TmpPtr;
3381  ULONG IntAState;
3382  ULONG DirAState;
3383  ULONG Timer;
3384 
3385  // Sample all necessary items as fast as possible
3386  IntAState = READIntA;
3387  DirAState = READDirA;
3388  Timer = FREERunning24bittimer;
3389 
3390  TmpPtr = (TachoSamples[0].ArrayPtr + 1) & (NO_OF_TACHO_SAMPLES-1);
3391  TachoSamples[0].TachoArray[TmpPtr] = Timer;
3392  TachoSamples[0].ArrayPtr = TmpPtr;
3393 
3394  if ((35 < Motor[0].Speed) || (-35 > Motor[0].Speed))
3395  {
3396  if (FORWARD == Motor[0].Direction)
3397  {
3398  (Motor[0].IrqTacho)++;
3399  }
3400  else
3401  {
3402  (Motor[0].IrqTacho)--;
3403  }
3404  if (Motor[0].DirChgPtr < SamplesPerSpeed[0][SAMPLES_ABOVE_SPEED_75])
3405  {
3406  Motor[0].DirChgPtr++;
3407  }
3408  }
3409  else
3410  {
3411  if (IntAState)
3412  {
3413  if(DirAState)
3414  {
3415  if (FORWARD == Motor[0].Direction)
3416  {
3417  if (Motor[0].DirChgPtr < SamplesPerSpeed[0][SAMPLES_ABOVE_SPEED_75])
3418  {
3419  Motor[0].DirChgPtr++;
3420  }
3421  }
3422  else
3423  {
3424  Motor[0].DirChgPtr = 0;
3425  }
3426  (Motor[0].IrqTacho)++;
3427  Motor[0].Direction = FORWARD;
3428  }
3429  else
3430  {
3431  if (BACKWARD == Motor[0].Direction)
3432  {
3433  TachoSamples[0].TachoArray[TmpPtr] = Timer;
3434  TachoSamples[0].ArrayPtr = TmpPtr;
3435  if (Motor[0].DirChgPtr < SamplesPerSpeed[0][SAMPLES_ABOVE_SPEED_75])
3436  {
3437  Motor[0].DirChgPtr++;
3438  }
3439  }
3440  else
3441  {
3442  Motor[0].DirChgPtr = 0;
3443  }
3444  (Motor[0].IrqTacho)--;
3445  Motor[0].Direction = BACKWARD;
3446  }
3447  }
3448  else
3449  {
3450  if(DirAState)
3451  {
3452  if (BACKWARD == Motor[0].Direction)
3453  {
3454  if (Motor[0].DirChgPtr < SamplesPerSpeed[0][SAMPLES_ABOVE_SPEED_75])
3455  {
3456  Motor[0].DirChgPtr++;
3457  }
3458  }
3459  else
3460  {
3461  Motor[0].DirChgPtr = 0;
3462  }
3463  (Motor[0].IrqTacho)--;
3464  Motor[0].Direction = BACKWARD;
3465  }
3466  else
3467  {
3468  if (FORWARD == Motor[0].Direction)
3469  {
3470  if (Motor[0].DirChgPtr < SamplesPerSpeed[0][SAMPLES_ABOVE_SPEED_75])
3471  {
3472  Motor[0].DirChgPtr++;
3473  }
3474  }
3475  else
3476  {
3477  Motor[0].DirChgPtr = 0;
3478  }
3479  (Motor[0].IrqTacho)++;
3480  Motor[0].Direction = FORWARD;
3481  }
3482  }
3483  }
3484  return IRQ_HANDLED;
3485 }
3486 
3487 
3488 static irqreturn_t IntB (int irq, void * dev)
3489 {
3490  UBYTE volatile TmpPtr;
3491  ULONG volatile IntBState;
3492  ULONG volatile DirBState;
3493  ULONG volatile Timer;
3494 
3495  // Sample all necessary items as fast as possible
3496  IntBState = READIntB;
3497  DirBState = READDirB;
3498  Timer = FREERunning24bittimer;
3499 
3500  TmpPtr = (TachoSamples[1].ArrayPtr + 1) & (NO_OF_TACHO_SAMPLES-1);
3501  TachoSamples[1].TachoArray[TmpPtr] = Timer;
3502  TachoSamples[1].ArrayPtr = TmpPtr;
3503 
3504  if ((35 < Motor[1].Speed) || (-35 > Motor[1].Speed))
3505  {
3506  if (FORWARD == Motor[1].Direction)
3507  {
3508  (Motor[1].IrqTacho)++;
3509  }
3510  else
3511  {
3512  (Motor[1].IrqTacho)--;
3513  }
3514 
3515  if (Motor[1].DirChgPtr < SamplesPerSpeed[1][SAMPLES_ABOVE_SPEED_75])
3516  {
3517  Motor[1].DirChgPtr++;
3518  }
3519  }
3520  else
3521  {
3522  if (IntBState)
3523  {
3524  if(DirBState)
3525  {
3526  if (FORWARD == Motor[1].Direction)
3527  {
3528  if (Motor[1].DirChgPtr < SamplesPerSpeed[1][SAMPLES_ABOVE_SPEED_75])
3529  {
3530  Motor[1].DirChgPtr++;
3531  }
3532  }
3533  else
3534  {
3535  Motor[1].DirChgPtr = 0;
3536  }
3537  (Motor[1].IrqTacho)++;
3538  Motor[1].Direction = FORWARD;
3539  }
3540  else
3541  {
3542  if (BACKWARD == Motor[1].Direction)
3543  {
3544  if (Motor[1].DirChgPtr < SamplesPerSpeed[1][SAMPLES_ABOVE_SPEED_75])
3545  {
3546  Motor[1].DirChgPtr++;
3547  }
3548  }
3549  else
3550  {
3551  Motor[1].DirChgPtr = 0;
3552  }
3553  (Motor[1].IrqTacho)--;
3554  Motor[1].Direction = BACKWARD;
3555  }
3556  }
3557  else
3558  {
3559  if(DirBState)
3560  {
3561  if (BACKWARD == Motor[1].Direction)
3562  {
3563  if (Motor[1].DirChgPtr < SamplesPerSpeed[1][SAMPLES_ABOVE_SPEED_75])
3564  {
3565  Motor[1].DirChgPtr++;
3566  }
3567  }
3568  else
3569  {
3570  Motor[1].DirChgPtr = 0;
3571  }
3572  (Motor[1].IrqTacho)--;
3573  Motor[1].Direction = BACKWARD;
3574  }
3575  else
3576  {
3577  if (FORWARD == Motor[1].Direction)
3578  {
3579  if (Motor[1].DirChgPtr < SamplesPerSpeed[1][SAMPLES_ABOVE_SPEED_75])
3580  {
3581  Motor[1].DirChgPtr++;
3582  }
3583  }
3584  else
3585  {
3586  Motor[1].DirChgPtr = 0;
3587  }
3588  (Motor[1].IrqTacho)++;
3589  Motor[1].Direction = FORWARD;
3590  }
3591  }
3592  }
3593  return IRQ_HANDLED;
3594 }
3595 
3596 
3597 static irqreturn_t IntC (int irq, void * dev)
3598 {
3599  UBYTE TmpPtr;
3600  ULONG IntCState;
3601  ULONG DirCState;
3602  ULONG Timer;
3603 
3604  // Sample all necessary items as fast as possible
3605  IntCState = READIntC;
3606  DirCState = READDirC;
3607  Timer = FREERunning24bittimer;
3608 
3609  TmpPtr = (TachoSamples[2].ArrayPtr + 1) & (NO_OF_TACHO_SAMPLES-1);
3610  TachoSamples[2].TachoArray[TmpPtr] = Timer;
3611  TachoSamples[2].ArrayPtr = TmpPtr;
3612 
3613  if ((35 < Motor[2].Speed) || (-35 > Motor[2].Speed))
3614  {
3615  if (FORWARD == Motor[2].Direction)
3616  {
3617  (Motor[2].IrqTacho)++;
3618  }
3619  else
3620  {
3621  (Motor[2].IrqTacho)--;
3622  }
3623  if (Motor[2].DirChgPtr < SamplesPerSpeed[2][SAMPLES_ABOVE_SPEED_75])
3624  {
3625  Motor[2].DirChgPtr++;
3626  }
3627  }
3628  else
3629  {
3630  if (IntCState)
3631  {
3632  if(DirCState)
3633  {
3634  if (FORWARD == Motor[2].Direction)
3635  {
3636  if (Motor[2].DirChgPtr < SamplesPerSpeed[2][SAMPLES_ABOVE_SPEED_75])
3637  {
3638  Motor[2].DirChgPtr++;
3639  }
3640  }
3641  else
3642  {
3643  Motor[2].DirChgPtr = 0;
3644  }
3645  (Motor[2].IrqTacho)++;
3646  Motor[2].Direction = FORWARD;
3647  }
3648  else
3649  {
3650  if (BACKWARD == Motor[2].Direction)
3651  {
3652  if (Motor[2].DirChgPtr < SamplesPerSpeed[2][SAMPLES_ABOVE_SPEED_75])
3653  {
3654  Motor[2].DirChgPtr++;
3655  }
3656  }
3657  else
3658  {
3659  Motor[2].DirChgPtr = 0;
3660  }
3661  (Motor[2].IrqTacho)--;
3662  Motor[2].Direction = BACKWARD;
3663  }
3664  }
3665  else
3666  {
3667  if(DirCState)
3668  {
3669  if (BACKWARD == Motor[2].Direction)
3670  {
3671  if (Motor[2].DirChgPtr < SamplesPerSpeed[2][SAMPLES_ABOVE_SPEED_75])
3672  {
3673  Motor[2].DirChgPtr++;
3674  }
3675  }
3676  else
3677  {
3678  Motor[2].DirChgPtr = 0;
3679  }
3680  (Motor[2].IrqTacho)--;
3681  Motor[2].Direction = BACKWARD;
3682  }
3683  else
3684  {
3685  if (FORWARD == Motor[2].Direction)
3686  {
3687  if (Motor[2].DirChgPtr < SamplesPerSpeed[2][SAMPLES_ABOVE_SPEED_75])
3688  {
3689  Motor[2].DirChgPtr++;
3690  }
3691  }
3692  else
3693  {
3694  Motor[2].DirChgPtr = 0;
3695  }
3696  (Motor[2].IrqTacho)++;
3697  Motor[2].Direction = FORWARD;
3698  }
3699  }
3700  }
3701  return IRQ_HANDLED;
3702 }
3703 
3704 
3705 static irqreturn_t IntD (int irq, void * dev)
3706 {
3707  UBYTE TmpPtr;
3708  ULONG IntDState;
3709  ULONG DirDState;
3710  ULONG Timer;
3711 
3712  // Sample all necessary items as fast as possible
3713  IntDState = READIntD;
3714  DirDState = READDirD;
3715  Timer = FREERunning24bittimer;
3716 
3717  TmpPtr = (TachoSamples[3].ArrayPtr + 1) & (NO_OF_TACHO_SAMPLES-1);
3718  TachoSamples[3].TachoArray[TmpPtr] = Timer;
3719  TachoSamples[3].ArrayPtr = TmpPtr;
3720 
3721  if ((35 < Motor[3].Speed) || (-35 > Motor[3].Speed))
3722  {
3723  if (FORWARD == Motor[3].Direction)
3724  {
3725  (Motor[3].IrqTacho)++;
3726  }
3727  else
3728  {
3729  (Motor[3].IrqTacho)--;
3730  }
3731  if (Motor[3].DirChgPtr < SamplesPerSpeed[3][SAMPLES_ABOVE_SPEED_75])
3732  {
3733  Motor[3].DirChgPtr++;
3734  }
3735  }
3736  else
3737  {
3738  if (IntDState)
3739  {
3740  if(DirDState)
3741  {
3742  if (FORWARD == Motor[3].Direction)
3743  {
3744  if (Motor[3].DirChgPtr < SamplesPerSpeed[3][SAMPLES_ABOVE_SPEED_75])
3745  {
3746  Motor[3].DirChgPtr++;
3747  }
3748  }
3749  else
3750  {
3751  Motor[3].DirChgPtr = 0;
3752  }
3753  (Motor[3].IrqTacho)++;
3754  Motor[3].Direction = FORWARD;
3755  }
3756  else
3757  {
3758  if (BACKWARD == Motor[3].Direction)
3759  {
3760  if (Motor[3].DirChgPtr < SamplesPerSpeed[3][SAMPLES_ABOVE_SPEED_75])
3761  {
3762  Motor[3].DirChgPtr++;
3763  }
3764  }
3765  else
3766  {
3767  Motor[3].DirChgPtr = 0;
3768  }
3769  (Motor[3].IrqTacho)--;
3770  Motor[3].Direction = BACKWARD;
3771  }
3772  }
3773  else
3774  {
3775  if(DirDState)
3776  {
3777  if (BACKWARD == Motor[3].Direction)
3778  {
3779  if (Motor[3].DirChgPtr < SamplesPerSpeed[3][SAMPLES_ABOVE_SPEED_75])
3780  {
3781  Motor[3].DirChgPtr++;
3782  }
3783  }
3784  else
3785  {
3786  Motor[3].DirChgPtr = 0;
3787  }
3788  (Motor[3].IrqTacho)--;
3789  Motor[3].Direction = BACKWARD;
3790  }
3791  else
3792  {
3793  if (FORWARD == Motor[3].Direction)
3794  {
3795  if (Motor[3].DirChgPtr < SamplesPerSpeed[3][SAMPLES_ABOVE_SPEED_75])
3796  {
3797  Motor[3].DirChgPtr++;
3798  }
3799  }
3800  else
3801  {
3802  Motor[3].DirChgPtr = 0;
3803  }
3804  (Motor[3].IrqTacho)++;
3805  Motor[3].Direction = FORWARD;
3806  }
3807  }
3808  }
3809  return IRQ_HANDLED;
3810 }
3811 
3812 
3978 UBYTE dCalculateSpeed(UBYTE No, SBYTE *pSpeed)
3979 {
3980 
3981  ULONG Tmp1, Tmp2;
3982  ULONG Diff;
3983  UBYTE Ptr, Status;
3984  SWORD Speed;
3985 
3986  Status = FALSE;
3987 
3988  Ptr = TachoSamples[No].ArrayPtr;
3989 
3990  if (Motor[No].DirChgPtr >= 1)
3991  {
3992  Diff = (((TachoSamples[No].TachoArray[Ptr])-(TachoSamples[No].TachoArray[((Ptr - 1) & (NO_OF_TACHO_SAMPLES - 1))])) & 0x00FFFFFF);
3993  if (Diff)
3994  {
3995  SETAvgTachoCount(No, (ULONG)((CountsPerPulse[No]/Diff) & 0x00FFFFFF));
3996  }
3997  else
3998  {
3999  SETAvgTachoCount(No, (ULONG)1);
4000  }
4001  }
4002 
4003  Speed = *pSpeed; // Maintain old speed if not changed
4004  Tmp1 = TachoSamples[No].TachoArray[Ptr];
4005  Tmp2 = TachoSamples[No].TachoArray[((Ptr - AVG_TACHO_COUNTS[No]) & (NO_OF_TACHO_SAMPLES - 1))];
4006 
4007  if ((Ptr != TachoSamples[No].ArrayPtrOld) && (Motor[No].DirChgPtr >= AVG_TACHO_COUNTS[No]))
4008  {
4009 
4010  Status = TRUE;
4011  TimeOutSpeed0[No] = Tmp1;
4012  TachoSamples[No].ArrayPtrOld = Ptr;
4013 
4014  Diff = ((Tmp1-Tmp2) & 0x00FFFFFF);
4015  if (Diff)
4016  {
4017  Speed = (SWORD)((ULONG)(AVG_COUNTS[No])/(ULONG)Diff);
4018  }
4019  else
4020  {
4021  // Safety check
4022  Speed = 1;
4023  }
4024 
4025  if (Speed > 100)
4026  {
4027  Speed = 100;
4028  }
4029 
4030  if (BACKWARD == Motor[No].Direction)
4031  {
4032  Speed = 0 - Speed;
4033  }
4034  }
4035  else
4036  {
4037  // No new Values check for speed 0
4038  if ((CountsPerPulse[No]) < ((FREERunning24bittimer - TimeOutSpeed0[No]) & 0x00FFFFFF))
4039  {
4040  TimeOutSpeed0[No] = FREERunning24bittimer;
4041  Speed = 0;
4042  Motor[No].DirChgPtr = 0;
4043  Status = TRUE;
4044  }
4045  }
4046 
4047  *pSpeed = (SBYTE)Speed;
4048 
4049  return(Status);
4050 }
4051 
4052 
4064 static ssize_t Device2Write(struct file *File,const char *Buffer,size_t Count,loff_t *Data)
4065 {
4066  int Lng = 0;
4067  SBYTE Buf[20];
4068 
4069  copy_from_user(Buf, Buffer, Count);
4070 
4071  ReadyStatus |= Buf[0]; // Set Ready flag
4072  TestStatus |= Buf[0]; // Set Test flag
4073 
4074  return (Lng);
4075 }
4076 
4077 
4088 static ssize_t Device2Read(struct file *File,char *Buffer,size_t Count,loff_t *Offset)
4089 {
4090  int Lng = 0;
4091  return (Lng);
4092 }
4093 
4094 #define SHM_LENGTH (sizeof(MotorData))
4095 #define NPAGES ((SHM_LENGTH + PAGE_SIZE - 1) / PAGE_SIZE)
4096 static void *kmalloc_ptr;
4097 
4098 
4099 static int Device2Mmap(struct file *filp, struct vm_area_struct *vma)
4100 {
4101  int ret;
4102 
4103  ret = remap_pfn_range(vma,vma->vm_start,virt_to_phys((void*)((unsigned long)pMotor)) >> PAGE_SHIFT,vma->vm_end-vma->vm_start,PAGE_SHARED);
4104 
4105  if (ret != 0)
4106  {
4107  ret = -EAGAIN;
4108  }
4109 
4110  return (ret);
4111 }
4112 
4113 
4114 static const struct file_operations Device2Entries =
4115 {
4116  .owner = THIS_MODULE,
4117  .read = Device2Read,
4118  .write = Device2Write,
4119  .mmap = Device2Mmap,
4120 };
4121 
4122 
4123 static struct miscdevice Device2 =
4124 {
4125  MISC_DYNAMIC_MINOR,
4126  DEVICE2_NAME,
4127  &Device2Entries
4128 };
4129 
4130 
4131 static int Device2Init(void)
4132 {
4133  int Result = -1;
4134  int i;
4135  MOTORDATA *pTmp;
4136 
4137  Result = misc_register(&Device2);
4138  if (Result)
4139  {
4140  printk(" %s device register failed\n",DEVICE2_NAME);
4141  }
4142  else
4143  { // allocate kernel shared memory for tacho counts and speed
4144 
4145  if ((kmalloc_ptr = kmalloc((NPAGES + 2) * PAGE_SIZE, GFP_KERNEL)) != NULL)
4146  {
4147  pTmp = (MOTORDATA*)((((unsigned long)kmalloc_ptr) + PAGE_SIZE - 1) & PAGE_MASK);
4148  for (i = 0; i < NPAGES * PAGE_SIZE; i += PAGE_SIZE)
4149  {
4150  SetPageReserved(virt_to_page(((unsigned long)pTmp) + i));
4151  }
4152  pMotor = pTmp;
4153  memset(pMotor,0,sizeof(MotorData));
4154 
4155 #ifdef DEBUG
4156  printk(" %s device register succes\n",DEVICE2_NAME);
4157 #endif
4158  }
4159  else
4160  {
4161  printk(" %s kmalloc failed !!\n",DEVICE2_NAME);
4162  }
4163  }
4164  return (Result);
4165 }
4166 
4167 
4168 static void Device2Exit(void)
4169 {
4170  MOTORDATA *pTmp;
4171  int i;
4172 
4173  pTmp = pMotor;
4174  pMotor = MotorData;
4175  // free shared memory
4176  for (i = 0; i < NPAGES * PAGE_SIZE; i+= PAGE_SIZE)
4177  {
4178  ClearPageReserved(virt_to_page(((unsigned long)pTmp) + i));
4179 #ifdef DEBUG
4180  printk(" %s memory page %d unmapped\n",DEVICE1_NAME,i);
4181 #endif
4182  }
4183  kfree(kmalloc_ptr);
4184 
4185  misc_deregister(&Device2);
4186 #ifdef DEBUG
4187  printk(" %s device unregistered\n",DEVICE2_NAME);
4188 #endif
4189 }
4190 
4191 
4192 #ifndef PCASM
4193 module_param (HwId, charp, 0);
4194 #endif
4195 
4196 static int ModuleInit(void)
4197 {
4198  Hw = HWID;
4199 
4200  if (Hw < PLATFORM_START)
4201  {
4202  Hw = PLATFORM_START;
4203  }
4204  if (Hw > PLATFORM_END)
4205  {
4206  Hw = PLATFORM_END;
4207  }
4208 
4209 #ifdef DEBUG
4210  printk("%s init started\n",MODULE_NAME);
4211 #endif
4212 
4213  if (request_mem_region(DA8XX_GPIO_BASE,0xD8,MODULE_NAME) >= 0)
4214  {
4215  GpioBase = (void*)ioremap(DA8XX_GPIO_BASE,0xD8);
4216  if (GpioBase != NULL)
4217  {
4218 #ifdef DEBUG
4219  printk("%s gpio address mapped\n",MODULE_NAME);
4220 #endif
4221 
4222  switch(Hw)
4223  {
4224  case EP2:
4225  {
4226  /* This is to comply with changing of inverters on the tacho direction pins */
4227  /* only Final hardware does not need to be inverted */
4228  HwInvBits = 0xFFFFFFFF;
4229 
4230  /* Motor PWM outputs has been switched in EP2 MA<->MB and MC<->MD */
4231  SetDuty[0] = SetDutyMB;
4232  SetDuty[1] = SetDutyMA;
4233  SetDuty[2] = SetDutyMD;
4234  SetDuty[3] = SetDutyMC;
4235  }
4236  break;
4237  case FINALB:
4238  {
4239  /* This is to comply with changing of inverters on the tacho direction pins */
4240  /* only Final hardware does not need to be inverted */
4241  HwInvBits = 0xFFFFFFFF;
4242 
4243  /* Motor PWM outputs has been switched in EP2 MA<->MB and MC<->MD */
4244  SetDuty[0] = SetDutyMA;
4245  SetDuty[1] = SetDutyMB;
4246  SetDuty[2] = SetDutyMC;
4247  SetDuty[3] = SetDutyMD;
4248  }
4249  break;
4250  case FINAL:
4251  {
4252  /* This is to comply with changing of inverters on the tacho direction pins */
4253  /* only Final hardware does not need to be inverted */
4254  HwInvBits = 0;
4255 
4256  /* Motor PWM outputs has been switched in EP2 MA<->MB and MC<->MD */
4257  SetDuty[0] = SetDutyMA;
4258  SetDuty[1] = SetDutyMB;
4259  SetDuty[2] = SetDutyMC;
4260  SetDuty[3] = SetDutyMD;
4261  }
4262  break;
4263  }
4264 
4265  InitGpio();
4266 
4267  Device1Init();
4268  Device2Init();
4269 
4270  /* Setup timer irq*/
4271  Device1Time = ktime_set(0,SOFT_TIMER_SETUP);
4272  hrtimer_init(&Device1Timer,CLOCK_MONOTONIC,HRTIMER_MODE_REL);
4273  Device1Timer.function = Device1TimerInterrupt1;
4274  hrtimer_start(&Device1Timer,Device1Time,HRTIMER_MODE_REL);
4275  }
4276  }
4277  return (0);
4278 }
4279 
4280 
4281 static void ModuleExit(void)
4282 {
4283 #ifdef DEBUG
4284  printk("%s exit started\n",MODULE_NAME);
4285 #endif
4286  STOPPwm;
4287  free_irq(gpio_to_irq(IRQA_PINNO), NULL);
4288  free_irq(gpio_to_irq(IRQB_PINNO), NULL);
4289  free_irq(gpio_to_irq(IRQC_PINNO), NULL);
4290  free_irq(gpio_to_irq(IRQD_PINNO), NULL);
4291 
4292  Device1Exit();
4293  Device2Exit();
4294 
4295  iounmap(GpioBase);
4296 
4297 }
4298 
4299 
4300 
4301 
LIMITED_UNREG_STEPUP
Definition: d_pwm.c:190
MOTOR::Owner
UBYTE Owner
Definition: d_pwm.c:247
REGLock
#define REGLock
Definition: am1808.h:235
dRegulateSpeed
void dRegulateSpeed(UBYTE No)
dRegulateSpeed
Definition: d_pwm.c:1270
SETMotorType
#define SETMotorType(Port, NewType)
Definition: d_pwm.c:577
NOTUSED4
Definition: d_pwm.c:136
FloatSyncedMotors
void FloatSyncedMotors(void)
Definition: d_pwm.c:1440
GP2_9
Definition: am1808.h:35
MOTOR::TargetPower
SLONG TargetPower
Definition: d_pwm.c:235
TRUE
Definition: lms2012.h:558
MOTORDATA::TachoSensor
SLONG TachoSensor
Definition: lms2012.h:1348
COUNTS_PER_PULSE_LM
#define COUNTS_PER_PULSE_LM
Definition: d_pwm.c:56
snprintf
#define snprintf
Definition: c_input.c:141
MOTOR::TachoSensor
SLONG TachoSensor
Definition: d_pwm.c:220
CMP1
Definition: d_pwm.c:143
COAST
Definition: d_pwm.c:180
PWM
Definition: d_pwm.c:325
SBYTE
signed char SBYTE
Basic Type used to symbolise 8 bit signed values.
Definition: lmstypes.h:33
SYNCED_SLAVE
Definition: d_pwm.c:202
GP5_9
Definition: am1808.h:38
MOTOR::OldSpeedErr
SLONG OldSpeedErr
Definition: d_pwm.c:233
SETUPPwmModules
#define SETUPPwmModules
Definition: d_pwm.c:670
TIMEPOWER::Time2
DATA32 Time2
Definition: lms2012.h:1368
GP2_1
Definition: am1808.h:35
SetGpio
void SetGpio(int Pin)
Definition: d_pwm.c:3273
MuxRegMap
MRM MuxRegMap[]
Definition: am1808.h:59
READIntC
#define READIntC
Definition: d_pwm.c:705
GP0_4
Definition: am1808.h:33
CMP4
Definition: d_pwm.c:146
CTRPHS
Definition: d_pwm.c:156
GetTachoInt
UWORD GetTachoInt(UBYTE Port)
Definition: d_pwm.c:800
MOTOR::TargetSpeed
SBYTE TargetSpeed
Definition: d_pwm.c:238
RAMP_FACTOR
#define RAMP_FACTOR
Definition: d_pwm.c:50
MODULE_DESCRIPTION
MODULE_DESCRIPTION(MODULE_NAME)
NOTUSED5
Definition: d_pwm.c:137
REGUnlock
#define REGUnlock
Definition: am1808.h:230
STOP_MOTOR
Definition: d_pwm.c:206
TIMEPOWER::Power
DATA8 Power
Definition: lms2012.h:1366
READDirB
#define READDirB
Definition: d_pwm.c:477
LIMITED_REG_STEPDOWN
Definition: d_pwm.c:189
CMPA
#define CMPA
Definition: d_pwm.c:97
CheckLessThanSpecial
UBYTE CheckLessThanSpecial(SLONG Param1, SLONG Param2, SLONG Inv)
Definition: d_pwm.c:725
StepSpeedCheckTachoCntUp
UBYTE StepSpeedCheckTachoCntUp(UBYTE No)
StepSpeedCheckTachoCntUp.
Definition: d_pwm.c:1000
INPIN
Definition: am1808.h:222
MAX_SPEED
#define MAX_SPEED
Definition: d_pwm.c:48
INV
#define INV
Definition: d_pwm.c:172
module_exit
module_exit(ModuleExit)
EP2
#define EP2
Schematics revision D.
Definition: lms2012.h:99
LIMITED_DIFF_TURN_SYNC
Definition: d_pwm.c:201
MOTOR::LockRampDown
UBYTE LockRampDown
Definition: d_pwm.c:241
MODULE_LICENSE
MODULE_LICENSE("GPL")
EP2_OutputPortPin
INPIN EP2_OutputPortPin[][OUTPUT_PORT_PINS]
Definition: d_pwm.c:343
SetDirFwd
void SetDirFwd(UBYTE Port)
Definition: d_pwm.c:775
MOTOR::RampDownFactor
SLONG RampDownFactor
Definition: d_pwm.c:229
MOTOR::TachoCntConst
SLONG TachoCntConst
Definition: d_pwm.c:225
TACHOSAMPLES::ArrayPtrOld
UBYTE ArrayPtrOld
Definition: d_pwm.c:258
ECEINT
Definition: d_pwm.c:163
CheckSpeedPowerLimits
void CheckSpeedPowerLimits(SBYTE *pCheckVal)
Definition: d_pwm.c:712
TYPE_NONE
Port empty or not available.
Definition: lms2012.h:585
FINAL
#define FINAL
Final prototype.
Definition: lms2012.h:101
FORWARD
Definition: d_pwm.c:177
OutputPortPins
OutputPortPins
Definition: d_analog.c:342
NO_OF_OUTPUT_PORTS
#define NO_OF_OUTPUT_PORTS
Definition: d_pwm.c:45
ClearPIDParameters
void ClearPIDParameters(UBYTE No)
Definition: d_pwm.c:926
TIMESYNC::Brake
DATA8 Brake
Definition: lms2012.h:1412
StepPowerCheckTachoCntUp
UBYTE StepPowerCheckTachoCntUp(UBYTE No)
StepPowerCheckTachoCntUp.
Definition: d_pwm.c:1142
TIMESYNC::Turn
DATA16 Turn
Definition: lms2012.h:1410
READIntD
#define READIntD
Definition: d_pwm.c:706
StepPowerCheckTachoCntConst
UBYTE StepPowerCheckTachoCntConst(UBYTE No, SLONG AdjustTachoValue)
StepPowerCheckTachoCntConst.
Definition: d_pwm.c:1186
SetDutyMC
void SetDutyMC(ULONG Duty)
Definition: d_pwm.c:916
LIMITED_UNREG_TIMECONST
Definition: d_pwm.c:197
MODULE_NAME
#define MODULE_NAME
Definition: d_pwm.c:33
MOTOR::TachoCntDown
SLONG TachoCntDown
Definition: d_pwm.c:226
SetRegulationPower
void SetRegulationPower(UBYTE Port, SLONG Power)
Definition: d_pwm.c:857
LIMITED_UNREG_TIMEUP
Definition: d_pwm.c:196
FALSE
Definition: lms2012.h:557
NOTUSED6
Definition: d_pwm.c:138
GP6_9
Definition: am1808.h:39
BRAKED
Definition: d_pwm.c:205
NOTUSED8
Definition: d_pwm.c:140
TIMESYNC::Time
DATA32 Time
Definition: lms2012.h:1411
CMP0
Definition: d_pwm.c:142
GP2_3
Definition: am1808.h:35
MOTOR::TurnRatio
SWORD TurnRatio
Definition: d_pwm.c:250
LIMITED_REG_STEPCONST
Definition: d_pwm.c:188
MOTOR::Pol
SBYTE Pol
Definition: d_pwm.c:242
StopAndBrakeMotor
void StopAndBrakeMotor(UBYTE MotorNo)
Definition: d_pwm.c:1412
TIMESPEED::Time1
DATA32 Time1
Definition: lms2012.h:1389
CAP1
Definition: d_pwm.c:157
IncSpeed
void IncSpeed(SBYTE Dir, SBYTE *pSpeed)
Definition: d_pwm.c:737
EPWM1B
Definition: am1808.h:46
MRM::MuxReg
u16 MuxReg
Definition: am1808.h:53
UNLIMITED_UNREG
Definition: d_pwm.c:185
LIMITED_REG_TIMEDOWN
Definition: d_pwm.c:195
ECCTL2
Definition: d_pwm.c:162
MOTOR::TimeInc
SLONG TimeInc
Definition: d_pwm.c:222
module_param
module_param(HwId, charp, 0)
STEPSYNC::Speed
DATA8 Speed
Definition: lms2012.h:1399
STEPPOWER::Nos
DATA8 Nos
Definition: lms2012.h:1354
GP5_8
Definition: am1808.h:38
NOTUSED7
Definition: d_pwm.c:139
MAX_SYNC_MOTORS
#define MAX_SYNC_MOTORS
Definition: d_pwm.c:51
NPAGES
#define NPAGES
Definition: d_pwm.c:4095
GetSyncDurationCnt
void GetSyncDurationCnt(SLONG *pCount0, SLONG *pCount1)
Definition: d_pwm.c:2148
TIMEPOWER::Time3
DATA32 Time3
Definition: lms2012.h:1369
APWM0
Definition: am1808.h:46
TestAndFloatSyncedMotors
void TestAndFloatSyncedMotors(UBYTE MotorBitField, UBYTE SyncCmd)
Definition: d_pwm.c:1464
MOTOR::Direction
UBYTE Direction
Definition: d_pwm.c:243
WDTCR
Definition: d_pwm.c:128
SOFT_TIMER_SETUP
#define SOFT_TIMER_SETUP
Definition: d_pwm.c:39
STEPPOWER::Power
DATA8 Power
Definition: lms2012.h:1355
CMP5
Definition: d_pwm.c:147
CheckforEndOfSync
void CheckforEndOfSync(void)
Definition: d_pwm.c:2163
GP6_8
Definition: am1808.h:39
CAP4
Definition: d_pwm.c:160
STEPSPEED::Speed
DATA8 Speed
Definition: lms2012.h:1377
Timer
#define Timer
TIMESYNC::Speed
DATA8 Speed
Definition: lms2012.h:1409
GP5_10
Definition: am1808.h:38
STEPPOWER::Cmd
DATA8 Cmd
Definition: lms2012.h:1353
STOPPwm
#define STOPPwm
Definition: d_pwm.c:655
IDLE
Definition: d_pwm.c:207
GP5_11
Definition: am1808.h:38
SAMPLES_ABOVE_SPEED_75
Definition: d_pwm.c:64
ECCLR
Definition: d_pwm.c:165
MODULE_AUTHOR
MODULE_AUTHOR("The LEGO Group")
FINAL_OutputPortPin
INPIN FINAL_OutputPortPin[][OUTPUT_PORT_PINS]
Definition: d_pwm.c:426
MOTOR::TachoCnt
SLONG TachoCnt
Definition: d_pwm.c:219
SetDutyMB
void SetDutyMB(ULONG Duty)
Definition: d_pwm.c:912
MOTOR::IVal
SLONG IVal
Definition: d_pwm.c:231
LIMITED_UNREG_TIMEDOWN
Definition: d_pwm.c:198
ECFLG
Definition: d_pwm.c:164
ULONG
unsigned int ULONG
Basic Type used to symbolise 32 bit unsigned values.
Definition: lmstypes.h:31
TIMEPOWER::Brake
DATA8 Brake
Definition: lms2012.h:1370
APWM1
Definition: am1808.h:46
GP2_8
Definition: am1808.h:35
TGCR
Definition: d_pwm.c:127
MOTOR::TargetBrake
SBYTE TargetBrake
Definition: d_pwm.c:239
GP3_6
Definition: am1808.h:36
GetTachoDir
UWORD GetTachoDir(UBYTE Port)
Definition: d_pwm.c:794
MOTOR
Definition: d_pwm.c:216
REL34
Definition: d_pwm.c:132
STEPSYNC::Turn
DATA16 Turn
Definition: lms2012.h:1400
READIntB
#define READIntB
Definition: d_pwm.c:704
HwInvBits
int HwInvBits
Definition: d_pwm.c:31
TIMEPOWER::Nos
DATA8 Nos
Definition: lms2012.h:1365
BACKWARD
Definition: d_pwm.c:178
NO_OF_SAMPLE_STEPS
Definition: d_pwm.c:65
DIR
Definition: d_pwm.c:329
TACHOSAMPLES::ArrayPtr
UBYTE ArrayPtr
Definition: d_pwm.c:257
STEPSYNC::Brake
DATA8 Brake
Definition: lms2012.h:1402
INPIN::Pin
int Pin
Definition: am1808.h:224
IRQA_PINNO
#define IRQA_PINNO
Definition: d_pwm.c:336
TIM34
Definition: d_pwm.c:123
SAMPLES_SPEED_25_50
Definition: d_pwm.c:62
MOTOR::Mutex
UBYTE Mutex
Definition: d_pwm.c:248
MOTORDATA::TachoCounts
SLONG TachoCounts
Definition: lms2012.h:1346
STEPPOWER::Step2
DATA32 Step2
Definition: lms2012.h:1357
SetDutyMA
void SetDutyMA(ULONG Duty)
Definition: d_pwm.c:908
pOutputPortPin
INPIN * pOutputPortPin[]
Definition: d_pwm.c:467
MOTORDATA::Speed
SBYTE Speed
Definition: lms2012.h:1347
STEPPOWER::Step3
DATA32 Step3
Definition: lms2012.h:1358
MOTOR::State
UBYTE State
Definition: d_pwm.c:245
GP3_10
Definition: am1808.h:36
SetGpioRisingIrq
void SetGpioRisingIrq(UBYTE PinNo, irqreturn_t(*IntFuncPtr)(int, void *))
Definition: d_pwm.c:3342
FINALB
#define FINALB
Schematics revision B and C.
Definition: lms2012.h:100
MOTOR::Type
UBYTE Type
Definition: d_pwm.c:244
MOTOR::OldTachoCnt
SLONG OldTachoCnt
Definition: d_pwm.c:223
CAP3
Definition: d_pwm.c:159
TIMESPEED::Brake
DATA8 Brake
Definition: lms2012.h:1392
GP4_12
Definition: am1808.h:37
StepPowerStopMotor
void StepPowerStopMotor(UBYTE No, SLONG AdjustTachoValue)
StepPowerStopMotor.
Definition: d_pwm.c:951
GetPeriphealBasePtr
void GetPeriphealBasePtr(ULONG Address, ULONG Size, ULONG **Ptr)
GetPeriphealBasePtr.
Definition: d_pwm.c:3140
TYPE_TACHO
Device is a tacho motor.
Definition: lms2012.h:571
IRQD_PINNO
#define IRQD_PINNO
Definition: d_pwm.c:339
ECAP_REVID
Definition: d_pwm.c:167
STEPSYNC::Cmd
DATA8 Cmd
Definition: lms2012.h:1397
REVID
Definition: d_pwm.c:118
LIMITED_REG_TIMEUP
Definition: d_pwm.c:193
NON_INV
#define NON_INV
Definition: d_pwm.c:171
MOTOR::BrakeAfter
UBYTE BrakeAfter
Definition: d_pwm.c:240
MOTOR::TargetState
UBYTE TargetState
Definition: d_pwm.c:246
STEPSPEED::Nos
DATA8 Nos
Definition: lms2012.h:1376
UBYTE
unsigned char UBYTE
Basic Type used to symbolise 8 bit unsigned values.
Definition: lmstypes.h:29
GP3_15
Definition: am1808.h:36
STEPSPEED::Brake
DATA8 Brake
Definition: lms2012.h:1381
TIMESYNC::Cmd
DATA8 Cmd
Definition: lms2012.h:1407
SetPower
void SetPower(UBYTE Port, SLONG Power)
Definition: d_pwm.c:806
TCR
Definition: d_pwm.c:126
INTCTLSTAT
Definition: d_pwm.c:141
MOTOR::Speed
SBYTE Speed
Definition: d_pwm.c:237
NOTUSED3
Definition: d_pwm.c:135
UNUSED_SYNC_MOTOR
Definition: d_pwm.c:212
SetCoast
void SetCoast(UBYTE Port)
Definition: d_pwm.c:787
module_init
module_init(ModuleInit)
CAP34
Definition: d_pwm.c:134
OutputRead
#define OutputRead(port, pin)
Definition: d_pwm.c:631
TBCLKSYNC
Definition: d_pwm.c:111
TACHOSAMPLES::TachoArray
ULONG TachoArray[NO_OF_TACHO_SAMPLES]
Definition: d_pwm.c:256
EMUMGT
Definition: d_pwm.c:119
MOTOR::IrqTacho
SLONG IrqTacho
Definition: d_pwm.c:218
BRAKE
Definition: d_pwm.c:179
GP5_3
Definition: am1808.h:38
LIMITED_STEP_SYNC
Definition: d_pwm.c:199
PRD12
Definition: d_pwm.c:124
TIMEPOWER::Time1
DATA32 Time1
Definition: lms2012.h:1367
UWORD
unsigned short UWORD
Basic Type used to symbolise 16 bit unsigned values.
Definition: lmstypes.h:30
TIM12
Definition: d_pwm.c:122
MODULE_SUPPORTED_DEVICE
MODULE_SUPPORTED_DEVICE(DEVICE1_NAME)
Buffer
char Buffer[1024]
Definition: c_wifi.c:102
GP6_0
Definition: am1808.h:39
OutputHigh
#define OutputHigh(port, pin)
Definition: d_pwm.c:633
GetCompareCounts
void GetCompareCounts(UBYTE No, SLONG *Counts, SLONG *CompareCounts)
Definition: d_pwm.c:1381
LIMITED_UNREG_STEPDOWN
Definition: d_pwm.c:192
CAP2
Definition: d_pwm.c:158
OUTPUTS
#define OUTPUTS
Number of output ports in the system.
Definition: lms2012.h:191
TIMESPEED::Cmd
DATA8 Cmd
Definition: lms2012.h:1386
STEPSYNC::Nos
DATA8 Nos
Definition: lms2012.h:1398
IRQB_PINNO
#define IRQB_PINNO
Definition: d_pwm.c:337
STEPPOWER::Step1
DATA32 Step1
Definition: lms2012.h:1356
SPEED_PWMCNT_REL
#define SPEED_PWMCNT_REL
Definition: d_pwm.c:49
RampDownToBrake
UBYTE RampDownToBrake(UBYTE No, SLONG CurrentCnt, SLONG TargetCnt, SLONG Dir)
Definition: d_pwm.c:1340
DEVICE2_NAME
#define DEVICE2_NAME
Definition: d_pwm.c:35
OUTPUT_PORT_PINS
Definition: d_pwm.c:332
MOTOR::RampUpFactor
SLONG RampUpFactor
Definition: d_pwm.c:227
ECCTL1
Definition: d_pwm.c:161
SWORD
signed short SWORD
Basic Type used to symbolise 16 bit signed values.
Definition: lmstypes.h:34
NOTUSED1
Definition: d_pwm.c:129
NOTUSED2
Definition: d_pwm.c:130
READDirA
#define READDirA
Definition: d_pwm.c:476
GP3_14
Definition: am1808.h:36
CAP12
Definition: d_pwm.c:133
TIMESPEED::Time2
DATA32 Time2
Definition: lms2012.h:1390
CMP2
Definition: d_pwm.c:144
TIMEPOWER::Cmd
DATA8 Cmd
Definition: lms2012.h:1364
MOTOR::DVal
SLONG DVal
Definition: d_pwm.c:232
STEPSPEED::Step2
DATA32 Step2
Definition: lms2012.h:1379
TYPE_MINITACHO
Device is a mini tacho motor.
Definition: lms2012.h:572
GP2_0
Definition: am1808.h:35
RAMP_DOWN_SYNC
Definition: d_pwm.c:203
PRD34
Definition: d_pwm.c:125
dCalculateSpeed
UBYTE dCalculateSpeed(UBYTE No, SBYTE *pSpeed)
dCalculateSpeed
Definition: d_pwm.c:3978
SetDirRwd
void SetDirRwd(UBYTE Port)
Definition: d_pwm.c:769
STEPSPEED::Step1
DATA32 Step1
Definition: lms2012.h:1378
TIMESPEED::Speed
DATA8 Speed
Definition: lms2012.h:1388
MOTOR::TachoCntUp
SLONG TachoCntUp
Definition: d_pwm.c:224
SETAvgTachoCount
#define SETAvgTachoCount(Port, Speed)
Definition: d_pwm.c:591
STEPPOWER::Brake
DATA8 Brake
Definition: lms2012.h:1359
FINALB_OutputPortPin
INPIN FINALB_OutputPortPin[][OUTPUT_PORT_PINS]
Definition: d_pwm.c:385
UNLIMITED_REG
Definition: d_pwm.c:186
StepPowerCheckTachoCntDown
void StepPowerCheckTachoCntDown(UBYTE No, SLONG AdjustTachoValue)
StepPowerCheckTachoCntDown.
Definition: d_pwm.c:1231
ECFRC
Definition: d_pwm.c:166
LIMITED_UNREG_STEPCONST
Definition: d_pwm.c:191
GP0_3
Definition: am1808.h:33
CMP7
Definition: d_pwm.c:149
InitGpio
void InitGpio(void)
Definition: d_pwm.c:3312
CLEARTachoArray
#define CLEARTachoArray(No)
Definition: d_pwm.c:622
GP5_13
Definition: am1808.h:38
READDirD
#define READDirD
Definition: d_pwm.c:479
u32
uint32_t u32
Definition: common.h:158
OutputLow
#define OutputLow(port, pin)
Definition: d_pwm.c:638
GPINTGPEN
Definition: d_pwm.c:120
SAMPLES_SPEED_50_75
Definition: d_pwm.c:63
FAULT
Definition: d_pwm.c:331
CMP3
Definition: d_pwm.c:145
SOFT_TIMER_MS
#define SOFT_TIMER_MS
Definition: d_pwm.c:38
StopAndFloatMotor
void StopAndFloatMotor(UBYTE MotorNo)
Definition: d_pwm.c:1426
GPDATGPDIR
Definition: d_pwm.c:121
MOTOR::Power
SLONG Power
Definition: d_pwm.c:234
TIMESPEED::Nos
DATA8 Nos
Definition: lms2012.h:1387
FREERunning24bittimer
#define FREERunning24bittimer
Definition: d_pwm.c:620
TSCTR
Definition: d_pwm.c:155
Ptr
void * Ptr
Definition: tistdtypes.h:56
DIR0
Definition: d_pwm.c:326
REL12
Definition: d_pwm.c:131
EPWM1A
Definition: am1808.h:46
STEPSPEED::Cmd
DATA8 Cmd
Definition: lms2012.h:1375
TIMESYNC::Nos
DATA8 Nos
Definition: lms2012.h:1408
HOLD
Definition: d_pwm.c:204
PLATFORM_END
#define PLATFORM_END
Newest supported hardware (newer versions will use this)
Definition: lms2012.h:105
SetBrake
void SetBrake(UBYTE Port)
Definition: d_pwm.c:781
SAMPLES_BELOW_SPEED_25
Definition: d_pwm.c:61
LIMITED_TURN_SYNC
Definition: d_pwm.c:200
StepSpeedCheckTachoCntConst
UBYTE StepSpeedCheckTachoCntConst(UBYTE No, SLONG AdjustTachoValue)
StepPowerCheckTachoCntConst.
Definition: d_pwm.c:1065
BrakeMotor
void BrakeMotor(UBYTE No, SLONG TachoCnt)
Definition: d_pwm.c:1318
StepSpeedCheckTachoCntDown
void StepSpeedCheckTachoCntDown(UBYTE No, SLONG AdjustTachoValue)
StepSpeedCheckTachoCntDown.
Definition: d_pwm.c:1109
DEVICE1_NAME
#define DEVICE1_NAME
Definition: d_pwm.c:34
OutputFloat
#define OutputFloat(port, pin)
Definition: d_pwm.c:627
CMP6
Definition: d_pwm.c:148
STEPSYNC::Step
DATA32 Step
Definition: lms2012.h:1401
NO_OF_TACHO_SAMPLES
#define NO_OF_TACHO_SAMPLES
Definition: d_pwm.c:44
MAX_PWM_CNT
#define MAX_PWM_CNT
Definition: d_pwm.c:47
Hw
int Hw
Definition: d_pwm.c:30
CalcRampDownFactor
void CalcRampDownFactor(UBYTE No, ULONG CurrentPower, ULONG Counts)
Definition: d_pwm.c:1371
MOTOR::PVal
SLONG PVal
Definition: d_pwm.c:230
IRQC_PINNO
#define IRQC_PINNO
Definition: d_pwm.c:338
MOTOR::RampUpOffset
SLONG RampUpOffset
Definition: d_pwm.c:228
SLEEP
Definition: d_pwm.c:330
SetDutyMD
void SetDutyMD(ULONG Duty)
Definition: d_pwm.c:920
LIMITED_REG_STEPUP
Definition: d_pwm.c:187
CFGCHIP1
Definition: d_pwm.c:106
DecSpeed
void DecSpeed(SBYTE Dir, SBYTE *pSpeed)
Definition: d_pwm.c:753
MOTOR::DirChgPtr
UBYTE DirChgPtr
Definition: d_pwm.c:249
INT
Definition: d_pwm.c:328
TIMESPEED::Time3
DATA32 Time3
Definition: lms2012.h:1391
DIR1
Definition: d_pwm.c:327
MOTOR::Dir
SLONG Dir
Definition: d_pwm.c:236
LIMITED_REG_TIMECONST
Definition: d_pwm.c:194
SLONG
signed int SLONG
Basic Type used to symbolise 32 bit signed values.
Definition: lmstypes.h:35
READIntA
#define READIntA
Definition: d_pwm.c:703
MOTOR::TimeCnt
SLONG TimeCnt
Definition: d_pwm.c:221
READDirC
#define READDirC
Definition: d_pwm.c:478
PLATFORM_START
#define PLATFORM_START
Oldest supported hardware (older versions will use this)
Definition: lms2012.h:104
STEPSPEED::Step3
DATA32 Step3
Definition: lms2012.h:1380
NULL
#define NULL
Definition: pru_can_emulation_api.h:37
CMPB
#define CMPB
Definition: d_pwm.c:98