add position read

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

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;

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_ */