philipp.kramer/haustorial
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

add barometer and servo status read

Browse source
This commit is contained in:
Philipp Kramer 2024-09-12 11:23:52 +02:00
parent 8849c7ba4f
commit c125512adb
14 changed files with 237 additions and 66 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
opencmSensorboard/~opencmSensorboard.kicad_pcb.lck
View file

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

66
prosthesis_controller/barometer.cpp Normal file
View file

@ -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;
}

16
prosthesis_controller/barometer.h Normal file
View file

@ -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

99
prosthesis_controller/behaviour.cpp
View file

@ -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 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;
if (body_present) { //attached
aim_map_mode_rollpitch=1.0;
}else{
map_mode_rollpitch-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
if (bellyup) {
map_mode_walking+=0.3 *WEIGHT_UPDATE_INTERVAL/1000.0;
map_mode_noise-=.2 *WEIGHT_UPDATE_INTERVAL/1000.0;
}else{ //detached
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;
map_mode_noise+=0.3*WEIGHT_UPDATE_INTERVAL/1000.0;
aim_map_mode_noise=1.0;
}
}
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);
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_noise;
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);
Serial.print("Vibrt:"); Serial.println(contact_main*map_mode_noise*160);
analogWrite(PIN_VIBRATION,contact_main*map_mode_noise*64); //TODO, remove
//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
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
#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()+millis_add)/1000.0);
float vcos=cos((millis()+millis_add)/1000.0);
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;
@ -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)
{

10
prosthesis_controller/behaviour.h
View file

@ -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 wakefulness;
float loneliness;
float arousal; //-1.0 to 1.0
float valence; //-1.0 to 1.0
};
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

7
prosthesis_controller/bodytemp.cpp
View file

@ -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);

3
prosthesis_controller/bodytemp.h
View file

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

10
prosthesis_controller/imu.cpp
View file

@ -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();
}

4
prosthesis_controller/imu.h
View file

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

19
prosthesis_controller/prosthesis_controller.ino
View file

@ -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();
@ -62,7 +65,10 @@ 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);
@ -83,11 +89,16 @@ void loop() {
//Serial.println();
//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();
}
}

42
prosthesis_controller/servo.cpp
View file

@ -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;
servos[2].angle=sin(millis/1000.0/1.0)*20;*/
static uint8_t servoReadI=0; //only one servo per function call
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();
}

20
prosthesis_controller/servo.h
View file

@ -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 angle;
float min_angle;
float max_angle;
int orientation;
uint8_t mode=OP_CURRENT_BASED_POSITION;
float current;
uint16_t modelnumber;
float offset_angle; //constant offset. set this to match angle=0 to a straight arm
float angle; //commanded angle
float actual_angle; //angle read from servo
float min_angle; //soft angle limit
float max_angle; //soft angle limit
int orientation; //1.0 = forward. -1.0 = backwards
uint8_t mode=OP_CURRENT_BASED_POSITION; // OP_POSITION, OP_CURRENT_BASED_POSITION
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();

4
prosthesis_controller/vacuum.cpp
View file

@ -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);

2
prosthesis_controller/vacuum.h
View file

@ -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_ */