implement more mood control
This commit is contained in:
parent
c1a5d8e2e4
commit
8d5c40deb3
3 changed files with 78 additions and 47 deletions
|
|
@ -41,7 +41,7 @@ void print_mpu_gyroscope_data();
|
|||
void print_mpu_offset();
|
||||
void print_loop_time_micros();
|
||||
void print_servo_torques();
|
||||
|
||||
void readMPU();
|
||||
|
||||
|
||||
void setup() {
|
||||
|
|
@ -88,6 +88,7 @@ void loop() {
|
|||
//move_protesis_with_sensor_data(); //old movement function
|
||||
//slither();
|
||||
|
||||
readMPU();
|
||||
update_mood();
|
||||
move_protesis_organic();
|
||||
last_servo_send_millis=millis();
|
||||
|
|
@ -97,7 +98,7 @@ void loop() {
|
|||
|
||||
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. read and send over serial.
|
||||
//read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -110,12 +111,24 @@ void update_mood() {
|
|||
#define GYROSCOPE_FILTER_WAKEFULNESS 0.99
|
||||
mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
|
||||
|
||||
/*Serial.print(gyro);
|
||||
double roll=current_mpu_inertia.roll_acceleration;
|
||||
double pitch=current_mpu_inertia.pitch_acceleration;
|
||||
double rollmapped=constrain( mapfloat(abs(roll),5,10,0,1),0,1);
|
||||
double pitchmapped=constrain( mapfloat(abs(pitch),5,10,0,1),0,1);
|
||||
|
||||
#define LONELINESS_FILTER 0.99
|
||||
mood.loneliness= constrain( mood.loneliness*LONELINESS_FILTER + (1-(rollmapped+pitchmapped)/2)*(1-LONELINESS_FILTER) ,0.0,1.0);
|
||||
|
||||
/*
|
||||
Serial.print(gyro);
|
||||
Serial.print(", ");
|
||||
Serial.print(mood.shakiness);
|
||||
Serial.print(", ");
|
||||
Serial.println(mood.wakefulness);
|
||||
Serial.print(mood.wakefulness);
|
||||
Serial.print(", ");
|
||||
Serial.println(mood.loneliness);
|
||||
*/
|
||||
|
||||
|
||||
matrix_weights_update();
|
||||
}
|
||||
|
|
@ -142,11 +155,15 @@ void move_protesis_with_sensor_data() {
|
|||
//print_servo_torques();
|
||||
}
|
||||
|
||||
void move_protesis_organic() {
|
||||
void readMPU() {
|
||||
read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
|
||||
calculate_inertia(current_mpu_inertia, current_mpu_measurement);
|
||||
correct_inertia_drift(current_mpu_inertia);
|
||||
denoise_inertia_with_complementary_filter();
|
||||
}
|
||||
|
||||
void move_protesis_organic() {
|
||||
|
||||
//substract_gyroscope_offset(current_mpu_measurement);
|
||||
//mpu_measurement_with_precission(current_mpu_measurement_dp, current_mpu_measurement);
|
||||
//denoise_mpu_measurement_with_complementary_filter();
|
||||
|
|
|
|||
|
|
@ -30,62 +30,76 @@ void matrix_weights_update(){
|
|||
//1=roll
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
float map_mode_noise=constrain(mapfloat(mood.wakefulness,0,0.2, 0,1),0,1);
|
||||
float map_mode_rollpitch=constrain(mapfloat(mood.shakiness,0,0.5, 0,1),0,1);
|
||||
float map_mode_slither=0;
|
||||
float map_mode_sleeping=mood.loneliness;
|
||||
|
||||
|
||||
|
||||
|
||||
//testing on servo
|
||||
|
||||
matrix_weights[0][W_COS]=30 *map_mode;
|
||||
matrix_weights[0][W_NOISE]=90.0 *map_mode;
|
||||
matrix_weights[0][W_NOISESLOW]=0.0 *map_mode;
|
||||
matrix_weights[1][W_SIN]=30 *map_mode;
|
||||
matrix_weights[1][W_NOISE]=70.0 *map_mode;
|
||||
matrix_weights[1][W_NOISESLOW]=20.0 *map_mode;
|
||||
matrix_weights[2][W_COS]=-30 *map_mode;
|
||||
matrix_weights[2][W_NOISE]=50.0 *map_mode;
|
||||
matrix_weights[2][W_NOISESLOW]=40.0 *map_mode;
|
||||
matrix_weights[3][W_SIN]=-30 *map_mode;
|
||||
matrix_weights[3][W_NOISE]=20.0 *map_mode;
|
||||
matrix_weights[3][W_NOISESLOW]=70.0 *map_mode;
|
||||
matrix_weights[4][W_COS]=30 *map_mode;
|
||||
matrix_weights[4][W_NOISE]=0.0 *map_mode;
|
||||
matrix_weights[4][W_NOISESLOW]=60.0 *map_mode;
|
||||
|
||||
float map_sum=map_mode_noise+map_mode_rollpitch+map_mode_slither+map_mode_sleeping;
|
||||
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
||||
map_mode_rollpitch/=map_sum;
|
||||
map_mode_slither/=map_sum;
|
||||
map_mode_sleeping/=map_sum;
|
||||
|
||||
matrix_weights[0][W_ROLL]=1.0* (1-map_mode);
|
||||
matrix_weights[1][W_PITCH]=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[4][W_ROLL]=1.0* (1-map_mode);
|
||||
|
||||
Serial.print(map_mode_noise); Serial.print(",");
|
||||
Serial.print(map_mode_rollpitch); Serial.print(",");
|
||||
Serial.print(map_mode_slither); Serial.print(",");
|
||||
Serial.print(map_mode_sleeping); Serial.println();
|
||||
|
||||
//Mode Noise
|
||||
matrix_weights[0][W_NOISE]=90.0 *map_mode_noise;
|
||||
matrix_weights[0][W_NOISESLOW]=0.0 *map_mode_noise;
|
||||
|
||||
matrix_weights[1][W_NOISE]=70.0 *map_mode_noise;
|
||||
matrix_weights[1][W_NOISESLOW]=20.0 *map_mode_noise;
|
||||
|
||||
matrix_weights[2][W_NOISE]=50.0 *map_mode_noise;
|
||||
matrix_weights[2][W_NOISESLOW]=40.0 *map_mode_noise;
|
||||
|
||||
matrix_weights[3][W_NOISE]=20.0 *map_mode_noise;
|
||||
matrix_weights[3][W_NOISESLOW]=70.0 *map_mode_noise;
|
||||
|
||||
matrix_weights[4][W_NOISE]=0.0 *map_mode_noise;
|
||||
matrix_weights[4][W_NOISESLOW]=60.0 *map_mode_noise;
|
||||
|
||||
//Mode Roll Pitch
|
||||
matrix_weights[0][W_ROLL]=1.0* map_mode_rollpitch;
|
||||
matrix_weights[1][W_PITCH]=1.0* map_mode_rollpitch;
|
||||
matrix_weights[2][W_ROLL]=1.0* map_mode_rollpitch;
|
||||
matrix_weights[3][W_PITCH]=1.0* map_mode_rollpitch;
|
||||
matrix_weights[4][W_ROLL]=1.0* map_mode_rollpitch;
|
||||
|
||||
//Mode Slither
|
||||
matrix_weights[0][W_COS]=30 *map_mode_slither;
|
||||
matrix_weights[1][W_SIN]=30 *map_mode_slither;
|
||||
matrix_weights[2][W_COS]=-30 *map_mode_slither;
|
||||
matrix_weights[3][W_SIN]=-30 *map_mode_slither;
|
||||
matrix_weights[4][W_COS]=30 *map_mode_slither;
|
||||
|
||||
//Mode Sleeping
|
||||
matrix_weights[0][W_NOISESLOW]=5 *map_mode_sleeping;
|
||||
matrix_weights[1][W_NOISESLOW]=10 *map_mode_sleeping;
|
||||
matrix_weights[2][W_NOISESLOW]=20 *map_mode_sleeping;
|
||||
matrix_weights[3][W_NOISESLOW]=15 *map_mode_sleeping;
|
||||
matrix_weights[4][W_NOISESLOW]=10 *map_mode_sleeping;
|
||||
|
||||
/*
|
||||
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.1, 0,1),0,1);
|
||||
for(int i = 0; i < SERVO_COUNT ; i++){
|
||||
matrix_weights[i][W_PITCH]*=sleeping;
|
||||
matrix_weights[i][W_ROLL]*=sleeping;
|
||||
matrix_weights[i][W_NOISE]*=sleeping;
|
||||
matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
|
||||
}
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
|
@ -261,7 +275,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){
|
||||
|
||||
static unsigned long millis_add;
|
||||
#define MILLISADD_MAX 100
|
||||
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
struct Mood{
|
||||
float shakiness;
|
||||
float wakefulness;
|
||||
float loneliness;
|
||||
};
|
||||
extern Mood mood;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue