@@ -37,27 +37,28 @@ void ProcessData()
37
37
Invalid ();
38
38
}
39
39
}
40
-
40
+
41
41
bool go = true ;
42
42
int total_turns = 0 ;
43
43
44
- while (go == true )
44
+ while (go == true )
45
45
{
46
-
46
+ // subtract from motor rotations when a rotation is detected
47
47
for (int i = 0 ; i < NUMBER_MOTORS; i++)
48
48
{
49
49
motor_sensor_counter2[i] = motor_sensor_counter1[i];
50
- motor_sensor_counter1 = CheckSwitch (i,ports[i][0 ]);
50
+ motor_sensor_counter1[i] = CheckSwitch (i, ports[i][0 ]);
51
51
52
- if (motor_sensor_counter1 == 1 && motor_sensor_counter2 == 0 ) {
52
+ if (motor_sensor_counter1[i] == 1 && motor_sensor_counter2[i] == 0 )
53
+ {
53
54
motor_commands[i][1 ] = motor_commands[i][1 ] - 1 ;
54
55
}
55
- }
56
-
56
+ }
57
57
// stop motors that have reached 0
58
58
for (int i = 0 ; i < NUMBER_MOTORS; i++)
59
59
{
60
- if (motor_commands[i][1 ] <= 0 ) {
60
+ if (motor_commands[i][1 ] <= 0 )
61
+ {
61
62
my_servo[i].write (90 );
62
63
}
63
64
}
@@ -67,7 +68,8 @@ void ProcessData()
67
68
total_turns += motor_commands[i][1 ];
68
69
}
69
70
// exit loop if there are no more motor rotations remaining
70
- if (total_turns <= 0 ) {
71
+ if (total_turns <= 0 )
72
+ {
71
73
go = false ;
72
74
}
73
75
total_turns = 0 ;
@@ -138,7 +140,7 @@ void PopulateArray()
138
140
// ////////////////////////////////////////////////////////////////////////////
139
141
// ////////////////////////////////////////////////////////////////////////////
140
142
141
- void CheckSwitch (int motor_number, int switchPort)
143
+ int CheckSwitch (int motor_number, int switchPort)
142
144
{
143
145
/* Step 1: Update the integrator based on the input signal. Note that the
144
146
integrator follows the input, decreasing or increasing towards the limits
@@ -158,10 +160,10 @@ void CheckSwitch(int motor_number, int switchPort)
158
160
either 0 or MAXIMUM. */
159
161
160
162
if (integrator[motor_number] == 0 )
161
- reuturn (0 )
163
+ return (0 );
162
164
else if (integrator[motor_number] >= MAXIMUM)
163
165
{
164
- return (1 );
165
- integrator[motor_number] = MAXIMUM; /* defensive code if integrator got corrupted */
166
+ return (1 );
167
+ integrator[motor_number] = MAXIMUM; /* defensive code if integrator got corrupted */
166
168
}
167
169
}
0 commit comments