Skip to content

Commit 1593f23

Browse files
Rabe1402Rabe1402
authored
Merge pull request #1 from Rabe1402/Beta
Working code
2 parents a279c5d + 4b5722d commit 1593f23

File tree

5 files changed

+190
-0
lines changed

5 files changed

+190
-0
lines changed

‎Code/FTL/FTL.ino‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#include "KeplerOpenBOT.h"
2+
3+
float motorl_offset;
4+
float motorr_offset;
5+
float motorl_speed;
6+
float motorr_speed;
7+
float kp;
8+
int line_error;
9+
int start_normal;
10+
11+
12+
void setup()
13+
{
14+
KeplerOpenBOT_INIT();
15+
WRITE_LCD_CONTRAST(160);
16+
17+
motorl_offset = 20;
18+
motorr_offset = 20;
19+
kp = 0.5;
20+
}
21+
22+
void loop()
23+
{
24+
25+
byte spi_buffer[8] = {0};
26+
27+
// read 8 Bytes from OpenMV BEGIN
28+
29+
digitalWrite(SPICAMCSPIN, LOW);
30+
delay(1);
31+
32+
if(SPI.transfer(1) == 85)
33+
{
34+
spi_buffer[1]=SPI.transfer(0);
35+
spi_buffer[2]=SPI.transfer(0);
36+
spi_buffer[3]=SPI.transfer(0);
37+
spi_buffer[4]=SPI.transfer(0);
38+
spi_buffer[5]=SPI.transfer(0);
39+
spi_buffer[6]=SPI.transfer(0);
40+
spi_buffer[7]=SPI.transfer(0);
41+
}
42+
43+
digitalWrite(SPICAMCSPIN, HIGH);
44+
// read 8 Bytes from OpenMV END
45+
46+
//--- Button logic -----
47+
if (READ_BUTTON_CLOSED(B1)==1)
48+
{
49+
start_normal = 1;
50+
WRITE_LCD_CLEAR();
51+
}
52+
53+
if (READ_BUTTON_CLOSED(B4)==1)
54+
{
55+
start_normal = 0;
56+
WRITE_LCD_CLEAR();
57+
WRITE_MOTOR(ML, 0);
58+
WRITE_MOTOR(MR, 0);
59+
}
60+
61+
if (start_normal==0)
62+
{
63+
Serial.println("no program selected");
64+
Serial.println("press button B1 for normal startup");
65+
WRITE_LCD_TEXT(1, 1, "Select Program");
66+
WRITE_LCD_TEXT(1, 2, "B1 for normal startup");
67+
delay(100);
68+
}
69+
70+
if (start_normal==1)
71+
{
72+
line_error = spi_buffer[1] - 100;
73+
motorl_speed = motorl_offset - kp * line_error;
74+
motorr_speed = motorr_offset + kp * line_error;
75+
76+
WRITE_MOTOR(ML,(int)motorl_speed);
77+
WRITE_MOTOR(MR,(int)motorr_speed);
78+
79+
WRITE_LCD_TEXT(1,1,"Angle:");
80+
WRITE_LCD_INT(7,2,line_error,4);
81+
82+
WRITE_LCD_TEXT(3,4,"ML");
83+
WRITE_LCD_INT(1,5,(int)motorl_speed,4);
84+
85+
WRITE_LCD_TEXT(12,4,"MR");
86+
WRITE_LCD_INT(10,5,(int)motorr_speed,4);
87+
88+
WRITE_LCD_INT(1, 6, (int)spi_buffer[2], 4);
89+
90+
//--- red line logic -----
91+
while (spi_buffer[2]==1)
92+
{
93+
WRITE_MOTOR(ML, 0);
94+
WRITE_MOTOR(MR, 0);
95+
WRITE_LCD_CLEAR();
96+
WRITE_LCD_TEXT(1, 1, "RED LINE");
97+
WRITE_LCD_TEXT(1, 2, "END OF CODE");
98+
}
99+
}
100+
}
101+

FollowLineBeta/FTL/KeplerOpenBOT.h renamed to Code/FTL/KeplerOpenBOT.h

File renamed without changes.

‎Code/FTLR.py‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
import pyb, sensor, image, time, math
2+
3+
# ******************** SPI SEND INTERUPT ********************
4+
spi = pyb.SPI(2, pyb.SPI.SLAVE, polarity=0, phase=0)
5+
led_red = pyb.LED(1)
6+
led_green = pyb.LED(2)
7+
spi_list = [85, 0, 0, 0, 0, 0, 0, 0]
8+
spi_data = bytearray(spi_list)
9+
def nss_callback(line):
10+
global spi, spi_data
11+
try:
12+
spi.send(spi_data, timeout=1000)
13+
led_green.on()
14+
led_red.off()
15+
print(spi_data)
16+
except OSError as err:
17+
led_green.off()
18+
led_red.on()
19+
pass
20+
21+
pyb.ExtInt(pyb.Pin("P3"), pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, nss_callback)
22+
23+
# ******************** IMAGE DETECTION ********************
24+
25+
GRAYSCALE_THRESHOLD = [(0, 64, -28, 7, -9, 16)]
26+
27+
ROIS = [
28+
(0, 100, 160, 20, 0.7),
29+
(0, 50, 160, 20, 0.3),
30+
(0, 0, 160, 20, 0.1)
31+
]
32+
33+
RED_THRESHOLD = [(24, 65, 23, 60, 0, 52)]
34+
35+
RROIS = [
36+
(0, 100, 160, 20)
37+
]
38+
39+
weight_sum = 0
40+
41+
for r in ROIS: weight_sum += r[4]
42+
43+
sensor.reset()
44+
sensor.set_pixformat(sensor.RGB565)
45+
sensor.set_framesize(sensor.QQVGA)
46+
sensor.set_vflip(True)
47+
sensor.set_hmirror(True)
48+
sensor.skip_frames(time = 2000)
49+
sensor.set_auto_gain(False)
50+
sensor.set_auto_whitebal(False)
51+
clock = time.clock()
52+
53+
while(True):
54+
# ***** IMAGE DETECTION CODE *****
55+
clock.tick()
56+
img = sensor.snapshot()
57+
centroid_sum = 0
58+
for r in ROIS:
59+
blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)
60+
if blobs:
61+
largest_blob = max(blobs, key=lambda b: b.pixels())
62+
img.draw_rectangle(largest_blob.rect())
63+
img.draw_cross(largest_blob.cx(), largest_blob.cy())
64+
centroid_sum += largest_blob.cx() * r[4]
65+
center_pos = (centroid_sum / weight_sum) # Determine center of line.
66+
deflection_angle = 0
67+
deflection_angle = -math.atan((center_pos-80)/60)
68+
deflection_angle = math.degrees(deflection_angle)
69+
70+
# Find red line
71+
for r in RROIS:
72+
redline = 0
73+
blobs = img.find_blobs(RED_THRESHOLD, roi=r[0:4], merge=True)
74+
if blobs:
75+
largest_blob = max(blobs, key=lambda b: b.pixels())
76+
img.draw_rectangle(largest_blob.rect(), color=(255, 0, 0))
77+
img.draw_cross(largest_blob.cx(), largest_blob.cy(), color=(255, 0, 0))
78+
redline = 1
79+
80+
81+
# ***** SET and SEND VALUES over SPI to ARDUINO *****
82+
spi_list[1]=int(deflection_angle)+100
83+
spi_list[2]=int(redline)
84+
spi_data = bytearray(spi_list)
85+
# print("Turn Angle: %f" % deflection_angle)
86+
# print("FPS: " + str(clock.fps()))
87+
print(spi_list)

‎README.md‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,3 +8,5 @@ All information about the Roboter itself can be found [here](https://www.lbotics
88

99
Newer Version of the KeplerOpenBOT.h file can be found [here](https://www.lbotics.at/kepleropenbot/ein-erstes-programm)
1010

11+
Der Roboter kann immoment: Der linie folgen und bei rot stehen bleiben.
12+
Was der Roboter noch können soll: Lücken überfahren können(ist schon in planung), in kurven ncht mehr stecken beliden, bei grünen eckenrichtig abbiegen.

‎SourceCode.PDF‎

4.67 KB
Binary file not shown.

0 commit comments

Comments
 (0)