philipp.kramer/haustorial
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

add servo position constraints

Browse source
This commit is contained in:
Philipp Kramer 2024-09-12 09:38:28 +02:00
parent 04465fc867
commit 705914f91c
1 changed files with 7 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
prosthesis_controller/servo.cpp
View file

@ -18,6 +18,8 @@ void initServos()
servos[0].mode=OP_CURRENT_BASED_POSITION; servos[0].mode=OP_CURRENT_BASED_POSITION;
servos[0].current=10.0; servos[0].current=10.0;
servos[0].offset_angle=180; servos[0].offset_angle=180;
servos[0].min_angle=180-90;
servos[0].max_angle=180+90;
servos[0].orientation=1; servos[0].orientation=1;
servos[0].modelnumber=1020; servos[0].modelnumber=1020;
@ -25,6 +27,8 @@ void initServos()
servos[1].id=12; servos[1].id=12;
servos[1].mode=OP_POSITION; servos[1].mode=OP_POSITION;
servos[1].offset_angle=180; servos[1].offset_angle=180;
servos[1].min_angle=180-90;
servos[1].max_angle=180+90;
servos[1].orientation=1; servos[1].orientation=1;
servos[1].modelnumber=1080; servos[1].modelnumber=1080;
@ -32,6 +36,8 @@ void initServos()
servos[2].id=13; servos[2].id=13;
servos[2].mode=OP_POSITION; servos[2].mode=OP_POSITION;
servos[2].offset_angle=180; servos[2].offset_angle=180;
servos[2].min_angle=180-90;
servos[2].max_angle=180+90;
servos[2].orientation=1; servos[2].orientation=1;
servos[2].modelnumber=1080; servos[2].modelnumber=1080;
@ -108,7 +114,7 @@ void loopServos(unsigned long millis) {
if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){ if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){
last_servo_update=millis; last_servo_update=millis;
dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); //asdf dxl.setGoalPosition(servos[servoUpdateI].id, constrain((servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation) , servos[servoUpdateI].min_angle,servos[servoUpdateI].max_angle), UNIT_DEGREE); //asdf
servoUpdateI++; servoUpdateI++;
servoUpdateI%=SERVO_COUNT; servoUpdateI%=SERVO_COUNT;
} }