218 lines
7 KiB
C++
218 lines
7 KiB
C++
#include "Arduino.h"
|
|
#include "behaviour.h"
|
|
|
|
SimplexNoise sn;
|
|
|
|
|
|
Mood mood;
|
|
|
|
float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
|
|
float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
|
|
float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
|
|
|
|
|
|
void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR,float height,float temp_ambient,float temp_object,float pumpspeed, float servoerror) {
|
|
static unsigned long last_mood_update=0;
|
|
if (millis-last_mood_update>MOOD_UPDATE_INTERVAL) {
|
|
last_mood_update=millis;
|
|
mood.arousal=0;
|
|
mood.valence=0;
|
|
}
|
|
}
|
|
|
|
void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){
|
|
|
|
static unsigned long last_weights_update=0;
|
|
if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
|
|
last_weights_update=millis;
|
|
|
|
|
|
|
|
|
|
//### Update Mode ###
|
|
static float map_mode_rollpitch=0;
|
|
static float map_mode_walking=0;
|
|
static float map_mode_sleeping=0;
|
|
static float map_mode_noise=0;
|
|
|
|
float aim_map_mode_rollpitch=0;
|
|
float aim_map_mode_walking=0;
|
|
float aim_map_mode_sleeping=0;
|
|
float aim_map_mode_noise=0;
|
|
|
|
|
|
|
|
if (body_present) { //attached
|
|
aim_map_mode_rollpitch=1.0;
|
|
|
|
}else{ //detached
|
|
if (bellyup) { //belly up
|
|
aim_map_mode_walking=1.0;
|
|
}else{ //sitting on a level survace
|
|
aim_map_mode_noise=1.0;
|
|
}
|
|
}
|
|
|
|
map_mode_rollpitch=interpolateTo(map_mode_rollpitch,aim_map_mode_rollpitch,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
|
|
map_mode_walking=interpolateTo(map_mode_walking,aim_map_mode_walking,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
|
|
map_mode_sleeping=interpolateTo(map_mode_sleeping,aim_map_mode_sleeping,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
|
|
map_mode_noise=interpolateTo(map_mode_noise,aim_map_mode_noise,WEIGHT_UPDATE_INTERVAL/1000.0,0.3);
|
|
|
|
|
|
/*
|
|
Serial.print("Walking: \t"); Serial.println(map_mode_walking);
|
|
Serial.print("RollPitch: \t"); Serial.println(map_mode_rollpitch);
|
|
Serial.print("Sleeping: \t"); Serial.println(map_mode_sleeping);
|
|
Serial.print("Noise: \t"); Serial.println(map_mode_noise);
|
|
Serial.println();
|
|
*/
|
|
|
|
float map_sum=map_mode_walking+map_mode_rollpitch+map_mode_sleeping+map_mode_noise;
|
|
map_mode_walking/=map_sum;
|
|
map_mode_rollpitch/=map_sum;
|
|
map_mode_sleeping/=map_sum;
|
|
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
|
|
|
|
|
analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64); //TODO, remove
|
|
|
|
|
|
//reset all weights
|
|
for (uint8_t i=0;i<SERVO_COUNT_MAIN;i++) {
|
|
for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
|
|
matrix_weights_main[i][w]=0;
|
|
}
|
|
}
|
|
for (uint8_t i=0;i<SERVO_COUNT_PERIPHERAL;i++) {
|
|
for (uint8_t w=0;w<WEIGHT_COUNT;w++) {
|
|
matrix_weights_peripheralL[i][w]=0;
|
|
matrix_weights_peripheralR[i][w]=0;
|
|
}
|
|
}
|
|
|
|
//Mode Noise
|
|
matrix_weights_main[0][W_NOISE]+=30.0 *map_mode_noise;
|
|
matrix_weights_main[0][W_NOISESLOW]+=0.0 *map_mode_noise;
|
|
|
|
matrix_weights_main[1][W_NOISE]+=30.0 *map_mode_noise;
|
|
matrix_weights_main[1][W_NOISESLOW]+=20.0 *map_mode_noise;
|
|
|
|
matrix_weights_main[2][W_NOISE]+=20.0 *map_mode_noise;
|
|
matrix_weights_main[2][W_NOISESLOW]+=30.0 *map_mode_noise;
|
|
|
|
|
|
|
|
//Mode Roll Pitch
|
|
matrix_weights_main[0][W_PITCH]+=1.0* map_mode_rollpitch;
|
|
matrix_weights_main[1][W_ROLL]+=1.0* map_mode_rollpitch;
|
|
matrix_weights_main[2][W_PITCH]+=1.0* map_mode_rollpitch;
|
|
|
|
//Mode Walking
|
|
matrix_weights_main[0][W_NOISESLOW]+=5 *map_mode_walking;
|
|
matrix_weights_main[0][W_SIN]+=10 *map_mode_walking;
|
|
matrix_weights_main[0][W_OFFSET]+=-60 *map_mode_walking;
|
|
matrix_weights_main[1][W_NOISESLOW]+=30 *map_mode_walking;
|
|
matrix_weights_main[2][W_COS]+=-20 *map_mode_walking;
|
|
matrix_weights_main[2][W_OFFSET]+=70 *map_mode_walking;
|
|
|
|
|
|
//Mode Sleeping
|
|
matrix_weights_main[0][W_NOISESLOW]+=20 *map_mode_sleeping;
|
|
matrix_weights_main[1][W_NOISESLOW]+=20 *map_mode_sleeping;
|
|
matrix_weights_main[2][W_NOISESLOW]+=30 *map_mode_sleeping;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void updateServosByWeights(unsigned long millis,float pitch,float roll){
|
|
static unsigned long last_servoweights_update=0;
|
|
if (millis-last_servoweights_update>SERVOWEIGHT_UPDATE_INTERVAL)
|
|
{
|
|
last_servoweights_update=millis;
|
|
static unsigned long millis_add;
|
|
#define MILLISADD_MIN SERVOWEIGHT_UPDATE_INTERVAL*0.5
|
|
#define MILLISADD_MAX SERVOWEIGHT_UPDATE_INTERVAL*2
|
|
millis_add+=constrain(mapfloat(mood.arousal, -0.5,0.5 ,MILLISADD_MIN,MILLISADD_MAX),MILLISADD_MIN,MILLISADD_MAX);
|
|
|
|
|
|
float vsin=sin((millis_add)/1000.0);
|
|
float vcos=cos((millis_add)/1000.0);
|
|
|
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
|
uint8_t servoPosInMatrix=0;
|
|
|
|
|
|
float pnoise=sn.noise((millis()+millis_add)/5000.0,i*10); //simplexnoise -1 to 1
|
|
float pnoiseslow=sn.noise((millis()+millis_add)/50000.0,i*10); //simplexnoise -1 to 1
|
|
|
|
pitch=constrain(pitch,-80,80);
|
|
roll=constrain(roll,-80,80);
|
|
|
|
|
|
float angle=0;
|
|
|
|
float lookdirection=(servos[i].angle>=0)?1:-1; //if angle positive, 1, else 0
|
|
|
|
|
|
|
|
|
|
if (i<SERVO_COUNT_MAIN) { //Main
|
|
/*if (i==0){
|
|
Serial.print("servoangle:");Serial.println(servos[i].angle);
|
|
Serial.print("lookdirection:");Serial.println(lookdirection);
|
|
Serial.print("weight:");Serial.println(matrix_weights_main[i][W_LOOKDIRECTION]);
|
|
|
|
|
|
|
|
}*/
|
|
|
|
servoPosInMatrix=i;
|
|
angle=
|
|
pitch*matrix_weights_main[servoPosInMatrix][W_PITCH]
|
|
+roll*matrix_weights_main[servoPosInMatrix][W_ROLL]
|
|
+pnoise*matrix_weights_main[servoPosInMatrix][W_NOISE]
|
|
+pnoiseslow*matrix_weights_main[servoPosInMatrix][W_NOISESLOW]
|
|
+vsin*matrix_weights_main[servoPosInMatrix][W_SIN]
|
|
+vcos*matrix_weights_main[servoPosInMatrix][W_COS]
|
|
+lookdirection*matrix_weights_main[servoPosInMatrix][W_LOOKDIRECTION]
|
|
+1.0*matrix_weights_main[servoPosInMatrix][W_OFFSET];
|
|
|
|
}else if(i<SERVO_COUNT_MAIN+SERVO_COUNT_PERIPHERAL) { //Peripheral Left
|
|
servoPosInMatrix=i-3;
|
|
}else if(i<SERVO_COUNT_MAIN+2*SERVO_COUNT_PERIPHERAL) { //Peripheral Right
|
|
servoPosInMatrix=i-2*3;
|
|
|
|
}
|
|
|
|
servos[i].angle=angle;
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
float interpolateTo(float value,float aimvalue,float timestep,float speed)
|
|
{
|
|
float result=value;
|
|
if (result<aimvalue){
|
|
result=result+timestep*speed;
|
|
if (result>aimvalue) {
|
|
result=aimvalue;
|
|
}
|
|
}else if (result>aimvalue){
|
|
result=result-timestep*speed;
|
|
if (result<aimvalue) {
|
|
result=aimvalue;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
|
|
{
|
|
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
|
|
}
|
|
|