diff --git a/esp/Robot/src/main.cpp b/esp/Robot/src/main.cpp index 33a48a7..ced53bc 100644 --- a/esp/Robot/src/main.cpp +++ b/esp/Robot/src/main.cpp @@ -1,4 +1,4 @@ -#include "data.h" + #include "data.h" #include "robot.h" #include "communication.h" #include "gps.h" @@ -65,6 +65,8 @@ void receiveCommand() { break; case 'P': gps.config(comm.rxData.mode); break; + case 'R': robot.reset(); + break; case 'V': { // send current version uint8_t buffer[2]; @@ -74,7 +76,7 @@ void receiveCommand() { comm.send('V', buffer, length); break; } - case 'R': robot.reset(); + case 'C': robot.setServo(comm.rxData.period); // container break; } if (robot.mode == AUTONOMOUS) { diff --git a/esp/Robot/src/robot.cpp b/esp/Robot/src/robot.cpp index 3e512ca..3b237c4 100644 --- a/esp/Robot/src/robot.cpp +++ b/esp/Robot/src/robot.cpp @@ -4,6 +4,7 @@ uint8_t idServo[SERVOS] = {1, 2, 3, 4}; AS5600 as5600(ZERO_JOINT); // zero joint position Power power; +Servo servo; Robot::Robot(Stream* stSerial) : Sts(stSerial) { } @@ -99,7 +100,7 @@ void Robot::control() { // TODO dodelat omezeni maximalni rychlosti -// Serial.printf("steer: %5.1f f0: %5.1f dfi: %5.2f ro: %5.1f ro1: %5.1f s: %5.1f s1: %5.1f dv: %5.1f dw: %5.1f\n\r", steer, f0, dfi * 180 / PI, ro, ro1, s, s1, dv, dw); +// Serial.printf("steer: %5.1f f0: %5.1f dfi: %5.2f ro: %5.1f ro1: %5.1f s: %5.1f s1: %5.1f dv: %5.1f dw: %5.1f\n\r", steer, joint, dfi * 180 / PI, ro, ro1, s, s1, dv, dw); int16_t p[SERVOS]; p[0] = -(vl + dv - dw) * LINE; @@ -190,3 +191,12 @@ void Robot::setLimits(uint16_t maxSpeed, uint16_t maxAngleSpeed) { // nastavi ma if (maxSpeed <= SPEED_MAX) v_max = maxSpeed; if (maxAngleSpeed <= STEER_MAX) a_max = maxAngleSpeed; } + +void Robot::setServo(uint16_t position) { + if (position > 0) { + servo.attach(SERVO_PIN); + servo.write(position); + } else { + servo.detach(); + } +} diff --git a/esp/Robot/src/robot.h b/esp/Robot/src/robot.h index 9676bd2..0d933d4 100644 --- a/esp/Robot/src/robot.h +++ b/esp/Robot/src/robot.h @@ -6,6 +6,7 @@ #include "as5600.h" #include "power.h" #include "sts.h" +#include "servo.h" #define STOP 0 #define REMOTE_CONTROL 1 @@ -33,6 +34,7 @@ class Robot : public Sts { void setTime(uint16_t p, uint16_t t); // nastavi periodu pro odomerii a odesilani dat, timeout pro automaticke zastaveni, pokud neprijde novy povel G void setLimits(uint16_t maxSpeed, uint16_t maxAngleSpeed); // nastavi maximalni rychlosti void updateSystem(); + void setServo(uint16_t position); float x, y, a; // aktualni pozice robota [mm, mm, rad] uint8_t status; diff --git a/esp/common/config.h b/esp/common/config.h index 4faa7eb..39b4997 100644 --- a/esp/common/config.h +++ b/esp/common/config.h @@ -10,7 +10,7 @@ // ROBOT // ======================================================================== -#define ROBOT_FW_VERSION 6 // please increase with every change of Robot project (or common library) +#define ROBOT_FW_VERSION 7 // please increase with every change of Robot project (or common library) #define ROBOT_TIMEOUT 250 // timeout do automatickeho zastaveni, pokud neprijde povel G #define CONTROL_PERIOD 20 // perioda rizeni 20 ms (50 Hz) @@ -32,7 +32,7 @@ #endif #if (MATTY == 02) - #define ZERO_JOINT -178.15f // nulova poloha kloubu Matty M02 - Martin Dlouhy + #define ZERO_JOINT -178.15f // nulova poloha kloubu Matty M02 - Martin Dlouhy #define L 320 // rozvor #define A 315 // rozchod #define D 135 // prumer kol @@ -56,7 +56,7 @@ #define LINE (1 / STEP) // steps/mm // ======================================================================== -// SERVO +// SERVO ST // ======================================================================== // the uart used to control servos. @@ -78,8 +78,14 @@ // ======================================================================== #define SerialGPS Serial2 /* UART2 */ -#define GPS_RXD 16 // zelena / GPS16 / B_C2 -#define GPS_TXD 27 // modra / GPS17 / B_C1 +#define GPS_RXD 16 // zelena / GPIO16 / B_C2 +#define GPS_TXD 27 // modra / GPIO17 / B_C1 #define GPS_BAUDRATE 9600L +// ======================================================================== +// SERVO +// ======================================================================== + +#define SERVO_PIN 4 // servo GPIO4 + #endif diff --git a/esp/lib/Servo/servo.cpp b/esp/lib/Servo/servo.cpp new file mode 100644 index 0000000..645ace9 --- /dev/null +++ b/esp/lib/Servo/servo.cpp @@ -0,0 +1,25 @@ +#include "servo.h" + +Servo::Servo(uint8_t channel) { + this->channel = channel; + ledcSetup(channel, PWM_FREQ, PWM_RESOLUTION); // nastaveni PWM vystupu +// write(1500); +} + +void Servo::attach(uint8_t pin) { + this->pin = pin; + ledcAttachPin(pin, channel); +} + +void Servo::detach() { + ledcDetachPin(pin); +} + +void Servo::write(int pos) { + pos = 65536 * pos / 20000; + ledcWrite(channel, pos); // nastaveni vystupni hodnoty na vystup +} + +Servo::~Servo() { + detach(); +} diff --git a/esp/lib/Servo/servo.h b/esp/lib/Servo/servo.h new file mode 100644 index 0000000..6f2ff53 --- /dev/null +++ b/esp/lib/Servo/servo.h @@ -0,0 +1,22 @@ +#ifndef SERVO_H +#define SERVO_H + +#include + +#define PWM_CHANNEL 1 +#define PWM_FREQ 50 +#define PWM_RESOLUTION 16 + +class Servo { + public: + Servo(uint8_t channel = PWM_CHANNEL); + void attach(uint8_t pin); + void detach(); + void write(int pos); + ~Servo(); + private: + uint8_t channel; + uint8_t pin; +}; + +#endif \ No newline at end of file diff --git a/esp/matty-v4.txt b/esp/matty-v5.txt similarity index 96% rename from esp/matty-v4.txt rename to esp/matty-v5.txt index 08c0557..a66102b 100644 --- a/esp/matty-v4.txt +++ b/esp/matty-v5.txt @@ -21,7 +21,8 @@ counter command data T uint16 period (ms) uint16 timeout (ms) ... nastavení periody posílání dat z robota a timeout pro příjem povelů P uint8 mode ... nastavení režimu gps (0 - vypnuto, 1 - surová data, 2 - ...) V ... version - + C uint16_t ... analogové servo na pinu GPIO4 (šířka pulzu 1000 ... 2000 us) + ESP -> PC potvrzení přijatého příkazu DATA: