addition of camera scripts

This commit is contained in:
Pavol Debnar
2023-05-16 16:48:21 +02:00
parent 3995ba6298
commit c30e47c260
46 changed files with 1911 additions and 0 deletions

View File

@ -0,0 +1,36 @@
import socket
import subprocess
#from picamera2 import Picamera2
import numpy
import time
TCP_IP = '192.168.1.2'
TCP_PORT = 5001
sock = socket.socket()
sock.connect((TCP_IP, TCP_PORT))
#picam2 = PiCamera()
#config = picam2.create_preview_configuration({"size": (640, 480)})
#picam2.configure(config)
#picam2.start()
#array = picam2.capture_array("main")
while(True):
#array = picam2.capture_array("main")
#ock.send( str(len(array)).ljust(16))
#sock.send( array )
#time.sleep(0.070)
command = sock.recv(1)
command = command.decode()
if command == "n":
p = subprocess.Popen("./sensors/sensors", stdout=subprocess.PIPE, shell=True)
(output, err) = p.communicate()
print(output)
print(len(output))
sock.send(str(len(output)).encode())
sock.send(output)
sock.close()

View File

@ -0,0 +1 @@
python3 ~/Arducam-Pivariety-V4L2-Driver/focus/FocuserExample.py -d /dev/v4l-subdev1 --focus-step 10

View File

@ -0,0 +1,40 @@
CC = g++
OPT = -O3
FLAGS = -std=c++20 -Wall -Wextra -pedantic
LIBS = -lwiringPi
TARGET = bbx-test
SOURCEDIR = src
BUILDDIR = build
SOURCES = $(wildcard $(SOURCEDIR)/*.cpp)
OBJECTS = $(patsubst $(SOURCEDIR)/%.cpp,$(BUILDDIR)/%.o,$(SOURCES))
.PHONY: all clean depend run
all: $(TARGET)
$(BUILDDIR):
@mkdir -p $(BUILDDIR)
$(TARGET): $(OBJECTS)
$(CC) $(FLAGS) $(OPT) $^ -o $@ $(LIBS)
$(OBJECTS): $(BUILDDIR)/%.o : $(SOURCEDIR)/%.cpp $(SOURCEDIR)/%.hpp | $(BUILDDIR)
$(CC) $(FLAGS) $(OPT) -c $< -o $@ $(LIBS)
depend: .depend
.depend: $(SOURCES)
rm -rf $(BUILDDIR)/.depend
$(CXX) $(FLAGS) -MM $^ -MF $(BUILDDIR)/.depend
clean:
rm -rf $(BUILDDIR)
rm -f $(TARGET)
run: $(TARGET)
sudo ./$(TARGET)
-include $(BUILDDIR)/.depend

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -0,0 +1,16 @@
#include "LEDs.hpp"
LEDs::LEDs(){
wiringPiSetup();
pinMode(GPIO_LED_left, PWM_OUTPUT);
pinMode(GPIO_LED_right, PWM_OUTPUT);
pwmSetClock(4000); // Clock prescaller for PWM generator
}
void LEDs::Left(float power){
pwmWrite(GPIO_LED_left, power * 1024);
}
void LEDs::Right(float power){
pwmWrite(GPIO_LED_right, power * 1024);
}

View File

@ -0,0 +1,45 @@
#pragma once
#include <wiringPi.h>
using namespace std;
/**
* @brief LEDs strips controlled by AL8861Y
* Dimmable by PWM on channel PWM0 and PWM1
*/
class LEDs{
/**
* @brief WiringPi GPIO number of pin which controls left channel
*/
int GPIO_LED_left = 23;
/**
* @brief WiringPi GPIO number of pin which controls left channel
*/
int GPIO_LED_right = 26;
public:
/**
* @brief Construct a new LEDs object
* Configure GPIo and sets up frequency of PWM
* Closes possible frequency reachable by Rpi is around 7 kHz
*/
LEDs();
/**
* @brief Set power for right LED strip
* Right channel is connected to pin 33 on header, GPIO13 (wiring pin 26)
*
* @param power Power of LED strip in range from 0.0 to 1.0
*/
void Right(float power = 0);
/**
* @brief Set power for left LED strip
* Left channel is connected to pin 32 on header, GPIO12 (wiring pin 23)
*
* @param power Power of LED strip in range from 0.0 to 1.0
*/
void Left(float power = 0);
};

View File

@ -0,0 +1,104 @@
#include "adxl345.hpp"
ADXL345::ADXL345(bool channel):channel(channel){
// SPI Mode 3 - CPOL = 1, CPHA = 1
wiringPiSPISetupMode(channel, 500000, 3);
// Power-up
Write(registers::POWER_CTL, 0b00001000);
// Maximal data output rate
Write(registers::BW_RATE, 0b00001010);
// Check connection by reading constant value of chip ID
if(ID() != 0xe5){
cout << "Cannot establish connection to accelerometer" << endl;
}
}
uint8_t ADXL345::ID(){
return Read(registers::DEVID);
}
array<int, 3> ADXL345::Trim_offsets(){
// Zero offset
Write(registers::OFSX, 0);
Write(registers::OFSY, 0);
Write(registers::OFSZ, 0);
// Acquire current values
usleep(100*1000);
auto axis_values = Raw_values();
// Calculate trims, Only acceleration of 1g should be on Z axis
int X_trim = -(round(axis_values[0]/4.0));
int Y_trim = -(round(axis_values[1]/4.0));
int Z_trim = -(round((axis_values[2]-255)/4.0));
// Write new trim values to registers
Write(registers::OFSX, X_trim);
Write(registers::OFSY, Y_trim);
Write(registers::OFSZ, Z_trim);
// Wait for propagation
usleep(100*1000);
return {X_trim, Y_trim, Z_trim};
}
void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
Write(registers::OFSX, axis_offsets[0]);
Write(registers::OFSY, axis_offsets[1]);
Write(registers::OFSZ, axis_offsets[2]);
// Wait for propagation
usleep(100*1000);
}
uint8_t ADXL345::Read(registers address){
// 0x80 is read command
vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
wiringPiSPIDataRW(channel, transfer_data.data(), 2);
return transfer_data[1];
}
vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
vector<uint8_t> transfer_data( length + 1, dummy_packet);
// Insert address to first position in data transfer, rest if filled with dummy bytes
transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
// Remove first byte (address) from data
transfer_data.erase(transfer_data.begin());
return transfer_data;
}
void ADXL345::Write(registers address, uint8_t data){
vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
wiringPiSPIDataRW(channel, transfer_data.data(), 2);
}
array<int16_t, 3> ADXL345::Raw_values(){
vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
array<int16_t, 3> axis_values;
// Lower register represents LSB of value
axis_values[0] = register_values[0] | (register_values[1] << 8);
axis_values[1] = register_values[2] | (register_values[3] << 8);
axis_values[2] = register_values[4] | (register_values[5] << 8);
return axis_values;
}
double ADXL345::Inclination(){
auto axis_values = Raw_values();
// Resulting value is angle of IC package to horizontal plane
double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
// Calculate absolute tilt based on quadrants from sensor values
if((axis_values[1] < 0) and (axis_values[2] >= 0)){
angle *= -1;
} else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
angle = 180 - angle;
} else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
angle = -angle + 180;
} else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
angle = 360 - angle;
}
// Calculate inverse angle to corespond with clockwise rotation
return - angle + 360;
}

View File

@ -0,0 +1,124 @@
#pragma once
#include <iostream>
#include <vector>
#include <array>
#include <cmath>
#include <numbers>
#include <unistd.h>
#include <wiringPiSPI.h>
using namespace std;
/**
* @brief Accelerometer AXDL345
* Datasheet: https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf
*/
class ADXL345{
/**
* @brief Channel on which is accelerometer connected
*/
bool channel;
/**
* @brief Some of ADXL345 regsisters
*/
enum class registers: uint8_t{
DEVID = 0x00,
OFSX = 0x1e,
OFSY = 0x1f,
OFSZ = 0x20,
BW_RATE = 0x2c,
POWER_CTL = 0x2d,
DATA_FORMAT = 0x31,
DATAX0 = 0x32,
DATAX1 = 0x33,
DATAY0 = 0x34,
DATAY1 = 0x35,
DATAZ0 = 0x36,
DATAZ1 = 0x37
};
/**
* @brief Dummy packet, is not interpreted as any command by ADXL
Is used as padding for SPI transfer
*/
uint8_t dummy_packet = 0xaa;
public:
/**
* @brief Construct a new ADXL345 object
*
* @param channel SPI channel of Rpi, has only two channels, 0 or 1
*/
ADXL345(bool channel = 0);
/**
* @brief Reads ID from accelerometer
* Value is read only
* Suitable for testing of connection
*
* @return uint8_t Always should returns 0xe5 if communications is working
*/
uint8_t ID();
/**
* @brief Calculates roll angle of sensor based on value from X and Z values of acceleration
*
* @return double Roll angle of sensor (around X axis of IC), From 0 to 360°, clockwise rotation
*/
double Inclination();
/**
* @brief Trims offsets of accelerometer based on current values !!
* During this calibration process accelerometer should be position in base position (0 degrees)
* @return array<int8_t, 3> Values which was used as trimming values for coresponding axis [X,Y,Z]
*/
array<int, 3> Trim_offsets();
/**
* @brief Trims offsets of accelerometer with defined values
*
* @param axis_offsets Trim values for all accelerometer axis [X,Y,Z]
* Value is multiplied by 4 and added to resulting acceleration of respective axis
*/
void Trim_offsets(array<int8_t, 3> axis_offsets);
private:
/**
* @brief Reads raw values of acceleration for all axis
*
* @return array<int16_t, 3> Acceleration values from sensor for each axis [X,Y,Z]
*/
array<int16_t, 3> Raw_values();
/**
* @brief Read one byte of data from ADXL memory
*
* @param address Address in ADXL memory from which are data read
* @return uint8_t Data from memory
*/
uint8_t Read(registers address);
/**
* @brief Reads sequence of data from ADXL memory
*
* @param address Address in ADXL memory from which reading starts
* @param length Amount of bytes to read
* @return vector<uint8_t> Readout data
*/
vector<uint8_t> Read_sequence(registers address, short length);
/**
* @brief Write one byte of data into ADXL memory
*
* @param address Address to which will be writing performed
* @param data Data to write
*/
void Write(registers address, uint8_t data);
};

View File

@ -0,0 +1,21 @@
#include "demo.hpp"
using namespace std;
int main(){
// ----- Fan -----
auto fan = Fan();
fan.On();
// ----- LEDS -----
auto led = LEDs();
for(int i = 0; i < 100000; i++){
led.Left(1.0);
led.Right(1.0);
usleep(10000);
led.Left(0.0);
led.Right(0.0);
}
fan.Off();
}

View File

@ -0,0 +1,12 @@
#pragma once
#include <iostream>
#include <iomanip>
#include <unistd.h>
#include "adxl345.hpp"
#include "fan.hpp"
#include "LEDs.hpp"
#include "sick_side.hpp"
#include "sick_front.hpp"

View File

@ -0,0 +1,14 @@
#include "fan.hpp"
Fan::Fan(){
wiringPiSetup();
pinMode(GPIO_fan, OUTPUT);
}
void Fan::On(){
digitalWrite(GPIO_fan, 1);
}
void Fan::Off(){
digitalWrite(GPIO_fan, 0);
}

View File

@ -0,0 +1,34 @@
#pragma once
#include <wiringPi.h>
#include <softPwm.h>
using namespace std;
/**
* @brief PWM controlled fan without RPM detection
*/
class Fan{
/**
* @brief WiringPi GPIO number of pin which controls fan
*/
int GPIO_fan = 25;
public:
/**
* @brief Construct a new Fan object
* Create soft-PWM thread for fan control signal
*/
Fan();
/**
* @brief Enable cooling fan
*/
void On();
/**
* @brief Disable cooling fan
*/
void Off();
};

View File

@ -0,0 +1,61 @@
#include "sick_front.hpp"
Sick_front::Sick_front(bool channel):channel(channel){
wiringPiSPISetupMode(channel, 500000, 3);
wiringPiSetup();
pinMode(GPIO_conversion_start, OUTPUT);
pinMode(GPIO_laser_enable, OUTPUT);
}
optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
unsigned int sum = 0;
unsigned int successful_measurements = 0;
for(unsigned int i = 0; i < measurement_count; i++){
auto distance = Distance();
if(distance){
sum += *distance;
successful_measurements++;
}
}
if(sum == 0){
return {};
}
return sum/successful_measurements;
}
optional<unsigned int> Sick_front::Distance(){
auto raw = Conversion_raw();
float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
// Calculation of true voltage from reference voltage and ADC value
double voltage = differential_raw / (1 << 16) * reference_voltage;
//cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
// Out of Range
if((voltage < voltage_close) | (voltage > voltage_far)){
return {};
}
double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
double distance = round((range_max-range_min)*range_percentage+range_min);
return distance;
}
uint16_t Sick_front::Conversion_raw(){
// Start conversion
digitalWrite(GPIO_conversion_start, 1);
array<uint8_t, 2> received_data = {0x00, 0x00};
wiringPiSPIDataRW(channel, received_data.data(), 2);
// Start conversion
digitalWrite(GPIO_conversion_start, 0);
return ((received_data[0]<<8) | received_data[1]);
}
void Sick_front::Laser(bool state){
if(state){
// ON
pinMode(GPIO_laser_enable, INPUT);
} else {
// OFF
pinMode(GPIO_laser_enable, OUTPUT);
digitalWrite(GPIO_laser_enable, 0);
}
}

View File

@ -0,0 +1,134 @@
#pragma once
#include <iostream>
#include <array>
#include <string>
#include <optional>
#include <cmath>
#include <iomanip>
#include <wiringPi.h>
#include <wiringPiSPI.h>
using namespace std;
/**
Type: DT50-N2113
**/
class Sick_front{
/**
* @brief Channel on which is ADC for front distance sensor connected
*/
bool channel;
/**
* @brief WiringPi GPIO number of pin which start conversion by change 0->1
*/
const int GPIO_conversion_start = 5;
/**
* @brief WiringPi GPIO number of pin which turn ON/OFF laser of sensor
*/
const int GPIO_laser_enable = 4;
/**
* @brief Minimal configured distance of sensor
*/
const int range_min = 200;
/**
* @brief Maximal configured distance of sensor
*/
const int range_max = 10000;
/**
* @brief Value of sense resistor in ohms used for current loop
*/
const int sense_resistor = 150;
/**
* @brief Current coresponding to minimal distance, configured in "range_min"
*/
const float current_loop_min = 0.004;
/**
* @brief Current coresponding to maximal distance, configured in "range_max"
*/
const float current_loop_max = 0.020;
/**
* @brief Current coresponding to state when minimal distance is exceeded
*/
const float current_loop_close = 0.0035;
/**
* @brief Current coresponding to state when maximal distance is exceeded
*/
const float current_loop_far = 0.0205;
/**
* @brief Voltage coresponding to maximal distance
*/
const float voltage_max = current_loop_max*sense_resistor;
/**
* @brief Voltage coresponding to minimal distance
*/
const float voltage_min = current_loop_min*sense_resistor;
/**
* @brief Voltage coresponding to state when maximal distance is exceeded
*/
const float voltage_close = current_loop_close*sense_resistor;
/**
* @brief Voltage coresponding to state when minimal distance is exceeded
*/
const float voltage_far = current_loop_far*sense_resistor;
/**
* @brief Reference voltage of ADC
*/
const float reference_voltage = 3.308f;
public:
/**
* @brief Construct a new Sick_front object
* Configure GPIO and SPI interface
*
* @param channel Channel on which is sensor connected
*/
Sick_front(bool channel = 1);
/**
* @brief Calculate distance measurted by distance sensor
*
* @return optional<unsigned int> True and value in mm if in range, False otherwise
*/
optional<unsigned int> Distance();
/**
* @brief Average multiple measurements of distance sensor output
*
* @param measurement_count Amount of measurents to average
* @return optional<unsigned int> Same as Distance()
*/
optional<unsigned int> Distance_avg(unsigned int measurement_count = 8);
/**
* @brief Turn ON or OFF laser of sensor
*
* @param state New state of laser
*/
void Laser(bool state);
private:
/**
* @brief Read raw ADC data from ADC
*
* @return uint16_t raw ADc data in 2's complement format
*/
uint16_t Conversion_raw();
};

View File

@ -0,0 +1,103 @@
#include "sick_side.hpp"
Sick_side::Sick_side(){
serial_descriptor = serialOpen("/dev/ttyS0", 9600);
if(serial_descriptor == -1){
cout << "Cannot open serial line for communication with side distance sensor" << endl;
}
}
uint8_t Sick_side::ID(){
Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
//while(Data_available() < 12);
// Readout data which are in buffer from transmittion
//Receive();
// Read real received data from sensor
if(auto responce = Receive()){
return (*responce)[2];
} else {
return 0x00;
}
}
void Sick_side::Laser(bool state){
if(state){
// ON
Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
} else {
// OFF
Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
}
//Read responce to clear buffer
Receive();
}
optional<double> Sick_side::Distance(){
auto distance_data = Distance_data();
if(distance_data == 0x7fff){
// Out of measuring range
return {};
} else {
// Convert to 2'complement and then to mm
return static_cast<int16_t>(distance_data)/100.0;
}
}
uint16_t Sick_side::Distance_data(){
Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
if(auto responce = Receive()){
return (((*responce)[1]<<8) | (*responce)[2]);
} else {
return 0x7fff;
}
}
void Sick_side::Send_data(array<uint8_t, 3> data){
array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
packet[5] = Calculate_BCC(packet);
Transmit(packet);
}
void Sick_side::Transmit(array<uint8_t, 6> data){
for(int i = 0; i < 6; i++){
serialPutchar(serial_descriptor, data[i]);
}
}
uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
uint8_t BCC = 0;
// BCC is calculated only from bytes on positions 1,2,3
for(int byte_index = 1; byte_index < 4; byte_index++){
BCC ^= data[byte_index];
}
return BCC;
}
optional<array<uint8_t, 3>> Sick_side::Receive(){
while(Data_available() < 12);
// Readout data which are in buffer from transmittion
for(int i = 0; i < 6; i++){
serialGetchar(serial_descriptor);
}
// Receive true data
array<uint8_t, 6> received_data;
for(int i = 0; i < 6; i++){
received_data[i] = serialGetchar(serial_descriptor);
}
// Check for communication errors
if(static_cast<signs>(received_data[1]) == signs::NAK){
cout << error_codes[received_data[2]] << endl;
return {};
}
if(received_data[5] != Calculate_BCC(received_data)){
cout << "Received BCC is invalid" << endl;
return {};
}
return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
}
unsigned int Sick_side::Data_available(){
return serialDataAvail(serial_descriptor);
}

View File

@ -0,0 +1,119 @@
#pragma once
#include <iostream>
#include <array>
#include <optional>
#include <map>
#include <string>
#include <wiringSerial.h>
using namespace std;
/**
Type: OD1-B100C50A15
**/
class Sick_side{
int serial_descriptor = 0;
enum class signs: uint8_t{
STX = 0x02,
ETX = 0x03,
ACK = 0x06,
NAK = 0x15,
};
map<uint8_t, string> error_codes{
{0x02, "Address is invalid"},
{0x04, "BCC values is invalid"},
{0x05, "Invalid command"},
{0x06, "Invalid setting value (out of spec)"},
{0x07, "Invalid setting value (out of range)"}
};
enum class commands: uint8_t{
Command = 0x43, // Also used for reading out measured values
Write = 0x57,
Read = 0x52,
};
public:
/**
* @brief Construct a new Sick_side object
* Opens serial line and save file descriptor
*/
Sick_side();
/**
* @brief Read sensor ID from memory of sensor
* Used for connection check
*
* @return uint8_t Value 0x64, 0x00 if communication fails
*/
uint8_t ID();
/**
* @brief Turn ON or OFF laser of sensor
*
* @param state New state of laser
*/
void Laser(bool state);
/**
* @brief Read and calculate distance from distance sensor
* Distance is relative to sensors zero point
*
* @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
*/
optional<double> Distance();
private:
/**
* @brief Reads distance data from sensor register
*
* @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
*/
uint16_t Distance_data();
/**
* @brief Transmit given data over serial to RS485 transmitter
*
* @param data Data to send via serial line, header and footer are added
*/
void Send_data(array<uint8_t, 3> data);
/**
* @brief Calculates BCC for given packet of data
* BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
*
* @param data Packet used for calculation
* @return uint8_t Values of BCC
*/
uint8_t Calculate_BCC(array<uint8_t, 6> data);
/**
* @brief Transmits data over UART to RS485 transmitter
* Data are also copied to input buffer due to half-duplex connection
*
* @param data Data to transmit
*/
void Transmit(array<uint8_t, 6> data);
/**
* @brief Data received from sensor
* Only usefull data are returned, without STX, ACK/NACK, BCC
*
* @return optional<array<uint8_t, 3>> True and value if communication is valid, False otherwise
*/
optional<array<uint8_t, 3>> Receive();
/**
* @brief Reads amopunt of data in input buffer
*
* @return unsigned int Number of bytes in input buffer
*/
unsigned int Data_available();
};

View File

@ -0,0 +1,6 @@
all:
make -C lights/
mv lights/bbx-test lights/lights
make -C sensors/
mv sensors/bbx-test sensors/sensors

View File

@ -0,0 +1,40 @@
CC = g++
OPT = -O3
FLAGS = -std=c++20 -Wall -Wextra -pedantic
LIBS = -lwiringPi
TARGET = bbx-test
SOURCEDIR = src
BUILDDIR = build
SOURCES = $(wildcard $(SOURCEDIR)/*.cpp)
OBJECTS = $(patsubst $(SOURCEDIR)/%.cpp,$(BUILDDIR)/%.o,$(SOURCES))
.PHONY: all clean depend run
all: $(TARGET)
$(BUILDDIR):
@mkdir -p $(BUILDDIR)
$(TARGET): $(OBJECTS)
$(CC) $(FLAGS) $(OPT) $^ -o $@ $(LIBS)
$(OBJECTS): $(BUILDDIR)/%.o : $(SOURCEDIR)/%.cpp $(SOURCEDIR)/%.hpp | $(BUILDDIR)
$(CC) $(FLAGS) $(OPT) -c $< -o $@ $(LIBS)
depend: .depend
.depend: $(SOURCES)
rm -rf $(BUILDDIR)/.depend
$(CXX) $(FLAGS) -MM $^ -MF $(BUILDDIR)/.depend
clean:
rm -rf $(BUILDDIR)
rm -f $(TARGET)
run: $(TARGET)
sudo ./$(TARGET)
-include $(BUILDDIR)/.depend

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -0,0 +1,16 @@
#include "LEDs.hpp"
LEDs::LEDs(){
wiringPiSetup();
pinMode(GPIO_LED_left, PWM_OUTPUT);
pinMode(GPIO_LED_right, PWM_OUTPUT);
pwmSetClock(4000); // Clock prescaller for PWM generator
}
void LEDs::Left(float power){
pwmWrite(GPIO_LED_left, power * 1024);
}
void LEDs::Right(float power){
pwmWrite(GPIO_LED_right, power * 1024);
}

View File

@ -0,0 +1,45 @@
#pragma once
#include <wiringPi.h>
using namespace std;
/**
* @brief LEDs strips controlled by AL8861Y
* Dimmable by PWM on channel PWM0 and PWM1
*/
class LEDs{
/**
* @brief WiringPi GPIO number of pin which controls left channel
*/
int GPIO_LED_left = 23;
/**
* @brief WiringPi GPIO number of pin which controls left channel
*/
int GPIO_LED_right = 26;
public:
/**
* @brief Construct a new LEDs object
* Configure GPIo and sets up frequency of PWM
* Closes possible frequency reachable by Rpi is around 7 kHz
*/
LEDs();
/**
* @brief Set power for right LED strip
* Right channel is connected to pin 33 on header, GPIO13 (wiring pin 26)
*
* @param power Power of LED strip in range from 0.0 to 1.0
*/
void Right(float power = 0);
/**
* @brief Set power for left LED strip
* Left channel is connected to pin 32 on header, GPIO12 (wiring pin 23)
*
* @param power Power of LED strip in range from 0.0 to 1.0
*/
void Left(float power = 0);
};

View File

@ -0,0 +1,104 @@
#include "adxl345.hpp"
ADXL345::ADXL345(bool channel):channel(channel){
// SPI Mode 3 - CPOL = 1, CPHA = 1
wiringPiSPISetupMode(channel, 500000, 3);
// Power-up
Write(registers::POWER_CTL, 0b00001000);
// Maximal data output rate
Write(registers::BW_RATE, 0b00001010);
// Check connection by reading constant value of chip ID
if(ID() != 0xe5){
cout << "Cannot establish connection to accelerometer" << endl;
}
}
uint8_t ADXL345::ID(){
return Read(registers::DEVID);
}
array<int, 3> ADXL345::Trim_offsets(){
// Zero offset
Write(registers::OFSX, 0);
Write(registers::OFSY, 0);
Write(registers::OFSZ, 0);
// Acquire current values
usleep(100*1000);
auto axis_values = Raw_values();
// Calculate trims, Only acceleration of 1g should be on Z axis
int X_trim = -(round(axis_values[0]/4.0));
int Y_trim = -(round(axis_values[1]/4.0));
int Z_trim = -(round((axis_values[2]-255)/4.0));
// Write new trim values to registers
Write(registers::OFSX, X_trim);
Write(registers::OFSY, Y_trim);
Write(registers::OFSZ, Z_trim);
// Wait for propagation
usleep(100*1000);
return {X_trim, Y_trim, Z_trim};
}
void ADXL345::Trim_offsets(array<int8_t, 3> axis_offsets){
Write(registers::OFSX, axis_offsets[0]);
Write(registers::OFSY, axis_offsets[1]);
Write(registers::OFSZ, axis_offsets[2]);
// Wait for propagation
usleep(100*1000);
}
uint8_t ADXL345::Read(registers address){
// 0x80 is read command
vector<uint8_t> transfer_data = {static_cast<uint8_t>(static_cast<uint8_t>(address) | 0x80), dummy_packet};
wiringPiSPIDataRW(channel, transfer_data.data(), 2);
return transfer_data[1];
}
vector<uint8_t> ADXL345::Read_sequence(registers address, short length){
vector<uint8_t> transfer_data( length + 1, dummy_packet);
// Insert address to first position in data transfer, rest if filled with dummy bytes
transfer_data[0] = static_cast<uint8_t>(address) | 0xc0;
wiringPiSPIDataRW(channel, transfer_data.data(), length + 1);
// Remove first byte (address) from data
transfer_data.erase(transfer_data.begin());
return transfer_data;
}
void ADXL345::Write(registers address, uint8_t data){
vector<uint8_t> transfer_data = {static_cast<uint8_t>(address), data};
wiringPiSPIDataRW(channel, transfer_data.data(), 2);
}
array<int16_t, 3> ADXL345::Raw_values(){
vector<uint8_t> register_values = Read_sequence(registers::DATAX0, 6);
array<int16_t, 3> axis_values;
// Lower register represents LSB of value
axis_values[0] = register_values[0] | (register_values[1] << 8);
axis_values[1] = register_values[2] | (register_values[3] << 8);
axis_values[2] = register_values[4] | (register_values[5] << 8);
return axis_values;
}
double ADXL345::Inclination(){
auto axis_values = Raw_values();
// Resulting value is angle of IC package to horizontal plane
double angle = atan(static_cast<double>(axis_values[1])/axis_values[2]) * (180 / numbers::pi);
// Calculate absolute tilt based on quadrants from sensor values
if((axis_values[1] < 0) and (axis_values[2] >= 0)){
angle *= -1;
} else if ((axis_values[1] < 0) and (axis_values[2] < 0)){
angle = 180 - angle;
} else if ((axis_values[1] >= 0) and (axis_values[2] < 0)){
angle = -angle + 180;
} else if ((axis_values[1] > 0) and (axis_values[2] >= 0)){
angle = 360 - angle;
}
// Calculate inverse angle to corespond with clockwise rotation
return - angle + 360;
}

View File

@ -0,0 +1,124 @@
#pragma once
#include <iostream>
#include <vector>
#include <array>
#include <cmath>
#include <numbers>
#include <unistd.h>
#include <wiringPiSPI.h>
using namespace std;
/**
* @brief Accelerometer AXDL345
* Datasheet: https://www.analog.com/media/en/technical-documentation/data-sheets/ADXL345.pdf
*/
class ADXL345{
/**
* @brief Channel on which is accelerometer connected
*/
bool channel;
/**
* @brief Some of ADXL345 regsisters
*/
enum class registers: uint8_t{
DEVID = 0x00,
OFSX = 0x1e,
OFSY = 0x1f,
OFSZ = 0x20,
BW_RATE = 0x2c,
POWER_CTL = 0x2d,
DATA_FORMAT = 0x31,
DATAX0 = 0x32,
DATAX1 = 0x33,
DATAY0 = 0x34,
DATAY1 = 0x35,
DATAZ0 = 0x36,
DATAZ1 = 0x37
};
/**
* @brief Dummy packet, is not interpreted as any command by ADXL
Is used as padding for SPI transfer
*/
uint8_t dummy_packet = 0xaa;
public:
/**
* @brief Construct a new ADXL345 object
*
* @param channel SPI channel of Rpi, has only two channels, 0 or 1
*/
ADXL345(bool channel = 0);
/**
* @brief Reads ID from accelerometer
* Value is read only
* Suitable for testing of connection
*
* @return uint8_t Always should returns 0xe5 if communications is working
*/
uint8_t ID();
/**
* @brief Calculates roll angle of sensor based on value from X and Z values of acceleration
*
* @return double Roll angle of sensor (around X axis of IC), From 0 to 360°, clockwise rotation
*/
double Inclination();
/**
* @brief Trims offsets of accelerometer based on current values !!
* During this calibration process accelerometer should be position in base position (0 degrees)
* @return array<int8_t, 3> Values which was used as trimming values for coresponding axis [X,Y,Z]
*/
array<int, 3> Trim_offsets();
/**
* @brief Trims offsets of accelerometer with defined values
*
* @param axis_offsets Trim values for all accelerometer axis [X,Y,Z]
* Value is multiplied by 4 and added to resulting acceleration of respective axis
*/
void Trim_offsets(array<int8_t, 3> axis_offsets);
private:
/**
* @brief Reads raw values of acceleration for all axis
*
* @return array<int16_t, 3> Acceleration values from sensor for each axis [X,Y,Z]
*/
array<int16_t, 3> Raw_values();
/**
* @brief Read one byte of data from ADXL memory
*
* @param address Address in ADXL memory from which are data read
* @return uint8_t Data from memory
*/
uint8_t Read(registers address);
/**
* @brief Reads sequence of data from ADXL memory
*
* @param address Address in ADXL memory from which reading starts
* @param length Amount of bytes to read
* @return vector<uint8_t> Readout data
*/
vector<uint8_t> Read_sequence(registers address, short length);
/**
* @brief Write one byte of data into ADXL memory
*
* @param address Address to which will be writing performed
* @param data Data to write
*/
void Write(registers address, uint8_t data);
};

View File

@ -0,0 +1,43 @@
#include "demo.hpp"
using namespace std;
int main(){
// ----- Accelerometer -----
auto accelerometer = ADXL345();
// "HOT" trimming with current values of tilt
// This emthod is not expected to be used in production
//accelerometer.Trim_offsets();
// Trimming with predefined values, based on previous measuments
// Data for command below can be taken from command above
accelerometer.Trim_offsets({0,0,0});
// ----- Side laser -----
auto side = Sick_side();
// Enable laser of for measuring
side.Laser(false);
// ----- Front laser -----
auto front = Sick_front();
front.Laser(true);
// ----- Test loop -----
// Read inclination from accelerometer and distance from both sensors in loop and print it
while(true){
if(auto distance = front.Distance_avg()) {
setprecision(1);
cout << "{ \n \"depthPos\" :\n {\"x\" : " << accelerometer.Inclination() << ",\n \"y\" : " << *distance << "\n},\n \"rectangle\" : \n {\n \"h\" : 50.0, \n \"w\" : 60.0 \n}} " <<std::endl;
break;
}
else
{
distance=0.0;
}
}
return 0;
}

View File

@ -0,0 +1,12 @@
#pragma once
#include <iostream>
#include <iomanip>
#include <unistd.h>
#include "adxl345.hpp"
#include "fan.hpp"
#include "LEDs.hpp"
#include "sick_side.hpp"
#include "sick_front.hpp"

View File

@ -0,0 +1,14 @@
#include "fan.hpp"
Fan::Fan(){
wiringPiSetup();
pinMode(GPIO_fan, OUTPUT);
}
void Fan::On(){
digitalWrite(GPIO_fan, 1);
}
void Fan::Off(){
digitalWrite(GPIO_fan, 0);
}

View File

@ -0,0 +1,34 @@
#pragma once
#include <wiringPi.h>
#include <softPwm.h>
using namespace std;
/**
* @brief PWM controlled fan without RPM detection
*/
class Fan{
/**
* @brief WiringPi GPIO number of pin which controls fan
*/
int GPIO_fan = 25;
public:
/**
* @brief Construct a new Fan object
* Create soft-PWM thread for fan control signal
*/
Fan();
/**
* @brief Enable cooling fan
*/
void On();
/**
* @brief Disable cooling fan
*/
void Off();
};

View File

@ -0,0 +1,61 @@
#include "sick_front.hpp"
Sick_front::Sick_front(bool channel):channel(channel){
wiringPiSPISetupMode(channel, 500000, 3);
wiringPiSetup();
pinMode(GPIO_conversion_start, OUTPUT);
pinMode(GPIO_laser_enable, OUTPUT);
}
optional<unsigned int> Sick_front::Distance_avg(unsigned int measurement_count){
unsigned int sum = 0;
unsigned int successful_measurements = 0;
for(unsigned int i = 0; i < measurement_count; i++){
auto distance = Distance();
if(distance){
sum += *distance;
successful_measurements++;
}
}
if(sum == 0){
return {};
}
return sum/successful_measurements;
}
optional<unsigned int> Sick_front::Distance(){
auto raw = Conversion_raw();
float differential_raw = static_cast<float>(static_cast<uint16_t>(raw));
// Calculation of true voltage from reference voltage and ADC value
double voltage = differential_raw / (1 << 16) * reference_voltage;
//cout << "Voltage: " << hex << raw << dec << " : " <<differential_raw << " : " << setprecision(4) << voltage << " V" << setprecision(0);
// Out of Range
if((voltage < voltage_close) | (voltage > voltage_far)){
return {};
}
double range_percentage = (voltage-voltage_min)/(voltage_max-voltage_min);
double distance = round((range_max-range_min)*range_percentage+range_min);
return distance;
}
uint16_t Sick_front::Conversion_raw(){
// Start conversion
digitalWrite(GPIO_conversion_start, 1);
array<uint8_t, 2> received_data = {0x00, 0x00};
wiringPiSPIDataRW(channel, received_data.data(), 2);
// Start conversion
digitalWrite(GPIO_conversion_start, 0);
return ((received_data[0]<<8) | received_data[1]);
}
void Sick_front::Laser(bool state){
if(state){
// ON
pinMode(GPIO_laser_enable, INPUT);
} else {
// OFF
pinMode(GPIO_laser_enable, OUTPUT);
digitalWrite(GPIO_laser_enable, 0);
}
}

View File

@ -0,0 +1,134 @@
#pragma once
#include <iostream>
#include <array>
#include <string>
#include <optional>
#include <cmath>
#include <iomanip>
#include <wiringPi.h>
#include <wiringPiSPI.h>
using namespace std;
/**
Type: DT50-N2113
**/
class Sick_front{
/**
* @brief Channel on which is ADC for front distance sensor connected
*/
bool channel;
/**
* @brief WiringPi GPIO number of pin which start conversion by change 0->1
*/
const int GPIO_conversion_start = 5;
/**
* @brief WiringPi GPIO number of pin which turn ON/OFF laser of sensor
*/
const int GPIO_laser_enable = 4;
/**
* @brief Minimal configured distance of sensor
*/
const int range_min = 200;
/**
* @brief Maximal configured distance of sensor
*/
const int range_max = 10000;
/**
* @brief Value of sense resistor in ohms used for current loop
*/
const int sense_resistor = 150;
/**
* @brief Current coresponding to minimal distance, configured in "range_min"
*/
const float current_loop_min = 0.004;
/**
* @brief Current coresponding to maximal distance, configured in "range_max"
*/
const float current_loop_max = 0.020;
/**
* @brief Current coresponding to state when minimal distance is exceeded
*/
const float current_loop_close = 0.0035;
/**
* @brief Current coresponding to state when maximal distance is exceeded
*/
const float current_loop_far = 0.0205;
/**
* @brief Voltage coresponding to maximal distance
*/
const float voltage_max = current_loop_max*sense_resistor;
/**
* @brief Voltage coresponding to minimal distance
*/
const float voltage_min = current_loop_min*sense_resistor;
/**
* @brief Voltage coresponding to state when maximal distance is exceeded
*/
const float voltage_close = current_loop_close*sense_resistor;
/**
* @brief Voltage coresponding to state when minimal distance is exceeded
*/
const float voltage_far = current_loop_far*sense_resistor;
/**
* @brief Reference voltage of ADC
*/
const float reference_voltage = 3.308f;
public:
/**
* @brief Construct a new Sick_front object
* Configure GPIO and SPI interface
*
* @param channel Channel on which is sensor connected
*/
Sick_front(bool channel = 1);
/**
* @brief Calculate distance measurted by distance sensor
*
* @return optional<unsigned int> True and value in mm if in range, False otherwise
*/
optional<unsigned int> Distance();
/**
* @brief Average multiple measurements of distance sensor output
*
* @param measurement_count Amount of measurents to average
* @return optional<unsigned int> Same as Distance()
*/
optional<unsigned int> Distance_avg(unsigned int measurement_count = 8);
/**
* @brief Turn ON or OFF laser of sensor
*
* @param state New state of laser
*/
void Laser(bool state);
private:
/**
* @brief Read raw ADC data from ADC
*
* @return uint16_t raw ADc data in 2's complement format
*/
uint16_t Conversion_raw();
};

View File

@ -0,0 +1,103 @@
#include "sick_side.hpp"
Sick_side::Sick_side(){
serial_descriptor = serialOpen("/dev/ttyS0", 9600);
if(serial_descriptor == -1){
cout << "Cannot open serial line for communication with side distance sensor" << endl;
}
}
uint8_t Sick_side::ID(){
Send_data({static_cast<uint8_t>(commands::Read), 0x01, 0x00});
//while(Data_available() < 12);
// Readout data which are in buffer from transmittion
//Receive();
// Read real received data from sensor
if(auto responce = Receive()){
return (*responce)[2];
} else {
return 0x00;
}
}
void Sick_side::Laser(bool state){
if(state){
// ON
Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x03});
} else {
// OFF
Send_data({static_cast<uint8_t>(commands::Command), 0xa0, 0x02});
}
//Read responce to clear buffer
Receive();
}
optional<double> Sick_side::Distance(){
auto distance_data = Distance_data();
if(distance_data == 0x7fff){
// Out of measuring range
return {};
} else {
// Convert to 2'complement and then to mm
return static_cast<int16_t>(distance_data)/100.0;
}
}
uint16_t Sick_side::Distance_data(){
Send_data({static_cast<uint8_t>(commands::Command), 0xb0, 0x01});
if(auto responce = Receive()){
return (((*responce)[1]<<8) | (*responce)[2]);
} else {
return 0x7fff;
}
}
void Sick_side::Send_data(array<uint8_t, 3> data){
array<uint8_t, 6> packet = {static_cast<uint8_t>(signs::STX), data[0], data[1], data[2], static_cast<uint8_t>(signs::ETX), 0x00};
packet[5] = Calculate_BCC(packet);
Transmit(packet);
}
void Sick_side::Transmit(array<uint8_t, 6> data){
for(int i = 0; i < 6; i++){
serialPutchar(serial_descriptor, data[i]);
}
}
uint8_t Sick_side::Calculate_BCC(array<uint8_t, 6> data){
uint8_t BCC = 0;
// BCC is calculated only from bytes on positions 1,2,3
for(int byte_index = 1; byte_index < 4; byte_index++){
BCC ^= data[byte_index];
}
return BCC;
}
optional<array<uint8_t, 3>> Sick_side::Receive(){
while(Data_available() < 12);
// Readout data which are in buffer from transmittion
for(int i = 0; i < 6; i++){
serialGetchar(serial_descriptor);
}
// Receive true data
array<uint8_t, 6> received_data;
for(int i = 0; i < 6; i++){
received_data[i] = serialGetchar(serial_descriptor);
}
// Check for communication errors
if(static_cast<signs>(received_data[1]) == signs::NAK){
cout << error_codes[received_data[2]] << endl;
return {};
}
if(received_data[5] != Calculate_BCC(received_data)){
cout << "Received BCC is invalid" << endl;
return {};
}
return array<uint8_t, 3>({received_data[1], received_data[2], received_data[3]});
}
unsigned int Sick_side::Data_available(){
return serialDataAvail(serial_descriptor);
}

View File

@ -0,0 +1,119 @@
#pragma once
#include <iostream>
#include <array>
#include <optional>
#include <map>
#include <string>
#include <wiringSerial.h>
using namespace std;
/**
Type: OD1-B100C50A15
**/
class Sick_side{
int serial_descriptor = 0;
enum class signs: uint8_t{
STX = 0x02,
ETX = 0x03,
ACK = 0x06,
NAK = 0x15,
};
map<uint8_t, string> error_codes{
{0x02, "Address is invalid"},
{0x04, "BCC values is invalid"},
{0x05, "Invalid command"},
{0x06, "Invalid setting value (out of spec)"},
{0x07, "Invalid setting value (out of range)"}
};
enum class commands: uint8_t{
Command = 0x43, // Also used for reading out measured values
Write = 0x57,
Read = 0x52,
};
public:
/**
* @brief Construct a new Sick_side object
* Opens serial line and save file descriptor
*/
Sick_side();
/**
* @brief Read sensor ID from memory of sensor
* Used for connection check
*
* @return uint8_t Value 0x64, 0x00 if communication fails
*/
uint8_t ID();
/**
* @brief Turn ON or OFF laser of sensor
*
* @param state New state of laser
*/
void Laser(bool state);
/**
* @brief Read and calculate distance from distance sensor
* Distance is relative to sensors zero point
*
* @return optional<double> And and relative distance from zero point, false otherwise (Out of Range)
*/
optional<double> Distance();
private:
/**
* @brief Reads distance data from sensor register
*
* @return uint16_t Distance in 2's complement, range -5000 to 5000, coresponds to -50 mm to +50 mm
*/
uint16_t Distance_data();
/**
* @brief Transmit given data over serial to RS485 transmitter
*
* @param data Data to send via serial line, header and footer are added
*/
void Send_data(array<uint8_t, 3> data);
/**
* @brief Calculates BCC for given packet of data
* BCC is XOR of bytes on index 1,2,3, remaning bytes are ignored
*
* @param data Packet used for calculation
* @return uint8_t Values of BCC
*/
uint8_t Calculate_BCC(array<uint8_t, 6> data);
/**
* @brief Transmits data over UART to RS485 transmitter
* Data are also copied to input buffer due to half-duplex connection
*
* @param data Data to transmit
*/
void Transmit(array<uint8_t, 6> data);
/**
* @brief Data received from sensor
* Only usefull data are returned, without STX, ACK/NACK, BCC
*
* @return optional<array<uint8_t, 3>> True and value if communication is valid, False otherwise
*/
optional<array<uint8_t, 3>> Receive();
/**
* @brief Reads amopunt of data in input buffer
*
* @return unsigned int Number of bytes in input buffer
*/
unsigned int Data_available();
};

3
camera/runThisOnRPI.txt Normal file
View File

@ -0,0 +1,3 @@
gst-launch-1.0 libcamerasrc ! videoscale method=0 add-borders=false ! video/x-raw, colorimetry=bt709, format=NV12, width=720, height=1280, framerate=30/1 ! videoflip method=clockwise ! jpegenc ! tcpserversink host=0.0.0.0 port=5000

93
camera/server.py Normal file
View File

@ -0,0 +1,93 @@
import socket
import cv2
import numpy as np
import os
def variance_of_laplacian(image):
return cv2.Laplacian(image, cv2.CV_64F).var()
os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"]="rtsp_transport;tcp"
gstreamerstr = "tcpclientsrc host=192.168.1.110 port=5000 ! jpegdec ! videoconvert ! appsink"
def recvall(sock, count):
buf = b''
while count:
newbuf = sock.recv(count)
if not newbuf: return None
buf += newbuf
count -= len(newbuf)
return buf
TCP_IP = '192.168.1.2'
TCP_PORT = 5001
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('192.168.1.2', TCP_PORT))
s.listen(True)
conn, addr = s.accept()
#if not capture.isOpened() : print("CANNOT OPEN STREAM")
#capture = cv2.VideoCapture("vid.mp4")
keypressNo = 1
while(True):
capture = cv2.VideoCapture(gstreamerstr,cv2.CAP_GSTREAMER)
ret, frame = capture.read()
if not ret:
print('fail')
break
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
variance = variance_of_laplacian(frame)
cv2.putText(frame, "{}: {:.2f}".format("Variance:", variance), (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
cv2.imshow('frame',frame)
#cv2.waitKey(0)
if cv2.waitKey(70) == ord('v'):
conn.send("n".encode())
length = conn.recv(3)
stringData = recvall(conn, int(length))
stringData=stringData.decode()
print(stringData)
frames=[]
maxVarIdx = 0
maxVar = 0
for i in range(30):
ret, frame = capture.read()
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
frames.append(frame)
var = variance_of_laplacian(frame)
if var > maxVar:
maxVar = var
maxVarIdx = i
conn.send("n".encode())
length = conn.recv(3)
stringData = recvall(conn, int(length))
stringData=stringData.decode()
print(stringData)
cv2.waitKey(40)
#test to see if the image with the highest variance gets picked
#cv2.putText(frame, "{}: {:.2f}".format("Variance:", var), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
#writeString="imgtest"+str(keypressNo)+"-"+str(i)+".jpg"
#cv2.imwrite(writeString, frames[i])
writeString="/home/palko/Documents/Documents/Diplomka/src/online/img"+str(keypressNo)+".png"
fileString = "/home/palko/Documents/Documents/Diplomka/src/online/img" + str(keypressNo) + ".json"
myfile = open(fileString, "w")
a=myfile.write(stringData)
myfile.close()
cv2.imwrite(writeString, frames[maxVarIdx])
keypressNo=keypressNo+1
capture.release()
cv2.destroyAllWindows()

96
camera/server2.py Normal file
View File

@ -0,0 +1,96 @@
import socket
import cv2
import numpy as np
import os
def variance_of_laplacian(image):
return cv2.Laplacian(image, cv2.CV_64F).var()
os.environ["OPENCV_FFMPEG_CAPTURE_OPTIONS"]="rtsp_transport;tcp"
gstreamerstr = "tcpclientsrc host=192.168.1.110 port=5000 ! jpegdec ! videoconvert ! appsink"
def recvall(sock, count):
buf = b''
while count:
newbuf = sock.recv(count)
if not newbuf: return None
buf += newbuf
count -= len(newbuf)
return buf
TCP_IP = '192.168.1.2'
TCP_PORT = 5001
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('192.168.1.2', TCP_PORT))
s.listen(True)
conn, addr = s.accept()
#if not capture.isOpened() : print("CANNOT OPEN STREAM")
#capture = cv2.VideoCapture("vid.mp4")
keypressNo = 1
while(True):
capture = cv2.VideoCapture(gstreamerstr,cv2.CAP_GSTREAMER)
ret, frame = capture.read()
if not ret:
print('fail')
break
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
variance = variance_of_laplacian(frame)
cv2.putText(frame, "{}: {:.2f}".format("Variance:", variance), (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
cv2.imshow('frame',frame)
#cv2.waitKey(0)
while(True):
frames=[]
maxVarIdx = 0
maxVar = 0
conn.send("n".encode())
length = conn.recv(3)
stringData = recvall(conn, int(length))
stringData=stringData.decode()
print(stringData)
maxVar=0
var=0
for i in range(30):
ret, frame = capture.read()
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
frames.append(frame)
cv2.imshow("frame",frame)
var = variance_of_laplacian(frame)
if var > maxVar:
maxVar = var
maxVarIdx = i
conn.send("n".encode())
length = conn.recv(3)
stringData = recvall(conn, int(length))
stringData=stringData.decode()
print(stringData)
cv2.waitKey(40)
#test to see if the image with the highest variance gets picked
#cv2.putText(frame, "{}: {:.2f}".format("Variance:", var), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
#writeString="imgtest"+str(keypressNo)+"-"+str(i)+".jpg"
#cv2.imwrite(writeString, frames[i])
writeString="/home/palko/Documents/Documents/Diplomka/src/online/img"+str(keypressNo)+".png"
fileString = "/home/palko/Documents/Documents/Diplomka/src/online/img" + str(keypressNo) + ".json"
myfile = open(fileString, "w")
a=myfile.write(stringData)
myfile.close()
cv2.imwrite(writeString, frames[maxVarIdx])
keypressNo=keypressNo+1
capture.release()
cv2.destroyAllWindows()