copy over behaviour template
This commit is contained in:
parent
52e97a5cdd
commit
7f51d63380
10 changed files with 517 additions and 17 deletions
114
prosthesis_controller/SimplexNoise.cpp
Normal file
114
prosthesis_controller/SimplexNoise.cpp
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
/*
|
||||
===============================================================================
|
||||
Ported the SimplexNoise algorithm from the C++ versions mentioned below
|
||||
to a reuseable Arduino Library.
|
||||
|
||||
By Jordan Shaw / http://jordanshaw.com / 2017-02
|
||||
|
||||
A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
|
||||
Based on example code by Stefan Gustavson (stegu@itn.liu.se).
|
||||
Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
|
||||
Better rank ordering method by Stefan Gustavson in 2012.
|
||||
C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
|
||||
This could be speeded up even further, but it's useful as it is.
|
||||
Version 2012-04-12
|
||||
The original Java code was placed in the public domain by its original author,
|
||||
Stefan Gustavson. You may use it as you see fit,
|
||||
but attribution is appreciated.
|
||||
|
||||
Original gist url: https://gist.github.com/Slipyx/2372043
|
||||
|
||||
===============================================================================
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "Arduino.h"
|
||||
#include "SimplexNoise.h"
|
||||
|
||||
// Private static member definitions
|
||||
const double SimplexNoise::F2 = 0.5 * (sqrt( 3.0 ) - 1.0);
|
||||
const double SimplexNoise::G2 = (3.0 - sqrt( 3.0 )) / 6.0;
|
||||
const uint8_t SimplexNoise::p[256] = {
|
||||
151,160,137,91,90,15,131,13,201,95,96,53,194,233,7,225,140,36,103,30,69,
|
||||
142,8,99,37,240,21,10,23,190,6,148,247,120,234,75,0,26,197,62,94,252,219,
|
||||
203,117,35,11,32,57,177,33,88,237,149,56,87,174,20,125,136,171,168,68,175,
|
||||
74,165,71,134,139,48,27,166,77,146,158,231,83,111,229,122,60,211,133,230,
|
||||
220,105,92,41,55,46,245,40,244,102,143,54,65,25,63,161,1,216,80,73,209,76,
|
||||
132,187,208,89,18,169,200,196,135,130,116,188,159,86,164,100,109,198,173,
|
||||
186,3,64,52,217,226,250,124,123,5,202,38,147,118,126,255,82,85,212,207,206,
|
||||
59,227,47,16,58,17,182,189,28,42,223,183,170,213,119,248,152,2,44,154,163,
|
||||
70,221,153,101,155,167,43,172,9,129,22,39,253,19,98,108,110,79,113,224,232,
|
||||
178,185,112,104,218,246,97,228,251,34,242,193,238,210,144,12,191,179,162,
|
||||
241,81,51,145,235,249,14,239,107,49,192,214,31,181,199,106,157,184,84,204,
|
||||
176,115,121,50,45,127,4,150,254,138,236,205,93,222,114,67,29,24,72,243,141,
|
||||
128,195,78,66,215,61,156,180
|
||||
};
|
||||
const Grad SimplexNoise::grad3[12] = {
|
||||
Grad(1,1,0),Grad(-1,1,0),Grad(1,-1,0),Grad(-1,-1,0),Grad(1,0,1),
|
||||
Grad(-1,0,1),Grad(1,0,-1),Grad(-1,0,-1),Grad(0,1,1),Grad(0,-1,1),
|
||||
Grad(0,1,-1),Grad(0,-1,-1)
|
||||
};
|
||||
uint8_t SimplexNoise::perm[512] = {0};
|
||||
uint8_t SimplexNoise::permMod12[512] = {0};
|
||||
|
||||
// Initialize permutaion arrays
|
||||
void SimplexNoise::init() {
|
||||
for ( uint16_t i = 0; i < 512; ++i ) {
|
||||
perm[i] = p[i & 255];
|
||||
permMod12[i] = static_cast<uint8_t>(perm[i] % 12);
|
||||
}
|
||||
delete[] &p; // lol what
|
||||
}
|
||||
|
||||
// Fast floor
|
||||
int32_t SimplexNoise::fastFloor( double x ) {
|
||||
int32_t xi = static_cast<int32_t>(x);
|
||||
return x < xi ? xi - 1 : xi;
|
||||
}
|
||||
|
||||
double SimplexNoise::dot( const Grad& g, double x, double y ) {
|
||||
return g.x * x + g.y * y;
|
||||
}
|
||||
|
||||
// 2D SimplexNoise noise
|
||||
double SimplexNoise::noise( double xin, double yin ) {
|
||||
double s = (xin + yin) * F2;
|
||||
int32_t i = fastFloor( xin + s );
|
||||
int32_t j = fastFloor( yin + s );
|
||||
double t = (i + j) * G2;
|
||||
double x0 = xin - (i - t);
|
||||
double y0 = yin - (j - t);
|
||||
uint8_t i1 = 0, j1 = 1;
|
||||
if ( x0 > y0 ) {
|
||||
i1 = 1;
|
||||
j1 = 0;
|
||||
}
|
||||
double x1 = x0 - i1 + G2;
|
||||
double y1 = y0 - j1 + G2;
|
||||
double x2 = x0 - 1.0 + 2.0 * G2;
|
||||
double y2 = y0 - 1.0 + 2.0 * G2;
|
||||
uint8_t ii = i & 255;
|
||||
uint8_t jj = j & 255;
|
||||
uint8_t gi0 = permMod12[ii + perm[jj]];
|
||||
uint8_t gi1 = permMod12[ii + i1 + perm[jj + j1]];
|
||||
uint8_t gi2 = permMod12[ii + 1 + perm[jj + 1]];
|
||||
double n0 = 0.0;
|
||||
double t0 = 0.5 - x0 * x0 - y0 * y0;
|
||||
if ( t0 >= 0.0 ) {
|
||||
t0 *= t0;
|
||||
n0 = t0 * t0 * dot( grad3[gi0], x0, y0 );
|
||||
}
|
||||
double n1 = 0.0;
|
||||
double t1 = 0.5 - x1 * x1 - y1 * y1;
|
||||
if ( t1 >= 0.0 ) {
|
||||
t1 *= t1;
|
||||
n1 = t1 * t1 * dot( grad3[gi1], x1, y1 );
|
||||
}
|
||||
double n2 = 0.0;
|
||||
double t2 = 0.5 - x2 * x2 - y2 * y2;
|
||||
if ( t2 >= 0.0 ) {
|
||||
t2 *= t2;
|
||||
n2 = t2 * t2 * dot( grad3[gi2], x2, y2 );
|
||||
}
|
||||
return 70.0 * (n0 + n1 + n2);
|
||||
}
|
||||
54
prosthesis_controller/SimplexNoise.h
Normal file
54
prosthesis_controller/SimplexNoise.h
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
/*
|
||||
===============================================================================
|
||||
Ported the SimplexNoise algorithm from the C++ versions mentioned below
|
||||
to a reuseable Arduino Library.
|
||||
|
||||
By Jordan Shaw / http://jordanshaw.com / 2017-02
|
||||
|
||||
A C++ port of a speed-improved simplex noise algorithm for 2D in Java.
|
||||
Based on example code by Stefan Gustavson (stegu@itn.liu.se).
|
||||
Optimisations by Peter Eastman (peastman@drizzle.stanford.edu).
|
||||
Better rank ordering method by Stefan Gustavson in 2012.
|
||||
C++ port and minor type and algorithm changes by Josh Koch (jdk1337@gmail.com).
|
||||
This could be speeded up even further, but it's useful as it is.
|
||||
Version 2012-04-12
|
||||
The original Java code was placed in the public domain by its original author,
|
||||
Stefan Gustavson. You may use it as you see fit,
|
||||
but attribution is appreciated.
|
||||
|
||||
Original gist url: https://gist.github.com/Slipyx/2372043
|
||||
|
||||
===============================================================================
|
||||
*/
|
||||
|
||||
#ifndef SimplexNoise_h
|
||||
#define SimplexNoise_h
|
||||
|
||||
#include <math.h>
|
||||
#include "Arduino.h"
|
||||
|
||||
class Grad {
|
||||
public:
|
||||
Grad( int8_t x, int8_t y, int8_t z ) : x(x), y(y), z(z) {}
|
||||
int8_t x, y, z;
|
||||
};
|
||||
|
||||
class SimplexNoise {
|
||||
public:
|
||||
// Initialize permutation arrays
|
||||
static void init();
|
||||
// 2D simplex noise
|
||||
static double noise( double xin, double yin );
|
||||
|
||||
private:
|
||||
static int32_t fastFloor( double x );
|
||||
static double dot( const Grad& g, double x, double y );
|
||||
static const double F2;
|
||||
static const double G2;
|
||||
static const Grad grad3[12];
|
||||
static const uint8_t p[256];
|
||||
static uint8_t perm[512];
|
||||
static uint8_t permMod12[512];
|
||||
};
|
||||
|
||||
#endif
|
||||
186
prosthesis_controller/behaviour.cpp
Normal file
186
prosthesis_controller/behaviour.cpp
Normal file
|
|
@ -0,0 +1,186 @@
|
|||
#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 updateMatrixWeights(unsigned long millis, bool body_present,bool contact_main){
|
||||
mood.loneliness=1.0;
|
||||
|
||||
static unsigned long last_weights_update=0;
|
||||
if (millis-last_weights_update>WEIGHT_UPDATE_INTERVAL) {
|
||||
last_weights_update=millis;
|
||||
|
||||
static float contact_main_persist=0;
|
||||
if (contact_main) {
|
||||
contact_main_persist=1.0;
|
||||
}else{
|
||||
if (contact_main_persist>0) {
|
||||
contact_main_persist-=0.2*WEIGHT_UPDATE_INTERVAL/1000.0;
|
||||
}else{
|
||||
contact_main_persist=0;
|
||||
}
|
||||
}
|
||||
|
||||
//0=pitch
|
||||
//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.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;*/
|
||||
|
||||
|
||||
bool state_bellyup=false;
|
||||
bool state_map_mode_detached=false;
|
||||
bool state_map_mode_attached=false;
|
||||
|
||||
float map_mode_rollpitch=0;
|
||||
float map_mode_slither=0;
|
||||
float map_mode_sleeping=0;
|
||||
|
||||
|
||||
float contact_main_smooth=constrain(mapfloat(contact_main_persist,0.0,0.3, 0.0,1.0), 0.0,1.0);
|
||||
float map_mode_noise=1.0-contact_main_smooth; //idle movement
|
||||
float map_mode_scared=0.0+contact_main_smooth; //contracted position
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
float map_sum=map_mode_noise+map_mode_scared;
|
||||
map_mode_noise/=map_sum; //divide mapping so the sum will be 1.0
|
||||
map_mode_scared/=map_sum;
|
||||
|
||||
/*
|
||||
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_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;
|
||||
|
||||
matrix_weights_main[0][W_LOOKDIRECTION]=30* map_mode_scared;
|
||||
matrix_weights_main[1][W_LOOKDIRECTION]=30.0* map_mode_scared;
|
||||
matrix_weights_main[2][W_LOOKDIRECTION]=30.0* map_mode_scared;
|
||||
|
||||
|
||||
//Mode Roll Pitch
|
||||
matrix_weights_main[0][W_ROLL]=1.0* map_mode_rollpitch;
|
||||
matrix_weights_main[1][W_PITCH]=1.0* map_mode_rollpitch;
|
||||
matrix_weights_main[2][W_ROLL]=1.0* map_mode_rollpitch;
|
||||
|
||||
//Mode Slither
|
||||
matrix_weights_main[0][W_COS]=30 *map_mode_slither;
|
||||
matrix_weights_main[1][W_SIN]=30 *map_mode_slither;
|
||||
matrix_weights_main[2][W_COS]=-30 *map_mode_slither;
|
||||
|
||||
//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;
|
||||
|
||||
/*
|
||||
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);
|
||||
}
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void updateServosByWeights(unsigned long millis){
|
||||
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_MAX 100
|
||||
millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
|
||||
|
||||
float pitch=0;
|
||||
float roll=0;
|
||||
|
||||
float vsin=sin((millis()+millis_add)/1000.0);
|
||||
float vcos=cos((millis()+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
|
||||
|
||||
|
||||
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];
|
||||
|
||||
}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 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;
|
||||
}
|
||||
|
||||
43
prosthesis_controller/behaviour.h
Normal file
43
prosthesis_controller/behaviour.h
Normal file
|
|
@ -0,0 +1,43 @@
|
|||
#ifndef BEHAVIOUR_H_
|
||||
#define BEHAVIOUR_H_
|
||||
|
||||
|
||||
#include "SimplexNoise.h" //Library from https://github.com/jshaw/SimplexNoise
|
||||
#include "definitions.h"
|
||||
#include "servo.h"
|
||||
|
||||
extern dxl_servo servos[SERVO_COUNT];
|
||||
|
||||
|
||||
|
||||
#define SERVO_COUNT_MAIN 3
|
||||
#define SERVO_COUNT_PERIPHERAL 3
|
||||
|
||||
#define WEIGHT_COUNT 7 //enter number of weights here for array size
|
||||
|
||||
|
||||
#define WEIGHT_UPDATE_INTERVAL 100
|
||||
#define SERVOWEIGHT_UPDATE_INTERVAL SERVO_UPDATE_INTERVAL
|
||||
|
||||
|
||||
#define W_PITCH 0
|
||||
#define W_ROLL 1
|
||||
#define W_NOISE 2
|
||||
#define W_NOISESLOW 3
|
||||
#define W_SIN 4
|
||||
#define W_COS 5
|
||||
#define W_LOOKDIRECTION 6
|
||||
|
||||
|
||||
struct Mood{
|
||||
float shakiness;
|
||||
float wakefulness;
|
||||
float loneliness;
|
||||
};
|
||||
extern Mood mood;
|
||||
|
||||
void updateMatrixWeights(unsigned long millis,bool body_present, bool contact_main);
|
||||
void updateServosByWeights(unsigned long millis);
|
||||
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
|
||||
|
||||
#endif
|
||||
57
prosthesis_controller/bodytemp.cpp
Normal file
57
prosthesis_controller/bodytemp.cpp
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
#include "Arduino.h"
|
||||
#include "bodytemp.h"
|
||||
|
||||
Adafruit_MLX90614 mlx = Adafruit_MLX90614();
|
||||
|
||||
double bodytemp_ambient=0;
|
||||
double bodytemp_object=0;
|
||||
|
||||
bool body_present=false;
|
||||
|
||||
const double bodytemp_activate_temperature=30; //°C min object temperature to detect human
|
||||
const double bodytemp_activate_mindiff_ambient=3; //°C object temperature has to be at least this amount above ambient
|
||||
const double bodytemp_deactivate_maxdiff_ambient=2; //°C maximum difference between object and ambient temperature to not detect human anymore
|
||||
const unsigned long bodytemp_deacitvate_mintime=1000; //minimum time in ms between activate and deactivate
|
||||
|
||||
void initBodytemp() {
|
||||
Wire.begin(PIN_SDA,PIN_SCL);
|
||||
if (!mlx.begin(0x5A,&Wire)) {
|
||||
Serial.println("Error connecting to MLX sensor. Check wiring.");
|
||||
while (1);
|
||||
};
|
||||
}
|
||||
|
||||
void printBodytempDebug() {
|
||||
Serial.print("min:20,max:40,");
|
||||
Serial.print("Ambient:");Serial.print(bodytemp_ambient);
|
||||
Serial.print(",Object:");Serial.print(bodytemp_object);
|
||||
Serial.print(",Presence:");Serial.print(body_present?39:21);
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool checkBodypresence(unsigned long millis) {
|
||||
static unsigned long last_read_bodytemp=0;
|
||||
if (millis-last_read_bodytemp > BODYTEMP_READ_MIN_INTERVAL)
|
||||
{
|
||||
last_read_bodytemp = millis;
|
||||
|
||||
bodytemp_ambient = mlx.readAmbientTempC();
|
||||
bodytemp_object = mlx.readObjectTempC();
|
||||
|
||||
static unsigned long last_change_bodypresent=0;
|
||||
if (millis-last_change_bodypresent > bodytemp_deacitvate_mintime) {}
|
||||
if (!body_present) { //is currently not present
|
||||
if (bodytemp_object >= bodytemp_activate_temperature && (bodytemp_object-bodytemp_ambient) >= bodytemp_activate_mindiff_ambient) {
|
||||
body_present=true;
|
||||
last_change_bodypresent=millis;
|
||||
}
|
||||
}else{ //was present last time
|
||||
if((bodytemp_object-bodytemp_ambient) <= bodytemp_deactivate_maxdiff_ambient) {
|
||||
body_present=false;
|
||||
last_change_bodypresent=millis;
|
||||
}
|
||||
}
|
||||
}
|
||||
return body_present;
|
||||
}
|
||||
13
prosthesis_controller/bodytemp.h
Normal file
13
prosthesis_controller/bodytemp.h
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
#ifndef BODYTEMP_H_
|
||||
#define BODYTEMP_H_
|
||||
|
||||
#include <Adafruit_MLX90614.h> //Adafruit MLX90614 Library by Adafruit 2.1.5
|
||||
#include "definitions.h"
|
||||
|
||||
#define BODYTEMP_READ_MIN_INTERVAL 500
|
||||
|
||||
void initBodytemp();
|
||||
void printBodytempDebug() ;
|
||||
bool checkBodypresence(unsigned long millis);
|
||||
|
||||
#endif
|
||||
|
|
@ -22,7 +22,7 @@
|
|||
#define PIN_OUT7 8 //PWM
|
||||
#define PIN_OUT8 9 //PWM
|
||||
|
||||
#define PIN_BUTTON 23
|
||||
#define PIN_BUTTON BOARD_BUTTON_PIN //BOARD_BUTTON_PIN = 23
|
||||
|
||||
#define PIN_PUMP1 PIN_OUT5
|
||||
#define PIN_PUMP2 PIN_OUT7
|
||||
|
|
@ -32,6 +32,11 @@
|
|||
|
||||
#define PIN_VIBRATION PIN_OUT2
|
||||
|
||||
#define PIN_SDA 25
|
||||
#define PIN_SCL 24
|
||||
|
||||
#define PIN_LED BOARD_LED_PIN //BOARD_LED_PIN=14
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -1,6 +1,8 @@
|
|||
#include "definitions.h"
|
||||
#include "vacuum.h"
|
||||
#include "servo.h"
|
||||
#include "bodytemp.h"
|
||||
#include "behaviour.h"
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
|
@ -9,11 +11,16 @@ void setup() {
|
|||
analogWrite(PIN_VIBRATION,0);
|
||||
|
||||
pinMode(PIN_BUTTON,INPUT_PULLDOWN);
|
||||
pinMode(PIN_LED,OUTPUT);
|
||||
digitalWrite(PIN_LED,HIGH); //LOW=Light, HIGH=Off
|
||||
|
||||
initVacuum();
|
||||
initBodytemp();
|
||||
|
||||
|
||||
pinMode(PIN_INPUTARM1,INPUT);
|
||||
pinMode(PIN_INPUTARM2,INPUT);
|
||||
pinMode(PIN_INPUTARM3,INPUT);
|
||||
|
||||
pinMode(PIN_INPUTAUX,INPUT);
|
||||
|
||||
|
|
@ -23,20 +30,34 @@ void setup() {
|
|||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void loop() {
|
||||
unsigned long loopmillis=millis();
|
||||
|
||||
bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button
|
||||
setVacuum(vac);
|
||||
|
||||
|
||||
bool body_present=checkBodypresence(loopmillis);
|
||||
digitalWrite(PIN_LED,!body_present);
|
||||
|
||||
loopVacuum(loopmillis);
|
||||
|
||||
|
||||
bool buttonstate=digitalRead(PIN_BUTTON); //Temporary Vacuum Button. HIGH=Pressed
|
||||
if (buttonstate) { //Button pressed
|
||||
setVacuum(false); //release
|
||||
}else{
|
||||
setVacuum(body_present);
|
||||
}
|
||||
|
||||
bool contact_main=digitalRead(PIN_INPUTARM1); //Sensor on tip of arm
|
||||
|
||||
updateMatrixWeights(loopmillis,body_present,contact_main);
|
||||
updateServosByWeights(loopmillis);
|
||||
|
||||
loopServos(loopmillis);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//Print Debug Information
|
||||
static unsigned long last_print=0;
|
||||
if (loopmillis - last_print >100) {
|
||||
|
|
@ -45,9 +66,11 @@ void loop() {
|
|||
//Serial.print(" Vac="); Serial.print(vac);
|
||||
|
||||
printServoDebug();
|
||||
|
||||
|
||||
Serial.println();
|
||||
//printBodytempDebug();
|
||||
|
||||
|
||||
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -3,10 +3,10 @@
|
|||
|
||||
using namespace DYNAMIXEL;
|
||||
|
||||
#define SERVO_COUNT 3
|
||||
const uint8_t DXL_ID = 1;
|
||||
const float DXL_PROTOCOL_VERSION = 2.0;
|
||||
|
||||
|
||||
dxl_servo servos[SERVO_COUNT];
|
||||
|
||||
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
|
||||
|
|
@ -100,18 +100,15 @@ void initServos()
|
|||
|
||||
|
||||
void loopServos(unsigned long millis) {
|
||||
servos[0].angle=sin(millis/1000.0/1.0)*20;
|
||||
/*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;
|
||||
servos[2].angle=sin(millis/1000.0/1.0)*20;*/
|
||||
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){
|
||||
last_servo_update=millis;
|
||||
//for (uint8_t i=0;i<SERVO_COUNT;i++) {
|
||||
//dxl.setGoalPosition(servos[i].id, servos[i].offset_angle+servos[i].angle*servos[i].orientation, UNIT_DEGREE);
|
||||
//}
|
||||
|
||||
dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE);
|
||||
dxl.setGoalPosition(servos[servoUpdateI].id, servos[servoUpdateI].offset_angle+servos[servoUpdateI].angle*servos[servoUpdateI].orientation, UNIT_DEGREE); //asdf
|
||||
servoUpdateI++;
|
||||
servoUpdateI%=SERVO_COUNT;
|
||||
}
|
||||
|
|
@ -120,7 +117,8 @@ void loopServos(unsigned long millis) {
|
|||
|
||||
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(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.println();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,6 +18,11 @@
|
|||
#define SERVO_UPDATE_INTERVAL 1000/50
|
||||
|
||||
|
||||
#define SERVO_COUNT 3
|
||||
|
||||
|
||||
|
||||
|
||||
struct dxl_servo {
|
||||
int id;
|
||||
//int initial_speed;
|
||||
|
|
@ -31,6 +36,8 @@ struct dxl_servo {
|
|||
uint16_t modelnumber;
|
||||
};
|
||||
|
||||
|
||||
|
||||
void scanDynamixel(int32_t baud, uint8_t protocol,uint8_t maxid);
|
||||
void scanDynamixel();
|
||||
void changeID(uint8_t present_id, uint8_t new_id);
|
||||
|
|
|
|||
Loading…
Reference in a new issue