add barometer and servo status read

opencmSensorboard/~opencmSensorboard.kicad_pcb.lck

@@ -1 +0,0 @@
-{"hostname":"WS42","username":"philipp.kramer"}

prosthesis_controller/barometer.cpp

@@ -0,0 +1,66 @@
+#include "Arduino.h"
+#include "barometer.h"
+
+Adafruit_BMP280 bmp; // I2C
+
+
+float barometer_temp;
+float barometer_pressure;
+float barometer_alt;
+
+float barometer_alt_calibrationpressure=0; //example: 1002.45
+bool barometer_alt_calibrated=false;
+
+void initBarometer() {
+  unsigned status;
+  Wire.begin(25,24);
+  status = bmp.begin(BMP280_ADDRESS_ALT, BMP280_CHIPID);
+  
+  //status = bmp.begin();
+  if (!status) {
+    Serial.println(F("Could not find a valid BMP280 sensor"));
+  }
+
+  /* Default settings from datasheet. */
+  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
+                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
+                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
+                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
+                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
+
+}
+
+void loopBarometer(unsigned long millis) {
+  static unsigned long last_barometerreading = 0;
+  if (millis - last_barometerreading > BAROMETER_UPDATE_INTERVAL)
+  {
+    last_barometerreading=millis;
+    barometer_temp=bmp.readTemperature();
+    
+    
+    
+    if (!barometer_alt_calibrated) { //only read pressure for calibration
+      barometer_pressure=bmp.readPressure();
+    }
+
+    if (!barometer_alt_calibrated && millis>5000) {
+
+      barometer_alt_calibrated=true;
+      barometer_alt_calibrationpressure=barometer_pressure/100.0;
+    }
+
+    if (barometer_alt_calibrated) {
+      barometer_alt=bmp.readAltitude(barometer_alt_calibrationpressure);
+    }
+  }
+
+  
+
+}
+
+float getBarometerHeight() {
+  return barometer_alt;
+}
+float getBarometerTemperature() {
+  return barometer_temp;
+}

prosthesis_controller/barometer.h

@@ -0,0 +1,16 @@
+#ifndef BAROMETER_H_
+#define BAROMETER_H_
+
+
+#include <Wire.h>
+#include <SPI.h>
+#include <Adafruit_BMP280.h> //Adafruit BMP280 Library by Adafruit 2.6.8
+
+#define BAROMETER_UPDATE_INTERVAL 1000/5
+
+void initBarometer();
+void loopBarometer(unsigned long millis);
+float getBarometerHeight();
+float getBarometerTemperature();
+
+#endif

prosthesis_controller/behaviour.cpp

@@ -10,66 +10,72 @@ float matrix_weights_main[SERVO_COUNT_MAIN][WEIGHT_COUNT]; //mapping outputs, se
 float matrix_weights_peripheralL[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
 float matrix_weights_peripheralR[SERVO_COUNT_PERIPHERAL][WEIGHT_COUNT]; 
 
-void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, bool contact_main, bool contact_peripheralL, bool contact_peripheralR, float pitch, float roll){
 
+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;
 
-    //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;*/
     
 
+    
+    //### 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) {      
-      map_mode_rollpitch+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
-      map_mode_walking-=0.5 *WEIGHT_UPDATE_INTERVAL/1000.0;
-      map_mode_noise-=0.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
 
-    }else{        
+    if (body_present) { //attached     
-      map_mode_rollpitch-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      aim_map_mode_rollpitch=1.0;
-      if (bellyup) {
+
-        map_mode_walking+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
+    }else{ //detached
-        map_mode_noise-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
+      if (bellyup) { //belly up
+        aim_map_mode_walking=1.0;
       }else{ //sitting on a level survace
-        map_mode_walking-=0.1 *WEIGHT_UPDATE_INTERVAL/1000.0;
+        aim_map_mode_noise=1.0;
-        map_mode_noise+=0.3*WEIGHT_UPDATE_INTERVAL/1000.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);
 
 
-    map_mode_walking=constrain(map_mode_walking,0.0,1.0);
+    /*
-    map_mode_rollpitch=constrain(map_mode_rollpitch,0.0,1.0);
-    map_mode_noise=constrain(map_mode_noise,0.0,1.0);
-
-    
     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_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);
+    analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64); //TODO, remove
-    Serial.print("Vibrt:"); Serial.println(contact_main*map_mode_noise*160);
+    
 
     //reset all weights
     for (uint8_t i=0;i<SERVO_COUNT_MAIN;i++) {
@@ -115,15 +121,6 @@ void updateMatrixWeights(unsigned long millis,bool bellyup, bool body_present, b
     matrix_weights_main[1][W_NOISESLOW]+=20 *map_mode_sleeping;
     matrix_weights_main[2][W_NOISESLOW]+=30 *map_mode_sleeping;
 
-    /*
-    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);
-    }
-    */
   }
 }
 
@@ -134,12 +131,13 @@ void updateServosByWeights(unsigned long millis,float pitch,float roll){
   {
     last_servoweights_update=millis;
     static unsigned long millis_add;
-    #define MILLISADD_MAX 100
+    #define MILLISADD_MIN SERVOWEIGHT_UPDATE_INTERVAL*0.5
-    millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
+    #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()+millis_add)/1000.0);
+    float vsin=sin((millis_add)/1000.0);
-    float vcos=cos((millis()+millis_add)/1000.0);
+    float vcos=cos((millis_add)/1000.0);
     
     for (uint8_t i=0;i<SERVO_COUNT;i++) {
       uint8_t servoPosInMatrix=0;
@@ -195,6 +193,23 @@ void updateServosByWeights(unsigned long millis,float pitch,float roll){
 }
 
 
+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)
 {

prosthesis_controller/behaviour.h

@@ -19,6 +19,8 @@ extern dxl_servo servos[SERVO_COUNT];
 #define WEIGHT_UPDATE_INTERVAL 100
 #define SERVOWEIGHT_UPDATE_INTERVAL SERVO_UPDATE_INTERVAL
 
+#define MOOD_UPDATE_INTERVAL 1000.0/50
+
 
 #define W_PITCH 0
 #define W_ROLL 1
@@ -31,17 +33,17 @@ extern dxl_servo servos[SERVO_COUNT];
 
 
 struct Mood{
-  float shakiness;
+  float arousal; //-1.0 to 1.0
-  float wakefulness;
+  float valence; //-1.0 to 1.0
-  float loneliness;
 };
 extern Mood 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 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);
+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);
 
 #endif

prosthesis_controller/bodytemp.cpp

@@ -21,6 +21,13 @@ void initBodytemp() {
   };
 }
 
+double getAmbienttemp() {
+  return bodytemp_ambient;
+}
+double getObjecttemp() {
+  return bodytemp_object;
+}
+
 void printBodytempDebug() {
   Serial.print("min:20,max:40,");
   Serial.print("Ambient:");Serial.print(bodytemp_ambient);

prosthesis_controller/bodytemp.h

@@ -10,4 +10,7 @@ void initBodytemp();
 void printBodytempDebug() ;
 bool checkBodypresence(unsigned long millis);
 
+double getObjecttemp();
+double getAmbienttemp();
+
 #endif

prosthesis_controller/imu.cpp

@@ -70,3 +70,13 @@ float getAccY() {
 float getAccZ() {
   return mpu6050.getAccZ();
 }
+
+float getGyroAngleX() {
+  return mpu6050.getGyroAngleX();
+}
+float getGyroAngleY() {
+  return mpu6050.getGyroAngleY();
+}
+float getGyroAngleZ() {
+  return mpu6050.getGyroAngleZ();
+}

prosthesis_controller/imu.h

@@ -24,6 +24,10 @@ float getAccX();
 float getAccY();
 float getAccZ();
 
+float getGyroAngleX();
+float getGyroAngleY();
+float getGyroAngleZ();
+
 
 
 #endif

prosthesis_controller/prosthesis_controller.ino

@@ -3,6 +3,7 @@
 #include "servo.h"
 #include "bodytemp.h"
 #include "behaviour.h"
+#include "barometer.h"
 #include "imu.h"
 
 void setup() {
@@ -18,6 +19,8 @@ void setup() {
   initVacuum();
   initBodytemp();
 
+  initBarometer();
+
   initIMU(7.54,-15.26,-1.41);
 
 
@@ -43,7 +46,7 @@ void loop() {
   loopVacuum(loopmillis);
 
 
-
+  loopBarometer(loopmillis);
   loopIMU(loopmillis);
   
   bool bellyup=isBellyUp();
@@ -63,6 +66,9 @@ void loop() {
   float pitch=-getAngleY();
   float roll=-getAngleX();
 
+
+
+  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);
   updateServosByWeights(loopmillis,pitch,roll);
 
@@ -84,10 +90,15 @@ void loop() {
     //printBodytempDebug();
 
     
+    Serial.print("Servo Error:");Serial.print(getAverageServoPositionerror()); Serial.println();
+    Serial.print("Avg Pump:");Serial.print(getAveragePumpspeed()); Serial.println();
+    Serial.print("Height:");Serial.print(getBarometerHeight()); Serial.println();
+    
+    Serial.print("GyroAngle: ");Serial.print(getGyroAngleX()); Serial.print(", ");Serial.print(getGyroAngleY()); Serial.print(", ");Serial.print(getGyroAngleZ()); Serial.println();
     
     
     
-    //Serial.println();
+    Serial.println();
   }
 
 }

prosthesis_controller/servo.cpp

@@ -106,27 +106,57 @@ void initServos()
 
 
 void loopServos(unsigned long millis) {
-  /*servos[0].angle=sin(millis/1000.0/1.0)*20;
+  
-  servos[1].angle=cos(millis/1000.0/1.0)*20;
+  static uint8_t servoReadI=0; //only one servo per function call
-  servos[2].angle=sin(millis/1000.0/1.0)*20;*/
+  static unsigned long last_readservo_update=0;
+
   static uint8_t servoUpdateI=0; //only one servo per function call
   static unsigned long last_servo_update=0;
-  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT){
+
+  //Sequentially set servo angles
+  if (millis-last_servo_update>SERVO_UPDATE_INTERVAL/SERVO_COUNT && millis-last_readservo_update>SERVO_MIN_COMMAND_TIME){
     last_servo_update=millis;
     
-    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
+    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);
     servoUpdateI++;
     servoUpdateI%=SERVO_COUNT;
   }
 
+  //Sequentially read servo status
+  if (millis-last_readservo_update>SERVO_READUPDATE_INTERVAL/SERVO_COUNT && millis-last_servo_update>SERVO_MIN_COMMAND_TIME){
+    last_readservo_update=millis;
+    
+    servos[servoReadI].actual_angle = dxl.getPresentPosition(servos[servoReadI].id, UNIT_DEGREE);
+    
+    servoReadI++;
+    servoReadI%=SERVO_COUNT;
   }
   
+}
+
+float getAverageServoPositionerror()
+{
+  float angleerror=0;
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    float current_error=servos[i].angle+servos[i].offset_angle - servos[i].actual_angle;
+    if (current_error<0){
+      current_error*=-1;
+    }
+    angleerror+=current_error;
+  }
+  angleerror/=SERVO_COUNT;
+  return angleerror;
+}
+
+
 void printServoDebug() {
   for (uint8_t i=0;i<SERVO_COUNT;i++) {
     //Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(dxl.getPresentPosition(servos[i].id, UNIT_DEGREE)); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
-    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].offset_angle+servos[i].angle);
+    Serial.print("Servo"); Serial.print(i);Serial.print(" : "); Serial.print(servos[i].actual_angle); Serial.print(" -> "); Serial.print(servos[i].offset_angle+servos[i].angle);
     Serial.println();
   }
+  Serial.print("erroravg:"); Serial.print(getAverageServoPositionerror());
+  Serial.println();
 }
 
 

prosthesis_controller/servo.h

@@ -16,6 +16,8 @@
 #define DXL_SERIAL   Serial1
 
 #define SERVO_UPDATE_INTERVAL 1000/50
+#define SERVO_READUPDATE_INTERVAL 1000/5
+#define SERVO_MIN_COMMAND_TIME 1 //should be less than any of the other servo intervals/SERVO_COUNT
 
 
 #define SERVO_COUNT 3
@@ -26,14 +28,15 @@
 struct dxl_servo {
 	int id;
 	//int initial_speed;
-	float offset_angle;
+	float offset_angle; //constant offset. set this to match angle=0 to a straight arm
-  float angle;
+  float angle; //commanded angle
-	float min_angle;
+  float actual_angle; //angle read from servo
-	float max_angle;
+	float min_angle; //soft angle limit
-	int orientation;
+	float max_angle; //soft angle limit
-  uint8_t mode=OP_CURRENT_BASED_POSITION;
+	int orientation; //1.0 = forward. -1.0 = backwards
-  float current;
+  uint8_t mode=OP_CURRENT_BASED_POSITION; // OP_POSITION, OP_CURRENT_BASED_POSITION
-  uint16_t modelnumber;
+  float current; //when mode=OP_CURRENT_BASED_POSITION, set current for servo
+  uint16_t modelnumber; //Servo Model number for connection check at startup. use 0 to deactivate
 };
 
 
@@ -43,6 +46,7 @@ void scanDynamixel();
 void changeID(uint8_t present_id, uint8_t new_id);
 void initServos();
 void loopServos(unsigned long millis);
+float getAverageServoPositionerror();
 void printServoDebug();
 
 

prosthesis_controller/vacuum.cpp

@@ -85,6 +85,10 @@ void loopVacuum(unsigned long millis) {
   }
 }
 
+float getAveragePumpspeed(){
+  return (vacuumPump1+vacuumPump2)/2.0;
+}
+
 void setVacuum(bool val) {
   if (val) { //True = turn off, False = Release
     pump2PID.SetMode(AUTOMATIC);

prosthesis_controller/vacuum.h

@@ -15,6 +15,6 @@ void loopVacuum(unsigned long millis);
 void setVacuum(bool val);
 void printVacuumValues();
 long readHX710B(uint8_t outpin);
-
+float getAveragePumpspeed();
 
 #endif /* VACUUM_H_ */