diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3d0b5c7 --- /dev/null +++ b/.gitignore @@ -0,0 +1,127 @@ +### C++ template +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### CMake template +CMakeLists.txt.user +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +_deps + +### CLion template +# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider +# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 + +# User-specific stuff +.idea/**/workspace.xml +.idea/**/tasks.xml +.idea/**/usage.statistics.xml +.idea/**/dictionaries +.idea/**/shelf + +# AWS User-specific +.idea/**/aws.xml + +# Generated files +.idea/**/contentModel.xml + +# Sensitive or high-churn files +.idea/**/dataSources/ +.idea/**/dataSources.ids +.idea/**/dataSources.local.xml +.idea/**/sqlDataSources.xml +.idea/**/dynamic.xml +.idea/**/uiDesigner.xml +.idea/**/dbnavigator.xml + +# Gradle +.idea/**/gradle.xml +.idea/**/libraries + +# Gradle and Maven with auto-import +# When using Gradle or Maven with auto-import, you should exclude module files, +# since they will be recreated, and may cause churn. Uncomment if using +# auto-import. +# .idea/artifacts +# .idea/compiler.xml +# .idea/jarRepositories.xml +# .idea/modules.xml +# .idea/*.iml +# .idea/modules +# *.iml +# *.ipr + +# CMake +cmake-build-*/ +build/ + +# Mongo Explorer plugin +.idea/**/mongoSettings.xml + +# File-based project format +*.iws + +# IntelliJ +out/ + +# mpeltonen/sbt-idea plugin +.idea_modules/ + +# JIRA plugin +atlassian-ide-plugin.xml + +# Cursive Clojure plugin +.idea/replstate.xml + +# SonarLint plugin +.idea/sonarlint/ + +# Crashlytics plugin (for Android Studio and IntelliJ) +com_crashlytics_export_strings.xml +crashlytics.properties +crashlytics-build.properties +fabric.properties + +# Editor-based Rest Client +.idea/httpRequests + +# Android studio 3.1+ serialized cache file +.idea/caches/build_file_checksums.ser + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e796b03 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.25) +project(servo_motor) + +set(CMAKE_CXX_STANDARD 17) + +find_package(PkgConfig REQUIRED) +find_package(PiPCA9685 REQUIRED) +pkg_check_modules(TCPSocket REQUIRED TCPSocket) + +add_executable(servo_motor + MyTCPClient.cpp + MyTCPClient.h + main.cpp) + +target_link_libraries(servo_motor TCPSocket) +target_link_libraries(servo_motor ${PiPCA9685_LIBRARY}) diff --git a/MyTCPClient.cpp b/MyTCPClient.cpp new file mode 100644 index 0000000..6dc3fe3 --- /dev/null +++ b/MyTCPClient.cpp @@ -0,0 +1,234 @@ +#include "MyTCPClient.h" + +MyTCPClient::MyTCPClient(const char *serverIP, int port) : TCPClient(serverIP, port), pca() { + this->pwm_init(); +} + +void MyTCPClient::handleMessage(const std::string &message) { + std::vector token = TCPSocket::split(message, ";"); + + if (token.size() != 4) { + std::cerr << "Message invalide: " << message << std::endl; + return; + } + + if (token[1] == "servo_moteur" || token[1] == "all") { + std::cout << message << std::endl; + if (token[2] == "ping") { + this->sendMessage("servo_moteur;ihm;pong;1"); + } + else if (token[2] == "ouvrir pince") { + int pince = std::stoi(token[3]); + std::cout << "ouvrir pince : " << pince << std::endl; + this->ouvrir_pince(pince); + } + else if (token[2] == "fermer pince") { + int pince = std::stoi(token[3]); + + this->fermer_pince(pince); + } + else if (token[2] == "baisser bras") { + this->fermer_pince(0); + this->fermer_pince(2); + this->baisser_bras(); + } + else if(token[2] == "transport bras"){ + this->fermer_pince(0); + this->fermer_pince(2); + this->transport_bras(); + } + else if (token[2] == "lever bras") { + this->fermer_pince(0); + this->fermer_pince(2); + this->lever_bras(); + } + else if (token[2] == "check panneau") { + int bras = std::stoi(token[3]); + this->check_panneau(bras); + } + else if (token[2] == "uncheck panneau") { + int bras = std::stoi(token[3]); + this->uncheck_panneau(bras); + } else if (token[2] == "angle") { + std::vector args = TCPSocket::split(token[3], ","); + if (args.size() != 2) { + std::cerr << "Nombre d'arguments invalide" << std::endl; + return; + } + int servo = std::stoi(args[0]); + int angleDeg = static_cast(std::stof(args[1])); + this->pwm_setServoPosition(servo, angleDeg); + } else if (token[2] == "clear") { + this->pwm_clear(); + } + } +} + +void MyTCPClient::pwm_setFrequency(float freq) { + pca.set_pwm_freq(freq); +} + +void MyTCPClient::pwm_init() { + pwm_setFrequency(50.0); + this->fermer_pince(0, true); + this->fermer_pince(1, true); + this->fermer_pince(2, true); + this->baisser_bras();//this->baisser_bras(true); +} + + +void MyTCPClient::pwm_setServoPosition(int servo, int position) { + int on_time = SERVO_MIN + (SERVO_MAX - SERVO_MIN) * position / 180 - 1;//temps ou le servo est allumé par rapport à 4096 + pca.set_pwm(servo, 0, on_time); +} + +void MyTCPClient::baisser_bras() { + switch(this->positionBras){ + case BRAS_HAUT: + for(int i = angleBrasHaut.servo4; i <= angleBrasBas.servo4;i++){ + this->pwm_setServoPosition(4, i); + usleep(5'000); + this->pwm_setServoPosition(5, angleBrasHaut.servo5-i+angleBrasHaut.servo4); + } + break; + case BRAS_TRANSPORT: + for(int i = angleBrasTransport.servo4; i <= angleBrasBas.servo4;i++){ + this->pwm_setServoPosition(4, i); + usleep(5'000); + this->pwm_setServoPosition(5, angleBrasTransport.servo5-i+angleBrasTransport.servo4); + } + break; + case BRAS_BAS: + this->pwm_setServoPosition(4, angleBrasBas.servo4); + this->pwm_setServoPosition(5, angleBrasBas.servo5); + return; + } + this->positionBras = BRAS_BAS; + this->pwm_setServoPosition(4, angleBrasBas.servo4); + this->pwm_setServoPosition(5, angleBrasBas.servo5); +} + +void MyTCPClient::transport_bras(){ + switch(this->positionBras){ + case BRAS_BAS: + for (int i = angleBrasBas.servo5; i <= angleBrasTransport.servo5; i++){ + this->pwm_setServoPosition(4, angleBrasBas.servo4-i+angleBrasBas.servo5); + usleep(5'000); + this->pwm_setServoPosition(5, i); + } + break; + case BRAS_HAUT: + for (int i = angleBrasHaut.servo4; i <= angleBrasTransport.servo4; i++){ + this->pwm_setServoPosition(4, i); + usleep(5'000); + this->pwm_setServoPosition(5, angleBrasHaut.servo5-i+angleBrasHaut.servo4); + } + break; + case BRAS_TRANSPORT: + this->pwm_setServoPosition(4, angleBrasTransport.servo4); + this->pwm_setServoPosition(5, angleBrasTransport.servo5); + return; + } + this->positionBras = BRAS_TRANSPORT; + this->pwm_setServoPosition(4, angleBrasTransport.servo4); + this->pwm_setServoPosition(5, angleBrasTransport.servo5); +} + +void MyTCPClient::lever_bras() { + switch(this->positionBras){ + case BRAS_BAS: + for (int i = angleBrasBas.servo5;i <= angleBrasHaut.servo5;i++){ + this->pwm_setServoPosition(4, angleBrasBas.servo4-i+angleBrasBas.servo5); + usleep(5'000); + this->pwm_setServoPosition(5, i); + } + break; + case BRAS_TRANSPORT: + for (int i = angleBrasTransport.servo5;i <= angleBrasHaut.servo5;i++){ + this->pwm_setServoPosition(4, angleBrasTransport.servo4-i+angleBrasTransport.servo5); + usleep(5'000); + this->pwm_setServoPosition(5, i); + } + break; + case BRAS_HAUT: + this->pwm_setServoPosition(4, angleBrasHaut.servo4); + this->pwm_setServoPosition(5, angleBrasHaut.servo5); + return; + } + this->positionBras = BRAS_HAUT; + this->pwm_setServoPosition(4, angleBrasHaut.servo4); + this->pwm_setServoPosition(5, angleBrasHaut.servo5); +} + +void MyTCPClient::fermer_pince(int pince, bool force) { + if (!pinceOuverte[pince] && !force){ + return; + } + int angle, old_angle; + if (pince < 0 || pince > 2){ + return; + } + switch(pince){ + case 0: + angle = 142; + old_angle = 115; + break; + case 1: + angle = 42; + old_angle = 22; + break; + case 2: + angle = 152; + old_angle = 130; + break; + } + std::cout << "Fermer pince : " << pince << std::endl; + for(int i = old_angle; i <= angle;i++){ + this->pwm_setServoPosition(pince, i); + usleep(5'000); + } + pinceOuverte[pince] = false; +} + +void MyTCPClient::ouvrir_pince(int pince, bool force) { + if (pinceOuverte[pince] && !force){ + return; + } + int angle, old_angle; + if (pince < 0 || pince > 2){ + return; + } + switch(pince){ + case 0: + angle = 115; + old_angle = 142; + break; + case 1: + angle = 22; + old_angle = 42; + break; + case 2: + angle = 130; + old_angle = 152; + break; + } + std::cout << "Ouvrir pince : " << pince << std::endl; + for (int i = old_angle; i >= angle;i--){ + this->pwm_setServoPosition(pince, i); + usleep(5'000); + } + pinceOuverte[pince] = true; +} + + +void MyTCPClient::check_panneau(int quelBras) { + this->pwm_setServoPosition(quelBras, 30); +} + +void MyTCPClient::uncheck_panneau(int quelBras) { + this->pwm_setServoPosition(quelBras, 0); +} + +void MyTCPClient::pwm_clear() { + pca.set_all_pwm(0,0); +} diff --git a/MyTCPClient.h b/MyTCPClient.h new file mode 100644 index 0000000..810d472 --- /dev/null +++ b/MyTCPClient.h @@ -0,0 +1,63 @@ +#pragma once + +#include "TCPSocket/TCPClient.hpp" +#include "TCPSocket/TCPUtils.hpp" + +// #include "servo_motor.h" +#include +#include + +#define SERVO_MIN 82 // 0.02*4096 +#define SERVO_MAX 492 // 0.12*4096 + +enum BrasState { + BRAS_BAS, + BRAS_HAUT, + BRAS_TRANSPORT +}; + +typedef struct { + int servo4; + int servo5; +}angleBras; + +class MyTCPClient : public TCPClient { +public: + explicit MyTCPClient(const char* serverIP = "127.0.0.1", int port = 8080); + + void handleMessage(const std::string &message) override; + + void pwm_setFrequency(float freq); + + void pwm_init(); + + void pwm_setServoPosition(int servo, int position); + + // void baisser_bras(bool force = false); + + void baisser_bras(); + + void transport_bras(); + +// void lever_bras(bool force = false); + + void lever_bras(); + + void fermer_pince(int pince, bool force = false); + + void ouvrir_pince(int pince, bool force = false); + + void check_panneau(int quelBras); + + void uncheck_panneau(int quelBras); + + void pwm_clear(); + +private: + PiPCA9685::PCA9685 pca; + BrasState positionBras = BRAS_BAS; + bool pinceOuverte[3] = {true, true, true}; + angleBras angleBrasBas = {180, 0}; + angleBras angleBrasTransport = {168, 12}; + angleBras angleBrasHaut = {53, 127};//78, 102 +}; diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..a10dd43 --- /dev/null +++ b/main.cpp @@ -0,0 +1,20 @@ +#include "MyTCPClient.h" + +int main(int argc, char* argv[]) { + int port = 8080; + if (argc > 1) { + port = std::stoi(argv[1]); + } + + MyTCPClient client("127.0.0.1", port); + + client.start(); + + client.sendMessage("servo_moteur;strat;ready;1"); + + while (!client.shouldStop()) { + usleep(100'000); + } + + return 0; +} \ No newline at end of file diff --git a/servo_motor.c b/servo_motor.c deleted file mode 100644 index 5e069e3..0000000 --- a/servo_motor.c +++ /dev/null @@ -1,177 +0,0 @@ -#include -#include -#include - -//Define registers address -#define PCA9685_ADDR 0x40 -#define MODE1_REG 0x00 -#define PRE_SCALE 0xFE - -#define LED0_ON_L 0x06 -#define LED0_ON_H 0x07 -#define LED0_OFF_L 0x08 -#define LED0_OFF_H 0x09 - -//Define duty cycle (in a range of 12 bits = 4096) for PWM (min 2%, max 12%) -#define SERVO_MIN 82 // 0.02*4096 -#define SERVO_MAX 492 // 0.12*4096 - -// Fonction pour initialiser le PCA9685 -void initPCA9685(int handle) -{ - // Mettre le PCA9685 en mode de sommeil - i2cWriteByteData(handle, MODE1_REG, 0x10); - - // Configurer la fréquence PWM (par exemple, 50 Hz) - int prescale_val = 25000000 / (4096 * 50) - 1; - i2cWriteByteData(handle, PRE_SCALE, prescale_val); - - // Mettre le PCA9685 en mode de travail normal - i2cWriteByteData(handle, MODE1_REG, 0x00); -} - -// Fonction pour définir la position du servo -void setServoPosition(int handle, int channel, int position) -{ - if (position < 0 || position > 180){ - return; - } - int on_time = SERVO_MIN + (SERVO_MAX - SERVO_MIN) * position / 180 - 1;//temps ou le servo est allumé par rapport à 4096 - int on_time_lsb = on_time & ~(0b1111 << 8);// 8 premiers bits de poids faible du temps ou le servo est allumé - int on_time_msb = on_time >> 8;// 4 premiers bits poids fort de on_time - i2cWriteWordData(handle, LED0_ON_L + 4 * channel, 0); - i2cWriteByteData(handle, LED0_ON_H + 4* channel, 0); - i2cWriteByteData(handle, LED0_OFF_L + 4 * channel, on_time_lsb);//0xCD = 0.05*2^12-1 - if (0 < (on_time >> 8) && (on_time >> 8) < 256){ - i2cWriteByteData(handle, LED0_OFF_H + 4*channel, on_time_msb); - } - else { - i2cWriteByteData(handle, LED0_OFF_H +4*channel, 0); - } -} - -//Change l'angle du servo de maniere precise (jspr par pitié) -void changeServoPositionSynchro(int handle, int channel, int currentPosition, int targetPosition){ - if (targetPosition - currentPosition > 0){ - for (int i = currentPosition+1; i <= targetPosition; i++){ - setServoPosition(handle, channel, i); - gpioSleep(PI_TIME_RELATIVE, 0, 10000); - }//Fo bouger les servo en meme temps aaah donc rajouter les position et channel et info des autres servo - } -} - -//Fonctions pour le bras sur le canal 1 -void baisser_bras(int handle){ - int angle = 100; - for (int i = 1; i <= angle;i++){ - gpioSleep(PI_TIME_RELATIVE, 0, 10000); - setServoPosition(handle, 4, i); - setServoPosition(handle, 5, angle-i); - } -} - -void lever_bras(int handle){ - int angle = 107; - for (int i = 1; i <= angle;i++){ - gpioSleep(PI_TIME_RELATIVE, 0, 10000); - setServoPosition(handle, 4, angle-i); - setServoPosition(handle, 5, i); - } -} - - -//Fonction pour les pinces -void fermer_pince(int handle, int pince){ - int angle; - if (pince < 0 || pince > 2){ - return; - } - switch(pince){ - case 0: - angle = 142; - break; - case 1: - angle = 42; - break; - case 2: - angle = 152; - break; - } - setServoPosition(handle, pince, angle); -//10° de + pour fermer completement -} - -void ouvrir_pince(int handle, int pince){ - int angle; - if (pince < 0 || pince > 2){ - return; - } - switch(pince){ - case 0: - angle = 115; - break; - case 1: - angle = 22; - break; - case 2: - angle = 125; - break; - } - setServoPosition(handle, pince, angle); -} - -void check_panneau(int handle, int quelBras){//bras gauche = 7 bras droit = 6 - setServoPosition(handle, quelBras, 30); -} - -void uncheck_panneau(int handle, int quelBras){ - setServoPosition(handle, quelBras, 0); -} - -int main() -{ - int handle; - - if (gpioInitialise() < 0) - { - fprintf(stderr, "Impossible d'initialiser pigpio\n"); - return 1; - } - - handle = i2cOpen(1, PCA9685_ADDR, 0); - - if (handle < 0) - { - fprintf(stderr, "Impossible d'ouvrir la connexion I2C\n"); - gpioTerminate(); - return 1; - } - - initPCA9685(handle); - /*lever_bras(handle); - gpioSleep(PI_TIME_RELATIVE, 1, 0); - ouvrir_pince(handle, 0); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - ouvrir_pince(handle, 1); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - ouvrir_pince(handle, 2); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - fermer_pince(handle, 0); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - fermer_pince(handle, 1); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - fermer_pince(handle, 2); - gpioSleep(PI_TIME_RELATIVE, 1, 0); - baisser_bras(handle); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - */ - check_panneau(handle, 6); - gpioSleep(PI_TIME_RELATIVE, 1, 0); // Pause de 2 secondes - uncheck_panneau(handle, 6); - i2cClose(handle); - - // Terminaison de pigpio - gpioTerminate(); - - return 0; -} diff --git a/servo_motor.h b/servo_motor.h deleted file mode 100644 index adf92a9..0000000 --- a/servo_motor.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef SERVO_MOTOR -#define SERVO_MOTOR -#include -#include -#include - -//Define registers address -#define PCA9685_ADDR 0x40 -#define MODE1_REG 0x00 -#define PRE_SCALE 0xFE - -#define LED0_ON_L 0x06 -#define LED0_ON_H 0x07 -#define LED0_OFF_L 0x08 -#define LED0_OFF_H 0x09 - -//Define duty cycle (in a range of 12 bits = 4096) for PWM (min 2%, max 12%) -#define SERVO_MIN 82 // 0.02*4096 -#define SERVO_MAX 492 // 0.12*4096 - -void initPCA9685(int handle); - -void setServoPosition(int handle, int channel, int position); - -void changeServoPositionSynchro(int handle, int channel, int currentPosition, int targetPosition); - -void baisser_bras(int handle); - -void lever_bras(int handle); - -void fermer_pince(int handle, int pince); - -void ouvrir_pince(int handle, int pince); - -#endif diff --git a/servo_motor.h.gch b/servo_motor.h.gch deleted file mode 100644 index 0e53e22..0000000 Binary files a/servo_motor.h.gch and /dev/null differ diff --git a/servo_motor.o b/servo_motor.o deleted file mode 100644 index c3928c4..0000000 Binary files a/servo_motor.o and /dev/null differ