From 5c14bf7e7ac98ffc03e234924b025c90c673e7ac Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Thu, 12 Jan 2023 12:18:09 +0100
Subject: [PATCH] add serial position sending

---
 protesis_03_dxl.ino | 44 +++++++++++++++++++++++++-------------------
 servo-control.cpp   | 11 +++++++++--
 2 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/protesis_03_dxl.ino b/protesis_03_dxl.ino
index cc36426..5051884 100644
--- a/protesis_03_dxl.ino
+++ b/protesis_03_dxl.ino
@@ -18,7 +18,6 @@ inertia previous_mpu_inertia;
 measurement_double_precission previous_mpu_measurement_dp;
 measurement_double_precission current_mpu_measurement_dp;
 
-unsigned long current_timestamp_us;
 unsigned long loop_counter = 0;
 int servo_id;
 int roll = 125;
@@ -45,7 +44,7 @@ void print_servo_torques();
 
 
 void setup() {
-	Serial.begin(115200);
+	Serial.begin(57600);
   
   Serial.println("booting");
   
@@ -65,7 +64,6 @@ void setup() {
 	servos_in_position_wait();
 	pinMode(switch_programm, INPUT_PULLUP);
 	
-	current_timestamp_us = micros();
 
 
 
@@ -75,27 +73,33 @@ void setup() {
 }
 
 void loop() {
+  unsigned long loopmillis=millis();
+  static unsigned long last_loop_movement_millis;
+  static unsigned long last_loop_readpos_millis;
 
+  static unsigned long last_servo_send_millis;
 
-  int state_switch_programm = digitalRead(switch_programm);
+  if (loopmillis-last_loop_movement_millis>50) {
+    last_loop_movement_millis=loopmillis;
+    //int state_switch_programm = digitalRead(switch_programm);  //currently not used
+    //Serial.println(digitalRead(switch_programm));
   
-  //Serial.println(digitalRead(switch_programm));
-	if(state_switch_programm == 1) {
-    slither();
-	} else {
-    //move_protesis_with_sensor_data();
-    //update_mood();
+    //move_protesis_with_sensor_data(); //old movement function
+    //slither();
+
+    update_mood();
     move_protesis_organic();
-   
-	}
+    last_servo_send_millis=millis();
 
-  read_servos_positions(); //takes around 8000us for all servos
+    //Serial.println("moved servos");
+  }
+
+  if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions
+    last_loop_readpos_millis=loopmillis;
+    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
-
 }
 
 void update_mood() {  
@@ -105,11 +109,12 @@ void update_mood() {
   #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
   mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
   
-  Serial.print(gyro);
+  /*Serial.print(gyro);
   Serial.print(", ");
   Serial.print(mood.shakiness);
   Serial.print(", ");
   Serial.println(mood.wakefulness);  
+  */
 
   matrix_weights_update();
 }
@@ -258,11 +263,12 @@ void print_mpu_offset() {
 	Serial.println("]");
 }
 
-void print_loop_time_micros() {
+/*void print_loop_time_micros() {
 	Serial.print(loop_counter++);
 	Serial.print(" diff ms: ");
 	Serial.println(micros()-current_timestamp_us);
 }
+*/
 
 void print_servo_torques() {
 	servos_print_current_torque();
diff --git a/servo-control.cpp b/servo-control.cpp
index de392f9..b38cd7f 100644
--- a/servo-control.cpp
+++ b/servo-control.cpp
@@ -34,7 +34,7 @@ void matrix_weights_update(){
   
   
   matrix_weights[0][W_COS]=40.0;
-  matrix_weights[1][W_SIN]=20.0;
+  matrix_weights[1][W_SIN]=-20.0;
   
 
   //testing on servo
@@ -292,9 +292,16 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
 
 void read_servos_positions()
 {
+  Serial.print("dxlgp");
   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));
+    //Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
+    
+    Serial.print(",");
+    
+    Serial.print(dxlGetPosition(servos[i].id)); 
+    
   }
+  Serial.println();
 }