Gordons Projects

--> Projects Top-Level GIT

Initial migration to GIT
[piTrak] / motors.c
1
2 // piTrak
3 //      motors.c
4
5 #include <wiringPi.h>
6
7 #include "motors.h"
8
9 static volatile int rotationCountVal = 0 ;
10
11 /*
12  * Motor control defs.
13  *********************************************************************************
14  */
15
16 #define COUNTER_PIN     0
17
18 #define DRIVE_MOTOR_A   5
19 #define DRIVE_MOTOR_B   6
20
21 #define TURN_MOTOR_A    2
22 #define TURN_MOTOR_B    3
23
24
25 /*
26  * stopMotors:
27  *      All stop. Brake then leave them in freewheel mode
28  *********************************************************************************
29  */
30
31 void stopMotors (void)
32 {
33   digitalWrite (DRIVE_MOTOR_A, 1) ;
34   digitalWrite (DRIVE_MOTOR_B, 1) ;
35   digitalWrite (TURN_MOTOR_A,  1) ;
36   digitalWrite (TURN_MOTOR_B,  1) ;
37   delay (500) ;
38   digitalWrite (DRIVE_MOTOR_A, 0) ;
39   digitalWrite (DRIVE_MOTOR_B, 0) ;
40   digitalWrite (TURN_MOTOR_A,  0) ;
41   digitalWrite (TURN_MOTOR_B,  0) ;
42   delay (500) ;
43   rotationCountVal = 0 ;
44 }
45
46
47 /*
48  * drive:
49  *      Set the motor controls and counter
50  *********************************************************************************
51  */
52
53 void driveForwards (int count)
54 {
55   rotationCountVal = count ;
56   digitalWrite (DRIVE_MOTOR_A, 0) ;
57   digitalWrite (DRIVE_MOTOR_B, 1) ;
58 }
59
60 void driveBackwards (int count)
61 {
62   rotationCountVal = count ;
63   digitalWrite (DRIVE_MOTOR_A, 1) ;
64   digitalWrite (DRIVE_MOTOR_B, 0) ;
65 }
66
67 void turnLeft (int count)
68 {
69   rotationCountVal = count ;
70   digitalWrite (TURN_MOTOR_A, 0) ;
71   digitalWrite (TURN_MOTOR_B, 1) ;
72 }
73
74 void turnRight (int count)
75 {
76   rotationCountVal = count ;
77   digitalWrite (TURN_MOTOR_A, 1) ;
78   digitalWrite (TURN_MOTOR_B, 0) ;
79 }
80
81
82 /*
83  * rotationCounter:
84  *      Count pulses from the rotation sensor and when the counter reaches
85  *      zero, stop all motors.
86  *********************************************************************************
87  */
88
89 PI_THREAD (rotationCounter)
90 {
91   int last ;
92
93   piHiPri (20) ;
94   rotationCountVal = 0 ;
95
96   for (;;)
97   {
98
99     pullUpDnControl (COUNTER_PIN, PUD_UP) ;
100
101 // Wait for a count to be set
102
103     while (rotationCountVal == 0)
104       delay (25) ;
105
106 // Count down:
107
108     while (rotationCountVal != 0)
109     {
110       last = digitalRead (COUNTER_PIN) ;
111
112       while (digitalRead (COUNTER_PIN) == last)
113         delay (10) ;
114
115       --rotationCountVal ;
116     }
117     stopMotors () ;
118
119   }
120 }
121
122
123 /*
124  * setupMotors:
125  *      Do all the stuff to setup the motors and the rotation counter
126  *********************************************************************************
127  */
128
129 void setupMotors (void)
130 {
131   pinMode (DRIVE_MOTOR_A, OUTPUT) ;
132   pinMode (DRIVE_MOTOR_B, OUTPUT) ;
133   pinMode (TURN_MOTOR_A,  OUTPUT) ;
134   pinMode (TURN_MOTOR_B,  OUTPUT) ;
135   stopMotors () ;
136
137   pinMode         (COUNTER_PIN, INPUT) ;
138   delay (100) ;
139   pullUpDnControl (COUNTER_PIN, PUD_UP) ;
140   delay (100) ;
141   pullUpDnControl (COUNTER_PIN, PUD_UP) ;
142
143   piThreadCreate (rotationCounter) ;
144 }