Skip to content

Commit d2d8db8

Browse files
machaj45machaj45
committed
Main controller update
1 parent ffcedc1 commit d2d8db8

File tree

874 files changed

+469
-41094
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

874 files changed

+469
-41094
lines changed

‎.gitignore

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,10 @@
33
*.elf
44
*.hex
55
*.d
6+
*.log
67
MainController/build*
78
workspace/devel/*
89
workspace/build*
10+
our_workspace/devel/*
11+
our_workspace/build/*
12+
our_workspace/logs/*

‎MainController/Config.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323
#define DSerial Serial3
2424
#define MSerial Serial1
2525
#define CSerial Serial2
26-
#define display_render_delay 10
26+
#define display_render_delay 0
2727
#define canCallSign 'C'
2828
#define displayCallSign 'D'
2929
#define motorCallSIgn 'M'

‎MainController/MainController.ino

Lines changed: 87 additions & 88 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#include <std_msgs/String.h>
33
#include <std_msgs/Int16.h>
44
#include <std_msgs/Int16MultiArray.h>
5+
#include <std_msgs/Empty.h>
56
#include "Config.h"
67
#include <SDL_Arduino_INA3221.h>
78
#include "lib.h"
@@ -12,6 +13,7 @@ void clearDisplay();
1213
void waitForInit();
1314
void initStuff();
1415
int retrieveCanCount();
16+
void bb();
1517

1618

1719
SDL_Arduino_INA3221 ina3221;
@@ -28,29 +30,29 @@ std_msgs::Int16 cans; //Create message to be send. Static alocation for safety
2830
ros::Publisher CanCount("CanCount", &cans); //this is probably important too
2931

3032
//----------------Motor controller msg----------//
31-
void enginesCb( const std_msgs::Int16MultiArray& engine_msg){
33+
void enginesCb(std_msgs::Int16MultiArray& engine_msg){
3234
int command = engine_msg.data[0];
3335
int parameter = engine_msg.data[1];
36+
delay(100);
3437
MSerial.print(command);
3538
MSerial.print(" ");
3639
MSerial.println(parameter);
37-
40+
//delay(50);
3841
DSerial.print("L2 ");
3942
DSerial.print(command);
4043
DSerial.print(" ");
4144
DSerial.println(parameter);
4245
}
4346
ros::Subscriber<std_msgs::Int16MultiArray> motorSubscriber("/master_node/main_loader/motor_control", &enginesCb );
44-
45-
/*void servo_cb(td_msgs::UInt16& cmd_msg){
46-
servo.write(cmd_msg.data); //set servo angle, should be from 0-180
47-
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
48-
} */
47+
48+
//----------Start can unloading process--------//
49+
void unloadCb(std_msgs::Empty& empty_msg){
4950

5051

51-
//ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
52-
53-
52+
53+
54+
}
55+
ros::Subscriber<std_msgs::Empty> unloadSubscriber("/master_node/main_loader/unload_cans", &unloadCb );
5456

5557
char hello[20] = "Hello from Arduino!";
5658
char time[20];
@@ -64,24 +66,24 @@ void setup() {
6466
//CSerial.begin(SERIAL2_BAUDRATE); //picker
6567
//CSerial.setTimeout(5);
6668
DSerial.begin(SERIAL3_BAUDRATE); //display
67-
DSerial.setTimeout(5);
69+
DSerial.setTimeout(1);
6870

6971
pinMode(16, INPUT);
7072
pinMode(17, INPUT);
7173

7274

7375
//waitForInit();
74-
MSerial.print('M');
75-
MSerial.print('C');
76-
MSerial.print('D');
76+
//MSerial.print('M');
77+
//MSerial.print('C');
78+
//MSerial.print('D');
7779
while(MSerial.peek()=='M'){MSerial.read();}
7880
while(CSerial.peek()=='C'){CSerial.read();}
79-
while(DSerial.peek()=='D'){DSerial.read();}
81+
//while(DSerial.peek()=='D'){DSerial.read();}
8082

8183

8284

8385
ina3221.begin();
84-
clearDisplay();
86+
//clearDisplay();
8587

8688

8789

@@ -92,37 +94,91 @@ void setup() {
9294
nh.advertise(chatter); //init topic in node
9395
nh.advertise(CanCount);
9496
nh.subscribe(motorSubscriber);
95-
// nh.subscribe(sub);
97+
nh.subscribe(unloadSubscriber);
98+
9699

97100
}
101+
void moveEngines(int command, int parameter){
102+
MSerial.print(command);
103+
MSerial.print(" ");
104+
MSerial.println(parameter);
105+
//MSerial.println("4 100");
106+
//delay(50);
107+
DSerial.print("L2 ");
108+
DSerial.print(command);
109+
DSerial.print(" ");
110+
DSerial.println(parameter);
111+
//DSerial.print("4 100 ");
112+
DSerial.print(MSerial.peek());
113+
DSerial.print(" | ");
114+
DSerial.println((char)MSerial.read());
115+
116+
117+
98118

119+
}
99120

100121
void loop(){
101-
initStuff();
102-
str_msg.data = hello; //save data into prealocated msg
122+
//initStuff();
123+
/*str_msg.data = hello; //save data into prealocated msg
103124
chatter.publish( &str_msg ); //publish msg
104125
105126
String foo = String(millis()); //save time to string
106127
foo.toCharArray(time, 20); //coppy string into char array
107128
str_msg.data = time; //save data into prealocated msg
108-
chatter.publish( &str_msg ); //publish msg
129+
chatter.publish( &str_msg ); //publish msg */
109130

110131
cans.data=retrieveCanCount();
111132
CanCount.publish( &cans );
112133
DSerial.print("L4Cans in storage ");
113-
DSerial.println(cans.data);
134+
DSerial.println(retrieveCanCount());
114135

115136

116137

117-
nh.spinOnce();
138+
//clearDisplay();
139+
//DSerial.print("DDDDDDDDDDDDDDDDDDDDDDDDDDDd");
118140
printDisplayInfo(); //print battery voltage, motor, PC and system current onto display
119-
printState();
120-
digitalWrite(LED1, HIGH);
121-
delay(500);
122-
digitalWrite(LED1, LOW);
123-
delay(500);
141+
printState(); //print ROS connection state and arduino time
142+
//bb();
143+
digitalWrite(LED1, !digitalRead(LED1)); // toggle state
144+
//DSerial.println("L5 STOP NECO NECO NECO");
145+
nh.spinOnce();
146+
147+
delay(5000);
148+
moveEngines(4, 50);
149+
delay(5000);
150+
moveEngines(4, -50);
124151
}
125152

153+
void bb(){
154+
delay(display_render_delay);
155+
156+
if(DSerial.peek()=='B'){
157+
DSerial.read();
158+
switch(DSerial.read()){
159+
case '0': //flush engines buffer
160+
MSerial.println("1 1");
161+
DSerial.println("L5B OOOO NECO NECO NECO");
162+
break;
163+
case '1':
164+
DSerial.println("L5B STOP OOOO NECO NECO");
165+
break;
166+
case '2':
167+
DSerial.println("L5B STOP NECO OOOO NECO");
168+
break;
169+
case '3':
170+
DSerial.println("L5B STOP NECO NECO OOOO");
171+
break;
172+
}
173+
} else {
174+
DSerial.println("L5B STOP NECO NECO NECO");
175+
}
176+
177+
178+
179+
}
180+
181+
126182
int retrieveCanCount(){
127183
CSerial.println("C");
128184
delay(10);
@@ -133,65 +189,7 @@ int retrieveCanCount(){
133189
}
134190
}
135191

136-
void initStuff(){
137-
/* if(MSerial.peek()=='M'){
138-
MSerial.write('M');
139-
MSerial.read();
140-
}
141-
if(DSerial.peek()=='D'){
142-
DSerial.write('D');
143-
DSerial.read();
144-
}
145-
if(CSerial.peek()=='C'){
146-
CSerial.write('C');
147-
CSerial.read();
148-
} */
149-
DSerial.write('D');
150-
MSerial.write('M');
151-
152-
}
153192

154-
void waitForInit() {
155-
156-
157-
MSerial.print('M');
158-
CSerial.print('C');
159-
DSerial.print('D');
160-
bool devOK[] = {false, false, false};
161-
//clearDisplay();
162-
DSerial.println("L0 (K)NightBot 0.1");
163-
DSerial.println("L2 Motors");
164-
DSerial.println("L3 CanCollector");
165-
DSerial.println("L4 Display");
166-
while (true) { //TODO resend unit letters, e.g. M,C,U,D to acknowledge
167-
if (!devOK[0]) {
168-
if(MSerial.read() == 'M'){
169-
devOK[0] = true;
170-
DSerial.println("L2 Motors OK");
171-
}
172-
}
173-
if (!devOK[1]) {
174-
if (CSerial.read() == 'C'){ //must be added U option - cube collector
175-
devOK[1] = true;
176-
DSerial.println("L3 Collector OK");
177-
}
178-
}
179-
if (!devOK[2]) {
180-
if (MSerial.read() == 'D'){ // well, cant print anything on display befor its initialisation
181-
devOK[2] = true;
182-
DSerial.println("L4 Display OK");
183-
}
184-
}
185-
if (devOK[0] && devOK[1] && devOK[2]) {
186-
break;
187-
}
188-
}
189-
while(MSerial.available()){MSerial.read();}
190-
while(CSerial.available()){CSerial.read();}
191-
while(DSerial.available()){DSerial.read();}
192-
delay(500);
193-
194-
}
195193

196194
void printDisplayInfo() {
197195
float battVoltage = ina3221.getBusVoltage_V(1);
@@ -229,7 +227,7 @@ void printDisplayInfo() {
229227

230228
void clearDisplay() {
231229
for (int i = 0; i < 6; i++) {
232-
delay(display_render_delay);
230+
delay(200);
233231
DSerial.print('L');
234232
DSerial.println(i);
235233
}
@@ -239,13 +237,14 @@ void printState() {
239237
delay(display_render_delay);
240238
DSerial.println("L0 (K)NightBot 0.1");
241239
DSerial.print("L3");
240+
delay(display_render_delay);
242241
delay(1);
243242
if (nh.connected()) {
244243
DSerial.print("ROS connected ");
245244
} else {
246-
DSerial.print("Waiting for ROS ");
245+
DSerial.print("ROS waiting ");
247246
}
248-
DSerial.print(millis());
247+
DSerial.print((int)(millis()/1000));
249248
DSerial.println();
250249

251250
}

‎MainController/old_junk.txt

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
void waitForInit() {
2+
3+
4+
MSerial.print('M');
5+
CSerial.print('C');
6+
DSerial.print('D');
7+
bool devOK[] = {false, false, false};
8+
//clearDisplay();
9+
DSerial.println("L0 (K)NightBot 0.1");
10+
DSerial.println("L2 Motors");
11+
DSerial.println("L3 CanCollector");
12+
DSerial.println("L4 Display");
13+
while (true) { //TODO resend unit letters, e.g. M,C,U,D to acknowledge
14+
if (!devOK[0]) {
15+
if(MSerial.read() == 'M'){
16+
devOK[0] = true;
17+
DSerial.println("L2 Motors OK");
18+
}
19+
}
20+
if (!devOK[1]) {
21+
if (CSerial.read() == 'C'){ //must be added U option - cube collector
22+
devOK[1] = true;
23+
DSerial.println("L3 Collector OK");
24+
}
25+
}
26+
if (!devOK[2]) {
27+
if (MSerial.read() == 'D'){ // well, cant print anything on display befor its initialisation
28+
devOK[2] = true;
29+
DSerial.println("L4 Display OK");
30+
}
31+
}
32+
if (devOK[0] && devOK[1] && devOK[2]) {
33+
break;
34+
}
35+
}
36+
37+
38+
39+
void initStuff(){
40+
/* if(MSerial.peek()=='M'){
41+
MSerial.write('M');
42+
MSerial.read();
43+
}
44+
if(DSerial.peek()=='D'){
45+
DSerial.write('D');
46+
DSerial.read();
47+
}
48+
if(CSerial.peek()=='C'){
49+
CSerial.write('C');
50+
CSerial.read();
51+
} */
52+
DSerial.write('D');
53+
//MSerial.write('M');
54+
55+
}
56+
57+
58+
while(MSerial.available()){MSerial.read();}
59+
while(CSerial.available()){CSerial.read();}
60+
while(DSerial.available()){DSerial.read();}
61+
delay(500);
62+
63+
}

‎our_workspace/.catkin_tools/profiles/default/packages/master_node/package.xml

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,25 +11,25 @@
1111
<buildtool_depend>catkin</buildtool_depend>
1212

1313
<build_depend>roscpp</build_depend>
14-
<build_depend>rospy</build_depend>
14+
<!-- <build_depend>rospy</build_depend>-->
1515
<build_depend>sensor_msgs</build_depend>
1616
<build_depend>pcl_ros</build_depend>
1717
<build_depend>pcl_conversions</build_depend>
1818
<build_depend>nodelet</build_depend>
19-
<build_depend>std_msgs</build_depend>
19+
<build_depend>std_msgs</build_depend>
2020

2121
<!-- <build_depend>message_runtime</build_depend>
2222
<build_depend>std_srvs</build_depend>
2323
<build_depend>message_generation</build_depend>
2424
-->
2525

2626
<run_depend>roscpp</run_depend>
27-
<run_depend>rospy</run_depend>
27+
<!-- <run_depend>rospy</run_depend>-->
2828
<run_depend>sensor_msgs</run_depend>
2929
<run_depend>pcl_ros</run_depend>
3030
<run_depend>pcl_conversions</run_depend>
3131
<run_depend>nodelet</run_depend>
32-
<run_depend>std_msgs</run_depend>
32+
<run_depend>std_msgs</run_depend>
3333
<!--
3434
<run_depend>message_runtime</run_depend>
3535
<run_depend>std_srvs</run_depend>

0 commit comments

Comments
 (0)