add more behaviour
This commit is contained in:
parent
c125512adb
commit
ec3afd83ce
7 changed files with 213 additions and 34 deletions
|
@ -6,17 +6,128 @@ SimplexNoise sn;
|
||||||
|
|
||||||
Mood mood;
|
Mood mood;
|
||||||
|
|
||||||
|
float map_mode_rollpitch=0;
|
||||||
|
float map_mode_walking=0;
|
||||||
|
float map_mode_sleeping=0;
|
||||||
|
float map_mode_noise=0;
|
||||||
|
|
||||||
float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, sensors/moods
|
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_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
|
||||||
float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
|
float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT];
|
||||||
|
|
||||||
|
void initBehaviour() {
|
||||||
|
mood.arousal=0;
|
||||||
|
mood.valence=0;
|
||||||
|
}
|
||||||
|
|
||||||
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) {
|
|
||||||
|
void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,float maxAccX,float maxAccY,float maxAccZ,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;
|
static unsigned long last_mood_update=0;
|
||||||
if (millis-last_mood_update>MOOD_UPDATE_INTERVAL) {
|
if (millis-last_mood_update>MOOD_UPDATE_INTERVAL) {
|
||||||
last_mood_update=millis;
|
last_mood_update=millis;
|
||||||
mood.arousal=0;
|
|
||||||
mood.valence=0;
|
|
||||||
|
float step=MOOD_UPDATE_INTERVAL/1000.0;
|
||||||
|
|
||||||
|
if (body_present && pumpspeed<0.8) { //body present and vacuum present (pump below speed 1.0)
|
||||||
|
if (mood.valence<0.0) { //when attached, and valence low
|
||||||
|
mood.valence=0.2; //jump valence up quickly
|
||||||
|
}
|
||||||
|
if (mood.valence<0.5) { //increase valence up to a certain point
|
||||||
|
mood.valence+=step*0.05;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!body_present) {
|
||||||
|
if (mood.valence>-0.5) { //decrease valence down to a certain point
|
||||||
|
mood.valence-=step*0.01;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static unsigned long last_contact_time=0;
|
||||||
|
static bool last_contact_main=false;
|
||||||
|
static bool last_contact_peripheralR=false;
|
||||||
|
static bool last_contact_peripheralL=false;
|
||||||
|
|
||||||
|
|
||||||
|
if (contact_main || contact_peripheralL || contact_peripheralR) {
|
||||||
|
if ((contact_main && !last_contact_main) || (contact_peripheralL && !last_contact_peripheralL) || (contact_peripheralR && !last_contact_peripheralR)) { //on first contact
|
||||||
|
|
||||||
|
long _contacttime_min=1000;
|
||||||
|
long _contacttime_max=20000;
|
||||||
|
float _contact_increase_arousal=0.7;
|
||||||
|
float _contact_increase_valence=0.1;
|
||||||
|
if (mood.valence<-0.2) {
|
||||||
|
long _contacttime_min=500;
|
||||||
|
long _contacttime_max=2000;
|
||||||
|
float _contact_increase_arousal=0.7;
|
||||||
|
float _contact_increase_valence=-0.2;
|
||||||
|
}
|
||||||
|
float olda=mood.arousal;
|
||||||
|
float oldv=mood.valence;
|
||||||
|
mood.arousal+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_arousal),0,_contact_increase_arousal);
|
||||||
|
mood.valence+=constrain(mapfloat(millis-last_contact_time,_contacttime_min,_contacttime_max,0,_contact_increase_valence),0,_contact_increase_valence);
|
||||||
|
Serial.print("Contact. increase arousal:"); Serial.print(mood.arousal-olda); Serial.print(" valence:"); Serial.print(mood.valence-oldv); Serial.println();
|
||||||
|
}
|
||||||
|
last_contact_time=millis;
|
||||||
|
}
|
||||||
|
|
||||||
|
last_contact_main=contact_main;
|
||||||
|
last_contact_peripheralR=contact_peripheralR;
|
||||||
|
last_contact_peripheralL=contact_peripheralL;
|
||||||
|
|
||||||
|
|
||||||
|
static float last_height=0;
|
||||||
|
static float heightdiffsmooth=0; //goes up to 0.2 when lifted or lowered quickly 1m and slowly deacreases to 0
|
||||||
|
heightdiffsmooth+=height-last_height;
|
||||||
|
if (heightdiffsmooth>0) {
|
||||||
|
heightdiffsmooth-=behaviour_confortable_lift*step;
|
||||||
|
}else if (heightdiffsmooth<0) {
|
||||||
|
heightdiffsmooth+=behaviour_confortable_lift*step; //0.1*step means 0.1m of heightchange per second does not affect heightdiffsmooth
|
||||||
|
}
|
||||||
|
|
||||||
|
//Serial.print("heightdiffsmooth:"); Serial.println(heightdiffsmooth,3);
|
||||||
|
|
||||||
|
|
||||||
|
float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
|
||||||
|
float preferredMaxAcc=0.5;
|
||||||
|
if (constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)>0.0) {
|
||||||
|
digitalWrite(PIN_LED,LOW); //On
|
||||||
|
}else{
|
||||||
|
digitalWrite(PIN_LED,HIGH); //Off
|
||||||
|
}
|
||||||
|
if (mood.arousal<-0.5) {
|
||||||
|
mood.arousal+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
|
||||||
|
}else{
|
||||||
|
mood.valence+=constrain(mapfloat(abs(preferredMaxAcc-maxAcc),0.0,0.3,1.0,0.0),0.0,1.0)*step*0.2;
|
||||||
|
if (mood.arousal>0.7) {
|
||||||
|
mood.arousal-=step*0.04;
|
||||||
|
}else if(mood.arousal<0.3) {
|
||||||
|
mood.arousal+=step*0.03;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (abs(heightdiffsmooth)>0.05) {
|
||||||
|
mood.valence-=abs(heightdiffsmooth)*step*1.0;
|
||||||
|
if (mood.arousal<0.9){
|
||||||
|
mood.arousal+=abs(heightdiffsmooth)*step*3.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
last_height=height;
|
||||||
|
|
||||||
|
|
||||||
|
mood.arousal-=step*0.02; //slowly deacrease arousal
|
||||||
|
|
||||||
|
if (mood.valence<0) {
|
||||||
|
mood.valence+=step*0.03;
|
||||||
|
}else if (mood.valence>0) {
|
||||||
|
mood.valence-=step*0.01;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
mood.arousal=constrain(mood.arousal,-1.0,1.0);
|
||||||
|
mood.valence=constrain(mood.valence,-1.0,1.0);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -30,10 +141,7 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
|
||||||
|
|
||||||
|
|
||||||
//### Update Mode ###
|
//### 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_rollpitch=0;
|
||||||
float aim_map_mode_walking=0;
|
float aim_map_mode_walking=0;
|
||||||
|
@ -42,14 +150,19 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (body_present) { //attached
|
if (body_present) { //attached
|
||||||
aim_map_mode_rollpitch=1.0;
|
|
||||||
|
aim_map_mode_rollpitch=constrain(mapfloat(mood.valence,-0.3,0.3,0,1.0),0,1.0);
|
||||||
|
|
||||||
|
aim_map_mode_noise=constrain(mapfloat(mood.valence,-0.2,-1.0,0,1.0),0,1.0);
|
||||||
|
|
||||||
|
|
||||||
}else{ //detached
|
}else{ //detached
|
||||||
if (bellyup) { //belly up
|
if (bellyup) { //belly up
|
||||||
aim_map_mode_walking=1.0;
|
aim_map_mode_walking=1.0;
|
||||||
}else{ //sitting on a level survace
|
}else{ //sitting on a level survace
|
||||||
aim_map_mode_noise=1.0;
|
aim_map_mode_noise=constrain(mapfloat(mood.arousal,-0.5,0.5, 0.0,1.0),0,1.0);
|
||||||
|
map_mode_sleeping=constrain(mapfloat(mood.arousal,-1.0,-0.6, 1.0,0.0),0,1.0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -59,13 +172,9 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
|
||||||
map_mode_noise=interpolateTo(map_mode_noise,aim_map_mode_noise,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;
|
float map_sum=map_mode_walking+map_mode_rollpitch+map_mode_sleeping+map_mode_noise;
|
||||||
map_mode_walking/=map_sum;
|
map_mode_walking/=map_sum;
|
||||||
|
@ -73,8 +182,7 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
|
||||||
map_mode_sleeping/=map_sum;
|
map_mode_sleeping/=map_sum;
|
||||||
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
||||||
|
|
||||||
|
analogWrite(PIN_VIBRATION,contact_main?64:0);
|
||||||
analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64); //TODO, remove
|
|
||||||
|
|
||||||
|
|
||||||
//reset all weights
|
//reset all weights
|
||||||
|
@ -124,6 +232,13 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void printModes() {
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
|
||||||
void updateServosByWeights(unsigned long millis,float pitch,float roll){
|
void updateServosByWeights(unsigned long millis,float pitch,float roll){
|
||||||
static unsigned long last_servoweights_update=0;
|
static unsigned long last_servoweights_update=0;
|
||||||
|
@ -216,3 +331,7 @@ 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;
|
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void printMood(){
|
||||||
|
Serial.print("A:"); Serial.print(mood.arousal); Serial.print(" V:"); Serial.print(mood.valence);
|
||||||
|
Serial.println();
|
||||||
|
}
|
||||||
|
|
|
@ -38,12 +38,16 @@ struct Mood{
|
||||||
};
|
};
|
||||||
extern Mood mood;
|
extern Mood mood;
|
||||||
|
|
||||||
|
const float behaviour_confortable_lift=0.1; // Meters per seond. heightchange below does not affect mood
|
||||||
|
|
||||||
|
|
||||||
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);
|
void initBehaviour();
|
||||||
|
void updateMood(unsigned long millis,float gyroX,float gyroY,float gyroZ,float maxAccX,float maxAccY,float maxAccZ,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);
|
||||||
void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll);
|
void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll);
|
||||||
void updateServosByWeights(unsigned long millis,float pitch,float roll);
|
void updateServosByWeights(unsigned long millis,float pitch,float roll);
|
||||||
float interpolateTo(float value,float aimvalue,float timestep,float speed);
|
float interpolateTo(float value,float aimvalue,float timestep,float speed);
|
||||||
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
|
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
|
||||||
|
void printMood();
|
||||||
|
void printModes();
|
||||||
|
|
||||||
#endif
|
#endif
|
|
@ -4,6 +4,10 @@
|
||||||
|
|
||||||
MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);
|
MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);
|
||||||
|
|
||||||
|
float maxAccX;
|
||||||
|
float maxAccY;
|
||||||
|
float maxAccZ;
|
||||||
|
|
||||||
|
|
||||||
bool imu_bellyup=false;
|
bool imu_bellyup=false;
|
||||||
|
|
||||||
|
@ -34,6 +38,23 @@ void loopIMU(unsigned long millis) {
|
||||||
Serial.print("\tangleZ : ");
|
Serial.print("\tangleZ : ");
|
||||||
Serial.println(mpu6050.getAngleZ());*/
|
Serial.println(mpu6050.getAngleZ());*/
|
||||||
|
|
||||||
|
|
||||||
|
maxAccX-=UPDATE_IMU_INTERVAL/1000.0*1.0; //slowly reduce values
|
||||||
|
maxAccY-=UPDATE_IMU_INTERVAL/1000.0*1.0;
|
||||||
|
maxAccZ-=UPDATE_IMU_INTERVAL/1000.0*1.0;
|
||||||
|
static float last_accX;
|
||||||
|
static float last_accY;
|
||||||
|
static float last_accZ;
|
||||||
|
float _accX=getAccX();
|
||||||
|
float _accY=getAccY();
|
||||||
|
float _accZ=getAccZ();
|
||||||
|
maxAccX=max(abs(last_accX-_accX), maxAccX);
|
||||||
|
maxAccY=max(abs(last_accY-_accY), maxAccY);
|
||||||
|
maxAccZ=max(abs(last_accZ-_accZ), maxAccZ);
|
||||||
|
last_accX=_accX;
|
||||||
|
last_accY=_accY;
|
||||||
|
last_accZ=_accZ;
|
||||||
|
|
||||||
if(mpu6050.getAccZ()<-0.5) { //is upside down
|
if(mpu6050.getAccZ()<-0.5) { //is upside down
|
||||||
imu_bellyup=true;
|
imu_bellyup=true;
|
||||||
}else{
|
}else{
|
||||||
|
@ -71,6 +92,22 @@ float getAccZ() {
|
||||||
return mpu6050.getAccZ();
|
return mpu6050.getAccZ();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
float getMaxAccX() {
|
||||||
|
return maxAccX;
|
||||||
|
}
|
||||||
|
float getMaxAccY() {
|
||||||
|
return maxAccY;
|
||||||
|
}
|
||||||
|
float getMaxAccZ() {
|
||||||
|
return maxAccZ;
|
||||||
|
}
|
||||||
|
void resetMaxAcc(){
|
||||||
|
maxAccX=0;
|
||||||
|
maxAccY=0;
|
||||||
|
maxAccZ=0;
|
||||||
|
}
|
||||||
|
|
||||||
float getGyroAngleX() {
|
float getGyroAngleX() {
|
||||||
return mpu6050.getGyroAngleX();
|
return mpu6050.getGyroAngleX();
|
||||||
}
|
}
|
||||||
|
|
|
@ -24,6 +24,11 @@ float getAccX();
|
||||||
float getAccY();
|
float getAccY();
|
||||||
float getAccZ();
|
float getAccZ();
|
||||||
|
|
||||||
|
float getMaxAccX();
|
||||||
|
float getMaxAccY();
|
||||||
|
float getMaxAccZ();
|
||||||
|
void resetMaxAcc();
|
||||||
|
|
||||||
float getGyroAngleX();
|
float getGyroAngleX();
|
||||||
float getGyroAngleY();
|
float getGyroAngleY();
|
||||||
float getGyroAngleZ();
|
float getGyroAngleZ();
|
||||||
|
|
|
@ -16,6 +16,8 @@ void setup() {
|
||||||
pinMode(PIN_LED,OUTPUT);
|
pinMode(PIN_LED,OUTPUT);
|
||||||
digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
|
digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
|
||||||
|
|
||||||
|
initBehaviour();
|
||||||
|
|
||||||
initVacuum();
|
initVacuum();
|
||||||
initBodytemp();
|
initBodytemp();
|
||||||
|
|
||||||
|
@ -24,9 +26,9 @@ void setup() {
|
||||||
initIMU(7.54,-15.26,-1.41);
|
initIMU(7.54,-15.26,-1.41);
|
||||||
|
|
||||||
|
|
||||||
pinMode(PIN_INPUTARM1,INPUT);
|
pinMode(PIN_INPUTARM1,INPUT_PULLDOWN);
|
||||||
pinMode(PIN_INPUTARM2,INPUT);
|
pinMode(PIN_INPUTARM2,INPUT_PULLDOWN);
|
||||||
pinMode(PIN_INPUTARM3,INPUT);
|
pinMode(PIN_INPUTARM3,INPUT_PULLDOWN);
|
||||||
|
|
||||||
pinMode(PIN_INPUTAUX,INPUT);
|
pinMode(PIN_INPUTAUX,INPUT);
|
||||||
|
|
||||||
|
@ -41,7 +43,7 @@ void loop() {
|
||||||
|
|
||||||
|
|
||||||
bool body_present=checkBodypresence(loopmillis);
|
bool body_present=checkBodypresence(loopmillis);
|
||||||
digitalWrite(PIN_LED,!body_present);
|
//digitalWrite(PIN_LED,!body_present);
|
||||||
|
|
||||||
loopVacuum(loopmillis);
|
loopVacuum(loopmillis);
|
||||||
|
|
||||||
|
@ -66,9 +68,11 @@ void loop() {
|
||||||
float pitch=-getAngleY();
|
float pitch=-getAngleY();
|
||||||
float roll=-getAngleX();
|
float roll=-getAngleX();
|
||||||
|
|
||||||
|
float maxAccX=getMaxAccX();
|
||||||
|
float maxAccY=getMaxAccY();
|
||||||
|
float maxAccZ=getMaxAccZ();
|
||||||
|
|
||||||
|
updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),maxAccX,maxAccY,maxAccZ,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
|
||||||
updateMood(loopmillis,getGyroAngleX(),getGyroAngleY(),getGyroAngleZ(),bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,getBarometerHeight(),getAmbienttemp(),getObjecttemp(),getAveragePumpspeed(),getAverageServoPositionerror());
|
|
||||||
updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
|
updateMatrixWeights(loopmillis,bellyup,body_present,contact_main,contact_peripheralL,contact_peripheralR,pitch,roll);
|
||||||
updateServosByWeights(loopmillis,pitch,roll);
|
updateServosByWeights(loopmillis,pitch,roll);
|
||||||
|
|
||||||
|
@ -80,7 +84,7 @@ void loop() {
|
||||||
|
|
||||||
//Print Debug Information
|
//Print Debug Information
|
||||||
static unsigned long last_print=0;
|
static unsigned long last_print=0;
|
||||||
if (loopmillis - last_print >100) {
|
if (loopmillis - last_print >500) {
|
||||||
last_print=loopmillis;
|
last_print=loopmillis;
|
||||||
//printVacuumValues();
|
//printVacuumValues();
|
||||||
//Serial.print(" Vac="); Serial.print(vac);
|
//Serial.print(" Vac="); Serial.print(vac);
|
||||||
|
@ -92,10 +96,16 @@ void loop() {
|
||||||
|
|
||||||
Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
|
Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
|
||||||
Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
|
Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
|
||||||
Serial.print("Height:");Serial.print(getBarometerHeight()); Serial.println();
|
Serial.print("Height:");Serial.print(getBarometerHeight(),3); Serial.println();
|
||||||
|
Serial.print("Contact:");Serial.print(contact_main); Serial.print(", "); Serial.print(contact_peripheralL); Serial.print(", "); Serial.print(contact_peripheralR);Serial.println();
|
||||||
|
|
||||||
Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
|
|
||||||
|
|
||||||
|
//Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
|
||||||
|
float maxAcc=sqrt(pow(maxAccX,2)+pow(maxAccY,2)+pow(maxAccZ,2));
|
||||||
|
Serial.print("Accel: ");Serial.print(maxAccX); Serial.print(", ");Serial.print(maxAccY); Serial.print(", ");Serial.print(maxAccZ); Serial.print(", max: ");Serial.print(maxAcc); Serial.println();
|
||||||
|
printMood();
|
||||||
|
|
||||||
|
printModes();
|
||||||
|
|
||||||
|
|
||||||
Serial.println();
|
Serial.println();
|
||||||
|
|
|
@ -15,6 +15,7 @@ void initServos()
|
||||||
{
|
{
|
||||||
//Main Arm first Servo. Model 1020
|
//Main Arm first Servo. Model 1020
|
||||||
servos[0].id=11;
|
servos[0].id=11;
|
||||||
|
servos[0].velocity=50;
|
||||||
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;
|
||||||
|
@ -25,6 +26,7 @@ void initServos()
|
||||||
|
|
||||||
//Main Arm second Servo. Model 1080
|
//Main Arm second Servo. Model 1080
|
||||||
servos[1].id=12;
|
servos[1].id=12;
|
||||||
|
servos[1].velocity=75;
|
||||||
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].min_angle=180-90;
|
||||||
|
@ -34,6 +36,7 @@ void initServos()
|
||||||
|
|
||||||
//Main Arm third Servo. Model 1080
|
//Main Arm third Servo. Model 1080
|
||||||
servos[2].id=13;
|
servos[2].id=13;
|
||||||
|
servos[2].velocity=100;
|
||||||
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].min_angle=180-90;
|
||||||
|
@ -83,7 +86,7 @@ void initServos()
|
||||||
dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
|
dxl.setGoalCurrent(servos[i].id, servos[i].current, UNIT_PERCENT);
|
||||||
}
|
}
|
||||||
|
|
||||||
dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, 30); //was 30
|
dxl.writeControlTableItem(ControlTableItem::PROFILE_VELOCITY, servos[i].id, servos[i].velocity); //was 30
|
||||||
dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
|
dxl.writeControlTableItem(ControlTableItem::PROFILE_ACCELERATION, servos[i].id, 10);
|
||||||
|
|
||||||
dxl.torqueOn(servos[i].id);
|
dxl.torqueOn(servos[i].id);
|
||||||
|
@ -139,9 +142,9 @@ float getAverageServoPositionerror()
|
||||||
float angleerror=0;
|
float angleerror=0;
|
||||||
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
||||||
float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
|
float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
|
||||||
if (current_error<0){
|
|
||||||
current_error*=-1;
|
current_error=abs(current_error);
|
||||||
}
|
|
||||||
angleerror+=current_error;
|
angleerror+=current_error;
|
||||||
}
|
}
|
||||||
angleerror/=SERVO_COUNT;
|
angleerror/=SERVO_COUNT;
|
||||||
|
|
|
@ -26,8 +26,9 @@
|
||||||
|
|
||||||
|
|
||||||
struct dxl_servo {
|
struct dxl_servo {
|
||||||
int id;
|
uint8_t id;
|
||||||
//int initial_speed;
|
//int initial_speed;
|
||||||
|
int32_t velocity;
|
||||||
float offset_angle; //constant offset. set this to match angle=0 to a straight arm
|
float offset_angle; //constant offset. set this to match angle=0 to a straight arm
|
||||||
float angle; //commanded angle
|
float angle; //commanded angle
|
||||||
float actual_angle; //angle read from servo
|
float actual_angle; //angle read from servo
|
||||||
|
|
Loading…
Reference in a new issue