Commit e9e00bc3 authored by Felix Kettner's avatar Felix Kettner

Merge branch 'feature-add-linesensor-and-gripper' into 'master'

# Conflicts:
#   hardware_test.py
#   pibot/nano.py
parents cb659646 3b156fc2
Pipeline #34316 passed with stage
in 30 seconds
This diff is collapsed.
......@@ -3,4 +3,8 @@
### boot.py & boot.sh
These files are responsible for the startup and background routine on the
PiBot.
They have to be placed in the /home/<USER> directory.
\ No newline at end of file
They have to be placed in the /home/<USER> directory.
### ArduinoPins.svg & RaspberryPins.svg
Here you can find a graphic overview of the used pins on the boards.
The files should be opened with an external viewer.
\ No newline at end of file
This diff is collapsed.
......@@ -3,6 +3,7 @@ from time import sleep
from pibot import leds
from pibot import constants as c
from pibot import buttons
from pibot import colorsensor
def test_motors():
......@@ -38,15 +39,44 @@ def test_ultrasonics():
sleep(0.5)
def test_gripper():
nano.close_gripper()
sleep(1)
nano.open_gripper()
sleep(1)
nano.close_gripper()
sleep(1)
nano.open_gripper()
def test_line_sensors():
for _ in range(10):
print(nano.get_line_sensors())
sleep(1)
def test_color_sensor():
colorsensor.init_color_sensor()
for _ in range(10):
print(nano.get_color())
sleep(1)
def main():
nano.set_motors(0, 0)
nano.reset_encoders()
test_mid_button()
test_motors()
test_front_leds()
test_ultrasonics()
test_gripper()
test_line_sensors()
test_color_sensor()
if __name__ == "__main__":
......
import RPi.GPIO as GPIO
from pibot import constants as c
def init_color_sensor():
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(c.COLOR_SENSOR_S2,GPIO.OUT)
GPIO.setup(c.COLOR_SENSOR_S3,GPIO.OUT)
......@@ -13,6 +13,7 @@ BUTTON_LEFT = 52
BUTTON_MID = 53
BUTTON_RIGHT = 54
# Pin definitions
LED_LEFT_RED_PIN = 19
LED_LEFT_GREEN_PIN = 0
......@@ -29,3 +30,6 @@ LED_FRONT_LEFT_PIN = 26
BUTTON_LEFT_PIN = 1
BUTTON_MID_PIN = 12
BUTTON_RIGHT_PIN = 20
COLOR_SENSOR_S2 = 17
COLOR_SENSOR_S3 = 27
\ No newline at end of file
......@@ -9,7 +9,7 @@ from pibot.serial_comm import (write_order,
read_ui8,
write_ui16)
from pibot.utils import open_serial_port, clamp
import pibot.constants as c
import struct
......@@ -50,9 +50,21 @@ class Nano:
else:
print("Battery voltage okay! ({} mV)".format(voltage))
def set_stat_led(self, toggle):
write_order(self.serial_file, order=Order.STAT_LED)
write_i8(self.serial_file, toggle)
def open_gripper(self):
write_order(self.serial_file, order=Order.GRIPPER)
write_i8(self.serial_file, 0)
def close_gripper(self):
write_order(self.serial_file, order=Order.GRIPPER)
write_i8(self.serial_file, 1)
def set_motors(self, left, right):
write_order(self.serial_file, order=Order.MOTORS)
......@@ -61,6 +73,7 @@ class Nano:
# Write right
write_i8(self.serial_file, clamp(right, -128, 127))
def get_encoders(self):
write_order(self.serial_file, order=Order.GET_ENCODERS)
......@@ -68,14 +81,27 @@ class Nano:
left = read_i16(self.serial_file)
right = read_i16(self.serial_file)
return left, right
def get_line_sensors(self):
write_order(self.serial_file, order=Order.GET_LINE_SENSORS)
# Wait for 3 values
left = read_i16(self.serial_file)
mid = read_i16(self.serial_file)
right = read_i16(self.serial_file)
return left, mid, right
def reset_encoders(self):
write_order(self.serial_file, order=Order.RESET_ENCODERS)
def get_battery_voltage(self):
write_order(self.serial_file, order=Order.VOLTAGE)
return read_i16(self.serial_file)
def get_distances(self):
write_order(self.serial_file, order=Order.DISTANCES)
......@@ -85,6 +111,7 @@ class Nano:
right = read_ui8(self.serial_file)
return left, mid, right
def set_buzzer(self, freq, duration):
write_order(self.serial_file, order=Order.BUZZER)
# send the frequency
......@@ -92,6 +119,58 @@ class Nano:
# send the duration
write_ui16(self.serial_file, duration)
def get_color_rgb(self):
# red
GPIO.output(c.COLOR_SENSOR_S2,GPIO.LOW)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.LOW)
write_order(self.serial_file, order=Order.COLOR)
red = read_i16(self.serial_file)
# blue
GPIO.output(c.COLOR_SENSOR_S2,GPIO.LOW)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.HIGH)
write_order(self.serial_file, order=Order.COLOR)
blue = read_i16(self.serial_file)
# green
GPIO.output(c.COLOR_SENSOR_S2,GPIO.HIGH)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.HIGH)
write_order(self.serial_file, order=Order.COLOR)
green = read_i16(self.serial_file)
return red, green, blue
def get_color(self):
# red
GPIO.output(c.COLOR_SENSOR_S2,GPIO.LOW)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.LOW)
write_order(self.serial_file, order=Order.COLOR)
red = read_i16(self.serial_file)
# blue
GPIO.output(c.COLOR_SENSOR_S2,GPIO.LOW)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.HIGH)
write_order(self.serial_file, order=Order.COLOR)
blue = read_i16(self.serial_file)
# green
GPIO.output(c.COLOR_SENSOR_S2,GPIO.HIGH)
GPIO.output(c.COLOR_SENSOR_S3,GPIO.HIGH)
write_order(self.serial_file, order=Order.COLOR)
green = read_i16(self.serial_file)
if red > green and blue > green:
return "green"
elif red > blue and green > blue:
return "blue"
elif green > red and blue > red:
return "red"
else:
return "none"
@staticmethod
def reset_nano():
GPIO.setmode(GPIO.BCM)
......
......@@ -24,6 +24,9 @@ class Order(Enum):
RESET_ENCODERS = 8
STAT_LED = 9
BUZZER = 10
GRIPPER = 11
GET_LINE_SENSORS = 12
COLOR = 13
def read_order(f):
......
# PiBot_Nano
Library for the Arduino Nano within the PiBot
\ No newline at end of file
#include <Arduino.h>
#include "pibot_nano.h"
#include "pibot_nano_distance.h"
#include "Servo.h"
//#include "pibot_nano_order.h"
//#include "pibot_nano_serial.h"
distance_sensors distance;
Servo servo;
void setup()
{
// Init Serial
pibot_nano_init();
servo.attach(SERVO);
servo.write(0);
}
void loop()
{
get_messages_from_serial();
//if(check_bumpers()){
// set_motors(0,0);
// }
distance.update();
}
//Serial
void get_messages_from_serial()
{
if(Serial.available() > 0)
{
// The first byte received is the instruction
Order order_received = read_order();
switch(order_received)
{
case VOLTAGE:
{
write_i16(get_battery_voltage());
break;
}
case STATUS_LED:
{
int8_t state = read_i8();
if(state==0){
stat_led(0);
}
else{
stat_led(1);
}
break;
}
case BUZ:
{
uint16_t freq = read_ui16();
uint16_t dur = read_ui16();
buzzer(freq,dur);
break;
}
case MOTORS:
{
int8_t left = read_i8();
int8_t right = read_i8();
set_motors(left,right);
break;
}
case DISTANCES:
{
write_ui8(distance.get_left());
write_ui8(distance.get_middle());
write_ui8(distance.get_right());
break;
}
case GET_ENCODERS:
{
write_i16(get_encoder(0));
write_i16(get_encoder(1));
break;
}
case GET_LINE_SENSORS:
{
write_i16(get_line_sensor(0));
write_i16(get_line_sensor(1));
write_i16(get_line_sensor(2));
break;
}
case RESET_ENCODERS:
{
reset_encoders();
break;
}
case BUZZER:
{
stat_led(1);
uint16_t freq = 2000;//read_ui16();
uint16_t dur = 2000;// read_ui16();
buzzer(freq,dur);
break;
}
case GRIPPER:
{
int8_t state = read_i8();
if(state==0){
servo.write(0);
}
else{
servo.write(90);
}
break;
}
case COLOR:
{
write_i16(get_color());
break;
}
// Unknown order
/*default:
write_order(ERROR);
write_i16(404);
return;
*/
}
//}
}
}
set_motors KEYWORD2
set_gripper KEYWORD2
get_line_sensors KEYWORD2
get_distance KEYWORD2
wait_for_button KEYWORD2
wait_for_any_button KEYWORD2
r_led KEYWORD2
y_led KEYWORD2
g_led KEYWORD2
r_led_pwm KEYWORD2
y_led_pwm KEYWORD2
g_led_pwm KEYWORD2
UNObot_init KEYWORD2
clear KEYWORD2
print KEYWORD2
lcd_goto_xy KEYWORD2
bar KEYWORD2
bar_manually KEYWORD2
buzzer KEYWORD2
get_color KEYWORD2
get_software_version KEYWORD2
get_hardware_version KEYWORD2
/*
This library is for the Aruino Nano on the Pibots.
pibot_nano.cpp - Library for Pibot
Created by Florian Zylla, June 21, 2019.
Edited by Felix Kettner.
*/
#include "Arduino.h"
//#include "Servo.h"
#include "pibot_nano.h"
#include "pibot_nano_serial.h"
#include "pibot_nano_order.h"
#include "pibot_nano_parameters.h"
//motors
int8_t v_max=100; //max. speed limit changeable (0<=v_max<=127)
void set_motors(int8_t left_speed, int8_t right_speed)
{
//turn off both canals
analogWrite(R_speed,0);
analogWrite(L_speed,0);
digitalWrite(R_dir,LOW);
digitalWrite(L_dir,LOW);
//detect backward driving and set phase pin
if(right_speed<0)
{
digitalWrite(R_dir,HIGH);
}
if(left_speed<0)
{
digitalWrite(L_dir,HIGH);
}
//make parameters positive
if(left_speed<0)
{
left_speed=left_speed*-1;
}
if(right_speed<0)
{
right_speed=right_speed*-1;
}
//detect and change too high values
if(left_speed>v_max)
{
left_speed=v_max;
}
if(right_speed>v_max)
{
right_speed=v_max;
}
//set speed
analogWrite(R_speed,right_speed);
analogWrite(L_speed,left_speed);
return;
}
uint16_t get_battery_voltage(void)
{
return (analogRead(BAT)/1023.0)*10000;
}
volatile int16_t ticks_left=0;
volatile int16_t ticks_right=0;
void encoder_left()
{
if(digitalRead(L_OUT1)==digitalRead(L_OUT2))
ticks_left++;
else
ticks_left--;
}
void encoder_right()
{
if(digitalRead(R_OUT1)==digitalRead(R_OUT2))
ticks_right--;
else
ticks_right++;
}
int16_t get_encoder(uint8_t nr)
{
switch(nr){
case 0:
return ticks_left;
break;
case 1:
return ticks_right;
break;
}
}
int16_t get_line_sensor(uint8_t nr)
{
switch(nr){
case 0:
return analogRead(LS_0);
break;
case 1:
return analogRead(LS_1);
break;
case 2:
return analogRead(LS_2);
break;
}
}
void reset_encoders(void)
{
ticks_left = 0;
ticks_right = 0;
}
//buzzer
void buzzer(uint16_t frequency, uint16_t duration)
{
tone(BUZZER,frequency,(uint32_t) duration);
}
//status led
void stat_led(boolean state)
{
digitalWrite(STAT_LED,state);
}
//colorsensor
int16_t get_color(void)
{
return pulseIn(CS, LOW);
}
//init function
void pibot_nano_init()
{
buzzer(3000,200);
//turn off motors
set_motors(0,0);
pinMode(R_dir, OUTPUT);
pinMode(R_speed,OUTPUT);
pinMode(L_dir, OUTPUT);
pinMode(L_speed,OUTPUT);
pinMode(R_OUT1, INPUT);
pinMode(R_OUT2, INPUT);
pinMode(L_OUT1, INPUT);
pinMode(L_OUT2, INPUT);
pinMode(BAT, INPUT);
//pinMode(B_L, INPUT_PULLUP);
//pinMode(B_R, INPUT_PULLUP);
//pinMode(B_B, INPUT_PULLUP);
pinMode(STAT_LED, OUTPUT);
pinMode(BUZZER, OUTPUT);
pinMode(SERVO, OUTPUT);
pinMode(LS_0, INPUT);
pinMode(LS_1, INPUT);
pinMode(LS_2, INPUT);
pinMode(CS, INPUT);
stat_led(1);
//Serial.begin(9600);
// Serial.println(get_battery_voltage());
delay(100);
attachInterrupt(digitalPinToInterrupt(L_OUT1),encoder_left,CHANGE);
attachInterrupt(digitalPinToInterrupt(R_OUT1),encoder_right,CHANGE);
stat_led(0);
Serial.begin(SERIAL_BAUD);
Serial.flush();
boolean connection = false;
while(!connection){
if(Serial.available())
{
buzzer(6000,100);
Order order_received = read_order();
if(order_received == START)
{
connection = true;
write_order(CONNECTED);
}
}
}
}
/*
pibot_nano.h - Library for Arduino Nano on Pibot by Roboschool .
Created by Florian Zylla, June 21, 2019.
Edited by Felix Kettner.
*/
#ifndef pibot_nano_h
#define pibot_nano_h
#include "Arduino.h"
#include "pibot_nano_serial.h"
#include "pibot_nano_order.h"
#include "pibot_nano_parameters.h"
void pibot_nano_init();
uint16_t get_battery_voltage(void);
void set_motors(int8_t left_speed, int8_t right_speed);
void encoder_left();
void encoder_right();
int16_t get_encoder(uint8_t nr);
int16_t get_line_sensor(uint8_t nr);
void reset_encoders(void);
void buzzer(uint16_t frequency, uint16_t duration);
void stat_led(boolean state);
int16_t get_color(void);
#endif
/*
This library is for the Aruino Nano on the Pibots.
pibot_nano.cpp - Library for Pibot
Created by Florian Zylla, June 21, 2019.
*/
#include "Arduino.h"
#include "pibot_nano_distance.h"
#include "pibot_nano_parameters.h"
distance_sensors::distance_sensors(){
left=0;
middle=0;