From c156ba7b0340794e2531abf5f381e7fc171c7206 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Thu, 12 Jan 2023 10:36:21 +0100
Subject: [PATCH] add position read

---
 protesis_03_dxl.ino |  3 +++
 servo-control.cpp   | 11 ++++++++++-
 servo-control.hpp   |  2 ++
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/protesis_03_dxl.ino b/protesis_03_dxl.ino
index 9db2081..cc36426 100644
--- a/protesis_03_dxl.ino
+++ b/protesis_03_dxl.ino
@@ -89,6 +89,9 @@ void loop() {
    
 	}
 
+  read_servos_positions(); //takes around 8000us for all servos
+  
+
 	while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
 	print_loop_time_micros();
 	current_timestamp_us = micros();                      //Reset the loop timer
diff --git a/servo-control.cpp b/servo-control.cpp
index 6e30c4e..de392f9 100644
--- a/servo-control.cpp
+++ b/servo-control.cpp
@@ -34,6 +34,7 @@ void matrix_weights_update(){
   
   
   matrix_weights[0][W_COS]=40.0;
+  matrix_weights[1][W_SIN]=20.0;
   
 
   //testing on servo
@@ -264,7 +265,7 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
   float vsin=sin((millis()+millis_add)/1000.0);
   float vcos=cos((millis()+millis_add)/1000.0);
   
-  for (uint8_t i=0;i<=SERVO_COUNT;i++) {
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
     //unsigned long blastart=micros();
     float pnoise=0;//PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
     float pnoiseslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
@@ -289,6 +290,14 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
 }
 
 
+void read_servos_positions()
+{
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
+  }
+}
+
+
 int servo_calculate_torque_with_direction(const int servo_id) {
 	int current_torque = dxlGetTorque(servo_id);
 	int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK;
diff --git a/servo-control.hpp b/servo-control.hpp
index 4f25299..c61fa59 100644
--- a/servo-control.hpp
+++ b/servo-control.hpp
@@ -112,4 +112,6 @@ void map_pitch_to_servos_with_initial_position_and_move(const int pitch);
 
 void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch);
 
+void read_servos_positions();
+
 #endif /* SERVO_CONTROL_HPP_ */