-
Notifications
You must be signed in to change notification settings - Fork 0
/
remoteControlRoomMeasuringRobot.ino
169 lines (127 loc) · 4.75 KB
/
remoteControlRoomMeasuringRobot.ino
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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
/* Remote-Control-Room-Measuring-Robot
https://github.com/saaketporay/Remote-Control-Room-Measuring-Robot
*/
/* Created by Saaket Poray using the TI-RSLK documentation
Uses these repositories:
https://github.com/saaketporay/Ultrasonic-Distance-Finder/
https://github.com/saaketporay/Controlling-TI-RSLK-Using-a-FM-Remote-Controller
This program lets you measure any room using the TI-RSLK
Robot-Library is required - available in the Github
*/
/*
BUGS:
1. Motor initializes and starts spinning if not connected to controller. Make sure the radio is on to prevent this. Please feel free to add a pull request if you have a solution.
*/
#include "Energia.h"
/* Defines motor functions of robot */
#include "Romi_Motor_Power.h"
/* Defines pin configuration of robot */
#include "RSLK_MAX_Pins.h"
Romi_Motor_Power left_motor;
Romi_Motor_Power right_motor;
#define LEFT_BTN 73 // LaunchPad S1 pin number or can use LP_S1_PIN
#define RIGHT_BTN 74 // LaunchPad S2 pin number or can use LP_S2_PIN
#define trigPin 10 // pin on ultrasonic sensor
#define echoPin 9 // pin on ultrasonic sensor
int rc2; //intializes variable that stores the value recieved from channel 2 on the controller
int rc4; //intializes variable that stores the value recieved from channel 4 on the controller
int rc5; //intializes variable that stores the value recieved from channel 5 on the controller
/* The setup() funtion runs one time at the beginning of the Energia program */
void setup() {
/* Set serial communication to 115200 baud rate for MSP432 */
Serial.begin(115200);
pinMode(trigPin, OUTPUT); //ultrasonic sensor
pinMode(echoPin, INPUT); //ultrasonic sensor
/* Initialize motor object pins */
left_motor.begin(MOTOR_L_SLP_PIN,
MOTOR_L_DIR_PIN,
MOTOR_L_PWM_PIN);
right_motor.begin(MOTOR_R_SLP_PIN,
MOTOR_R_DIR_PIN,
MOTOR_R_PWM_PIN);
/* Set bump switches to inputs */
pinMode(BP_SW_PIN_0, INPUT_PULLUP);
pinMode(BP_SW_PIN_1, INPUT_PULLUP);
pinMode(BP_SW_PIN_2, INPUT_PULLUP);
pinMode(BP_SW_PIN_3, INPUT_PULLUP);
pinMode(BP_SW_PIN_4, INPUT_PULLUP);
pinMode(BP_SW_PIN_5, INPUT_PULLUP);
/* Set left and right button on LaunchPad as input */
pinMode(LEFT_BTN, INPUT_PULLUP);
pinMode(RIGHT_BTN, INPUT_PULLUP);
pinMode(2, INPUT); //channel 2
pinMode(4, INPUT); //channel 4
pinMode(5, INPUT); //channel 5
}
/* The loop() function runs after the setup() function completes in an Energia porgram
and will continue to run in a repeating loop until the LaunchPad is reset or powered off */
void loop() {
left_motor.disableMotor();
right_motor.disableMotor();
rc2 = pulseIn(2, HIGH, 115200);
rc4 = pulseIn(4, HIGH, 115200);
rc5 = pulseIn(5, HIGH, 115200);
digitalWrite(LP_RGB_LED_BLUE_PIN, HIGH);
while (rc4 > 1550) //turn right
{
left_motor.enableMotor();
right_motor.enableMotor();
left_motor.directionForward();
right_motor.directionBackward();
left_motor.setSpeed((rc4 - 1550) / 5);
right_motor.setSpeed((rc4 - 1550) / 5);
rc2 = pulseIn(2, HIGH, 115200);
rc4 = pulseIn(4, HIGH, 115200);
}
while (rc4 < 1490) //turn left
{
left_motor.enableMotor();
right_motor.enableMotor();
left_motor.directionBackward();
right_motor.directionForward();
left_motor.setSpeed((1490 - rc4) / 5);
right_motor.setSpeed((1490 - rc4) / 5);
rc2 = pulseIn(2, HIGH, 115200);
rc4 = pulseIn(4, HIGH, 115200);
}
while (rc2 > 1550) //going forward
{
left_motor.enableMotor();
right_motor.enableMotor();
digitalWrite(LP_RGB_LED_RED_PIN, LOW);
digitalWrite(LP_RGB_LED_GREEN_PIN, HIGH);
left_motor.directionForward();
right_motor.directionForward();
left_motor.setSpeed((rc2 - 1550) / 5);
right_motor.setSpeed((rc2 - 1550) / 5);
rc2 = pulseIn(2, HIGH, 115200);
rc4 = pulseIn(4, HIGH, 115200);
}
while (rc2 < 1490) //going backward
{
left_motor.enableMotor();
right_motor.enableMotor();
digitalWrite(LP_RGB_LED_GREEN_PIN, LOW);
digitalWrite(LP_RGB_LED_RED_PIN, HIGH);
left_motor.directionBackward();
right_motor.directionBackward();
left_motor.setSpeed((1490 - rc2) / 5);
right_motor.setSpeed((1490 - rc2) / 5);
rc2 = pulseIn(2, HIGH, 115200);
rc4 = pulseIn(4, HIGH, 115200);
}
if (rc5 > 2000) //to control ultrasonic sensor
{
digitalWrite(LP_RGB_LED_GREEN_PIN, HIGH);
digitalWrite(LP_RGB_LED_RED_PIN, HIGH);
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration / 2) / 29.1;
Serial.println(distance); //prints distance
}
}