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

add helix movement

Browse source
This commit is contained in:
Philipp Kramer 2023-01-10 15:13:55 +01:00
parent d7b6bb8c86
commit 89ebccd59f
2 changed files with 45 additions and 20 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

63
servo-control.cpp
View file

@ -7,17 +7,20 @@
#include "servo-control.hpp" #include "servo-control.hpp"
int angle_to_dxl_servo_position(const int deg); int angle_to_dxl_servo_position(const int deg);
int rpm_to_dxl_servo_speed(const int rpm); int rpm_to_dxl_servo_speed(const int rpm);
Mood mood; Mood mood;
float matrix_weights[SERVO_COUNT][4]; //mapping outputs, sensors/moods float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
#define W_PITCH 0 #define W_PITCH 0
#define W_ROLL 1 #define W_ROLL 1
#define W_NOISE 2 #define W_NOISE 2
#define W_NOISESLOW 3 #define W_NOISESLOW 3
#define W_SIN 4
#define W_COS 5
void matrix_weights_update(){ void matrix_weights_update(){
@ -26,31 +29,45 @@ void matrix_weights_update(){
float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
//float map_mode=constrain(mapfloat(mood.shakiness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
//testing on servo //testing on servo
matrix_weights[0][W_NOISE]=90.0*map_mode; matrix_weights[0][W_COS]=30 *map_mode;
matrix_weights[0][W_NOISESLOW]=90.0*map_mode; matrix_weights[0][W_NOISE]=90.0 *map_mode;
matrix_weights[0][W_ROLL]=1.0* (1-map_mode); matrix_weights[1][W_SIN]=30 *map_mode;
matrix_weights[1][W_NOISE]=90.0 *map_mode;
matrix_weights[2][W_COS]=-30 *map_mode;
matrix_weights[2][W_NOISE]=90.0 *map_mode;
matrix_weights[3][W_SIN]=-30 *map_mode;
matrix_weights[3][W_NOISE]=90.0 *map_mode;
matrix_weights[4][W_COS]=30 *map_mode;
matrix_weights[4][W_NOISE]=90.0 *map_mode;
matrix_weights[1][W_NOISE]=90.0*map_mode;
matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
matrix_weights[1][W_PITCH]=1.0* (1-map_mode);
matrix_weights[2][W_NOISE]=90.0*map_mode; matrix_weights[0][W_ROLL]=1.0* (1-map_mode);
matrix_weights[2][W_NOISESLOW]=90.0*map_mode; matrix_weights[1][W_PITCH]=1.0* (1-map_mode);
matrix_weights[2][W_ROLL]=1.0* (1-map_mode); matrix_weights[2][W_ROLL]=1.0* (1-map_mode);
matrix_weights[3][W_PITCH]=1.0* (1-map_mode);
matrix_weights[3][W_NOISE]=90.0*map_mode;
matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
matrix_weights[3][W_PITCH]=1.0* (1-map_mode);
matrix_weights[4][W_NOISE]=90.0*map_mode;
matrix_weights[4][W_NOISESLOW]=90.0*map_mode;
matrix_weights[4][W_ROLL]=1.0* (1-map_mode); matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
/*
matrix_weights[0][W_NOISE]=90.0*map_mode;
matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
matrix_weights[1][W_NOISE]=90.0*map_mode;
matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
matrix_weights[2][W_NOISE]=90.0*map_mode;
matrix_weights[2][W_NOISESLOW]=90.0*map_mode;
matrix_weights[3][W_NOISE]=90.0*map_mode;
matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
matrix_weights[4][W_NOISE]=90.0*map_mode;
matrix_weights[4][W_NOISESLOW]=90.0*map_mode;
*/
float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1); float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1);
@ -241,13 +258,21 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
//Serial.print(mood.shakiness); //Serial.print(mood.shakiness);
//Serial.print(", "); //Serial.print(", ");
//Serial.println(millis_add); //Serial.println(millis_add);
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++) {
float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
float pnoiseslow=PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves float pnoiseslow=PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
int angle=0; int angle=0;
angle=pitch*matrix_weights[i][W_PITCH]+roll*matrix_weights[i][W_ROLL]+pnoise*matrix_weights[i][W_NOISE]+pnoiseslow*matrix_weights[i][W_NOISESLOW]; angle=
pitch*matrix_weights[i][W_PITCH]
+roll*matrix_weights[i][W_ROLL]
+pnoise*matrix_weights[i][W_NOISE]
+pnoiseslow*matrix_weights[i][W_NOISESLOW]
+vsin*matrix_weights[i][W_SIN]
+vcos*matrix_weights[i][W_COS];
dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle))); dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));
} }
delay(1); delay(1);

2
servo-control.hpp
View file

@ -23,7 +23,7 @@ extern Mood mood;
#define ANGLE_PER_DIGIT_RATIO .2925 #define ANGLE_PER_DIGIT_RATIO .2925
//#define ANGLE_PER_DIGIT_RATIO 0.29 // From datasheet //#define ANGLE_PER_DIGIT_RATIO 0.29 // From datasheet
#define MOTOR_TORQUE_LIMIT 75 #define MOTOR_TORQUE_LIMIT 100
#define MOTOR_TORQUE_RATIO 0.1 #define MOTOR_TORQUE_RATIO 0.1
#define MOTOR_TORQUE_DIRECTION_MASK 0x00000200 #define MOTOR_TORQUE_DIRECTION_MASK 0x00000200
#define MOTOR_TORQUE_VALUE_MASK 0x000001ff #define MOTOR_TORQUE_VALUE_MASK 0x000001ff