Add analog servo control

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) {

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();
+  }
+}

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;

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

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();
+}

esp/lib/Servo/servo.h

@@ -0,0 +1,22 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include <Arduino.h>
+
+#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

esp/matty-v4.txt → 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: