philipp.kramer/protesis_03
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

add serial position sending

Browse source
This commit is contained in:
Philipp Kramer 2023-01-12 12:18:09 +01:00
parent c156ba7b03
commit 5c14bf7e7a
2 changed files with 34 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

44
protesis_03_dxl.ino
View file

@ -18,7 +18,6 @@ inertia previous_mpu_inertia;
measurement_double_precission previous_mpu_measurement_dp; measurement_double_precission previous_mpu_measurement_dp;
measurement_double_precission current_mpu_measurement_dp; measurement_double_precission current_mpu_measurement_dp;
unsigned long current_timestamp_us;
unsigned long loop_counter = 0; unsigned long loop_counter = 0;
int servo_id; int servo_id;
int roll = 125; int roll = 125;
@ -45,7 +44,7 @@ void print_servo_torques();
void setup() { void setup() {
Serial.begin(115200); Serial.begin(57600);
Serial.println("booting"); Serial.println("booting");
@ -65,7 +64,6 @@ void setup() {
servos_in_position_wait(); servos_in_position_wait();
pinMode(switch_programm, INPUT_PULLUP); pinMode(switch_programm, INPUT_PULLUP);
current_timestamp_us = micros();
@ -75,27 +73,33 @@ void setup() {
} }
void loop() { 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)); //move_protesis_with_sensor_data(); //old movement function
if(state_switch_programm == 1) { //slither();
slither();
} else { update_mood();
//move_protesis_with_sensor_data();
//update_mood();
move_protesis_organic(); 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() { void update_mood() {
@ -105,11 +109,12 @@ void update_mood() {
#define GYROSCOPE_FILTER_WAKEFULNESS 0.99 #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.); 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(", ");
Serial.print(mood.shakiness); Serial.print(mood.shakiness);
Serial.print(", "); Serial.print(", ");
Serial.println(mood.wakefulness); Serial.println(mood.wakefulness);
*/
matrix_weights_update(); matrix_weights_update();
} }
@ -258,11 +263,12 @@ void print_mpu_offset() {
Serial.println("]"); Serial.println("]");
} }
void print_loop_time_micros() { /*void print_loop_time_micros() {
Serial.print(loop_counter++); Serial.print(loop_counter++);
Serial.print(" diff ms: "); Serial.print(" diff ms: ");
Serial.println(micros()-current_timestamp_us); Serial.println(micros()-current_timestamp_us);
} }
*/
void print_servo_torques() { void print_servo_torques() {
servos_print_current_torque(); servos_print_current_torque();

11
servo-control.cpp
View file

@ -34,7 +34,7 @@ void matrix_weights_update(){
matrix_weights[0][W_COS]=40.0; matrix_weights[0][W_COS]=40.0;
matrix_weights[1][W_SIN]=20.0; matrix_weights[1][W_SIN]=-20.0;
//testing on servo //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() void read_servos_positions()
{ {
Serial.print("dxlgp");
for (uint8_t i=0;i<SERVO_COUNT;i++) { 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();
} }