SSR_VERBOSEビルドオプションに対応 #26

ssr2022-saka-maza · Sep 6, 2022 · c9356af · c9356af
1 parent 4618ffa
commit c9356af
Show file tree

Hide file tree

Showing 9 changed files with 60 additions and 0 deletions.
diff --git a/include/ssr/SyncServo.hpp b/include/ssr/SyncServo.hpp
@@ -15,6 +15,7 @@
  */
 #define SSR_SYNC_SERVO_HPP
 
+#include <Arduino.h>
 #include <Servo.h>
 #include "ssr/PinType.hpp"
 #include "ssr/Input.hpp"

diff --git a/src/ssr/AnalogIn.cpp b/src/ssr/AnalogIn.cpp
@@ -8,5 +8,13 @@ void ssr::AnalogIn::begin() {
 }
 
 uint16_t ssr::AnalogIn::read() {
+    #ifdef    SSR_VERBOSE
+    int v = analogRead(pin);
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::AnalogIn] read value %d\n"), v);
+    Serial.print(buffer);
+    return v;
+    #else  /* SSR_VERBOSE */
     return analogRead(pin);
+    #endif /* SSR_VERBOSE */
 }
diff --git a/src/ssr/AnalogOut.cpp b/src/ssr/AnalogOut.cpp
@@ -12,6 +12,11 @@ void ssr::AnalogOut::begin(uint16_t value) {
 void ssr::AnalogOut::setValue(uint16_t value) {
     _value = value;
     analogWrite(pin, _value);
+    #ifdef SSR_VERBOSE
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::AnalogOut] set value as %d\n"), _value);
+    Serial.print(buffer);
+    #endif /* SSR_VERBOSE */
 }
 
 uint16_t ssr::AnalogOut::getValue() {

diff --git a/src/ssr/DigitalIn.cpp b/src/ssr/DigitalIn.cpp
@@ -8,5 +8,12 @@ void ssr::DigitalIn::begin() {
 }
 
 bool ssr::DigitalIn::read() {
+    #ifdef SSR_VERBOSE
+    bool v = digitalRead(pin);
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::DigitalIn] read value %d\n"), v);
+    Serial.print(buffer);
+    #else  /* SSR_VERBOSE */
     return (bool)digitalRead(pin);
+    #endif /* SSR_VERBOSE */
 }
diff --git a/src/ssr/DigitalOut.cpp b/src/ssr/DigitalOut.cpp
@@ -16,6 +16,11 @@ bool ssr::DigitalOut::getValue() {
 void ssr::DigitalOut::setValue(bool value) {
     _value = value;
     digitalWrite(pin, _value);
+    #ifdef SSR_VERBOSE
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::DigitalOut] set value as %d\n"), _value);
+    Serial.print(buffer);
+    #endif /* SSR_VERBOSE */
 }
 
 void ssr::DigitalOut::write(bool value) {

diff --git a/src/ssr/LowerBody.cpp b/src/ssr/LowerBody.cpp
@@ -26,6 +26,17 @@ void ssr::LowerBody::twist(float v_x, float v_y, float v_theta) {
     motor1.setPower((int16_t)v1);
     motor2.setPower((int16_t)v2);
     motor3.setPower((int16_t)v3);
+    #ifdef SSR_VERBOSE
+    char buffer[256] = "";
+    char * ptr = buffer;
+    ptr += snprintf_P(buffer, 200, PSTR("[ssr::LowerBody] set motor powers as "));
+    dtostrf(v1, 6, 2, ptr);       ptr += 6;
+    memcpy_P(ptr, PSTR(", "), 2); ptr += 2;
+    dtostrf(v2, 6, 2, ptr);       ptr += 6;
+    memcpy_P(ptr, PSTR(", "), 2); ptr += 2;
+    dtostrf(v3, 6, 2, ptr);       ptr += 6;
+    Serial.println(ptr);
+    #endif /* SSR_VERBOSE */
 }
 
 void ssr::LowerBody::parallel(float v_x, float v_y) {

diff --git a/src/ssr/MotorDriver.cpp b/src/ssr/MotorDriver.cpp
@@ -17,6 +17,11 @@ int16_t ssr::MotorDriver::getPower() {
 void ssr::MotorDriver::setPower(int16_t power) {
     dir.setValue(power >= 0);
     pwm.setValue(abs(power));
+    #ifdef SSR_VERBOSE
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::MotorDriver] set motor power as %d\n"), power);
+    Serial.print(buffer);
+    #endif /* SSR_VERBOSE */
 }
 
 void ssr::MotorDriver::write(int16_t value) {

diff --git a/src/ssr/Solenoid.cpp b/src/ssr/Solenoid.cpp
@@ -9,9 +9,19 @@ void ssr::Solenoid::begin() {
 void ssr::Solenoid::update() {
     if (_pin.getValue()) {
         _pin.write(false);
+        #ifdef SSR_VERBOSE
+        char buffer[256] = "";
+        snprintf_P(buffer, 200, PSTR("[ssr::Solenoid] chill\n"));
+        Serial.print(buffer);
+        #endif /* SSR_VERBOSE */
     }
 }
 
 void ssr::Solenoid::fire() {
     _pin.write(true);
+    #ifdef SSR_VERBOSE
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::Solenoid] fire\n"));
+    Serial.print(buffer);
+    #endif /* SSR_VERBOSE */
 }
diff --git a/src/ssr/SyncServo.cpp b/src/ssr/SyncServo.cpp
@@ -13,8 +13,16 @@ uint16_t ssr::SyncServo::attach(ssr::PinType pin1, ssr::PinType pin2) {
 }
 
 void ssr::SyncServo::write(uint8_t value) {
+    #ifdef SSR_VERBOSE
+    uint8_t value2 = mirrored ? 180 - value : value;
+    _servo1.write(value);
+    _servo2.write(value2);
+    char buffer[256] = "";
+    snprintf_P(buffer, 200, PSTR("[ssr::SyncServo] wrote %d to servo1, %d to servo2"), value, value2);
+    #else
     _servo1.write(value);
     _servo2.write(mirrored ? 180 - value : value);
+    #endif /* SSR_VERBOSE */
 }
 
 int16_t ssr::SyncServo::read() {