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

add position read

Browse source
This commit is contained in:
Philipp Kramer 2023-01-12 10:36:21 +01:00
parent 7da708ecea
commit c156ba7b03
3 changed files with 15 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
protesis_03_dxl.ino
View file

@ -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

11
servo-control.cpp
View file

@ -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;

2
servo-control.hpp
View file

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