-
Notifications
You must be signed in to change notification settings - Fork 3
/
motionTask.c
executable file
·151 lines (135 loc) · 4.03 KB
/
motionTask.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
#include "FreeRTOS.h"
#include "motion.h"
#include "motionTask.h"
#include "task.h"
#include "LEDHandler.h"
/// Flag used to control the movement of the thermo sensor relative to the
/// robot's movement.
bool thermoSensorFlag = false;
/// Variable used to control the thermo sensor's movement.
static int thermoLeft = 1;
/// Value used for calculating the thermo sensor's wave values.
static int pulseWidth = 2800;
/**
* Initializes motion module and also calls the method to set up the led module
*/
void motionInit()
{
motion_init();
setupLED();
}
/**
* Move Chico backwards by setting the pulse width of the left wheel to 4800
* and the right wheel to 1100.
* Also display the green led from the LEDHandler class.
*/
void motionForward()
{
thermoSensorFlag = true;
motion_servo_set_pulse_width(MOTION_WHEEL_RIGHT, 1100);
motion_servo_set_pulse_width(MOTION_WHEEL_LEFT, 4800);
motion_servo_set_pulse_width(MOTION_SERVO_CENTER, 2560);
motion_servo_start(MOTION_WHEEL_RIGHT);
motion_servo_start(MOTION_WHEEL_LEFT);
motion_servo_start(MOTION_SERVO_CENTER);
displayGreenLED();
}
/**
* Move Chico backwards by setting the pulse width of the left wheel to 1100
* and the right wheel to 4800.
* Also display the red led from the LEDHandler class.
*/
void motionBackward()
{
thermoSensorFlag = true;
motion_servo_set_pulse_width(MOTION_WHEEL_RIGHT, 4800);
motion_servo_set_pulse_width(MOTION_WHEEL_LEFT, 1100);
motion_servo_start(MOTION_WHEEL_RIGHT);
motion_servo_start(MOTION_WHEEL_LEFT);
displayRedLED();
}
/**
* Spin Chico to the left slowly by setting the pulse width on both wheels to 1100
* Also display the blue led from the LEDHandler class.
*/
void motionSpinLeft()
{
thermoSensorFlag = false;
motion_servo_set_pulse_width(MOTION_WHEEL_RIGHT, 1100);
motion_servo_set_pulse_width(MOTION_WHEEL_LEFT, 1100);
motion_servo_start(MOTION_WHEEL_RIGHT);
motion_servo_start(MOTION_WHEEL_LEFT);
displayBlueLED();
}
/**
* Spin Chico to the left slowly by setting the pulse width on both wheels to 2750
* Also display the blue led from the LEDHandler class.
*/
void motionSpinLeftSlow()
{
thermoSensorFlag = false;
motion_servo_set_pulse_width(MOTION_WHEEL_RIGHT, 2750);
motion_servo_set_pulse_width(MOTION_WHEEL_LEFT, 2750);
motion_servo_start(MOTION_WHEEL_RIGHT);
motion_servo_start(MOTION_WHEEL_LEFT);
displayBlueLED();
}
/**
* Spin Chico to the right by setting both the right and left wheel pulse width to 4800.
* Also displays the blue led from the LEDHandler class.
*/
void motionSpinRight()
{
thermoSensorFlag = false;
motion_servo_set_pulse_width(MOTION_WHEEL_RIGHT, 4800);
motion_servo_set_pulse_width(MOTION_WHEEL_LEFT, 4800);
motion_servo_start(MOTION_WHEEL_RIGHT);
motion_servo_start(MOTION_WHEEL_LEFT);
displayBlueLED();
}
/**
* Stop Chico from moving by stopping both the wheels, also calls the method to display white led
* from the LEDHandler class.
*/
void motionStop()
{
thermoSensorFlag = false;
motion_servo_stop(MOTION_WHEEL_RIGHT);
motion_servo_stop(MOTION_WHEEL_LEFT);
displayWhiteLED();
}
/**
* Move the Thermo Sensor to the left and right while Chico is moving, and
* return it to the center
* when Chico is not moving
*/
void motionThermoSensor()
{
if (thermoLeft == 1)
{
pulseWidth -= 100;
if (pulseWidth <= 1100)
{
thermoLeft = 0;
}
}
else
{
pulseWidth += 100;
if (pulseWidth >= 4800)
{
thermoLeft = 1;
}
}
motion_servo_set_pulse_width(MOTION_SERVO_CENTER, pulseWidth);
motion_servo_start(MOTION_SERVO_CENTER);
}
/**
* Stop the Thermo Sensor by setting the pulse width to 2800.
*/
void motionThermoSensorStop()
{
motion_servo_set_pulse_width(MOTION_SERVO_CENTER, 2800);
motion_servo_start(MOTION_SERVO_CENTER);
pulseWidth = 2800;
}