roll pitch control and automatic self movement

noise.cpp

@@ -0,0 +1,86 @@
+
+#include "noise.hpp"
+
+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;
+}
+
+
+//using the algorithm from http://freespace.virgin.net/hugo.elias/models/m_perlin.html
+// thanks to hugo elias
+float Noise2(float x, float y)
+{
+  long noise;
+  noise = x + y * 57;
+  //noise = pow(noise << 13,noise);
+  noise = (noise << 13)*noise;
+  return ( 1.0 - ( long(noise * (noise * noise * 15731L + 789221L) + 1376312589L) & 0x7fffffff) / 1073741824.0);
+}
+
+float SmoothNoise2(float x, float y)
+{
+  float corners, sides, center;
+  corners = ( Noise2(x-1, y-1)+Noise2(x+1, y-1)+Noise2(x-1, y+1)+Noise2(x+1, y+1) ) / 16;
+  sides   = ( Noise2(x-1, y)  +Noise2(x+1, y)  +Noise2(x, y-1)  +Noise2(x, y+1) ) /  8;
+  center  =  Noise2(x, y) / 4;
+  return (corners + sides + center);
+}
+
+float InterpolatedNoise2(float x, float y)
+{
+  float v1,v2,v3,v4,i1,i2,fractionX,fractionY;
+  long longX,longY;
+
+  longX = long(x);
+  fractionX = x - longX;
+
+  longY = long(y);
+  fractionY = y - longY;
+
+  v1 = SmoothNoise2(longX, longY);
+  v2 = SmoothNoise2(longX + 1, longY);
+  v3 = SmoothNoise2(longX, longY + 1);
+  v4 = SmoothNoise2(longX + 1, longY + 1);
+
+  i1 = Interpolate(v1 , v2 , fractionX);
+  i2 = Interpolate(v3 , v4 , fractionX);
+
+  
+  return(Interpolate(i1 , i2 , fractionY));
+}
+
+float Interpolate(float a, float b, float x)
+{
+  //cosine interpolations
+  return(CosineInterpolate(a, b, x));
+}
+
+float LinearInterpolate(float a, float b, float x)
+{
+  return(a*(1-x) + b*x);
+}
+
+float CosineInterpolate(float a, float b, float x)
+{
+  float ft = x * 3.1415927;
+  float f = (1 - cos(ft)) * .5;
+
+  return(a*(1-f) + b*f);
+}
+
+float PerlinNoise2(float x, float y, float persistance, int octaves)
+{
+  float frequency, amplitude;
+  float total = 0.0;
+
+  for (int i = 0; i <= octaves - 1; i++)
+  {
+    frequency = pow(2,i);
+    amplitude = pow(persistance,i);
+
+    total = total + InterpolatedNoise2(x * frequency, y * frequency) * amplitude;
+  }
+
+  return(total);
+}

noise.hpp

@@ -0,0 +1,16 @@
+#include <math.h>
+
+#ifndef NOISE_HPP_
+#define NOISE_HPP_
+
+float mapfloat(float x, float in_min, float in_max, float out_min, float out_max);
+
+float Noise2(float x, float y);
+float SmoothNoise2(float x, float y);
+float InterpolatedNoise2(float x, float y);
+float Interpolate(float a, float b, float x);
+float LinearInterpolate(float a, float b, float x);
+float CosineInterpolate(float a, float b, float x);
+float PerlinNoise2(float x, float y, float persistance, int octaves);
+
+#endif

protesis_03_dxl.ino

@@ -3,6 +3,7 @@
 #include "imu.hpp"
 #include "servo-control.hpp"
 #include "movement.hpp"
+#include "noise.hpp"
 
 #define DENOISE_PREVIOUS_INERTIA_RATE 0.9999
 #define DENOISE_CURRENT_INERTIA_RATE 0.0001
@@ -42,12 +43,13 @@ void print_loop_time_micros();
 void print_servo_torques();
 
 
+
 void setup() {
 	Serial.begin(115200);
   
   Serial.println("booting");
   
-  Serial.println("setupr registers");
+  Serial.println("setup registers");
   setup_mpu_6050_registers();
   Serial.println("calc mpu offsets");
 	calculate_mpu_offsets_as_average();
@@ -61,25 +63,29 @@ void setup() {
 	//servos_init_with_current_position();
   Serial.println("wait servos");
 	servos_in_position_wait();
-	pinMode(switch_programm, INPUT);
+	pinMode(switch_programm, INPUT_PULLUP);
 	
 	current_timestamp_us = micros();
 
+
+
+  matrix_weights_update(); //initialize weights matrix
   Serial.println("booted");
   
 }
 
 void loop() {
-  Serial.print("state_switch_programm=");
-  Serial.println(state_switch_programm);
 
 
   int state_switch_programm = digitalRead(switch_programm);
+  
   //Serial.println(digitalRead(switch_programm));
 	if(state_switch_programm == 1) {
     slither();
 	} else {
-    move_protesis_with_sensor_data();
+    //move_protesis_with_sensor_data();
+    update_mood();
+    move_protesis_organic();
    
 	}
 
@@ -89,6 +95,22 @@ void loop() {
 
 }
 
+void update_mood() {  
+  #define GYROSCOPE_FILTER_SHAKINESS 0.8
+  float gyro=constrain(sqrt(pow(current_mpu_measurement.gyroscope.x,2)+pow(current_mpu_measurement.gyroscope.y,2)) / 10000.0   ,0.0, 1.0);
+  mood.shakiness = constrain(mood.shakiness*GYROSCOPE_FILTER_SHAKINESS + (1-GYROSCOPE_FILTER_SHAKINESS)*gyro, 0,1.) ;
+  #define GYROSCOPE_FILTER_WAKEFULNESS 0.99
+  mood.wakefulness = constrain(mood.wakefulness*GYROSCOPE_FILTER_WAKEFULNESS + (1-GYROSCOPE_FILTER_WAKEFULNESS)*gyro, 0,1.);
+  
+  Serial.print(gyro);
+  Serial.print(", ");
+  Serial.print(mood.shakiness);
+  Serial.print(", ");
+  Serial.println(mood.wakefulness);  
+
+  matrix_weights_update();
+}
+
 void move_protesis_with_sensor_data() {
 	read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
 	calculate_inertia(current_mpu_inertia, current_mpu_measurement);
@@ -111,6 +133,22 @@ void move_protesis_with_sensor_data() {
 	//print_servo_torques();
 }
 
+void move_protesis_organic() {
+  read_mpu_6050_data_with_offset_substraction(current_mpu_measurement);
+	calculate_inertia(current_mpu_inertia, current_mpu_measurement);
+	correct_inertia_drift(current_mpu_inertia);
+	denoise_inertia_with_complementary_filter();
+	//substract_gyroscope_offset(current_mpu_measurement);
+	//mpu_measurement_with_precission(current_mpu_measurement_dp, current_mpu_measurement);
+	//denoise_mpu_measurement_with_complementary_filter();
+
+	//servos_collect_current_torque();
+  map_servos_by_weights_with_initial_position_and_move(current_mpu_inertia.roll_acceleration,current_mpu_inertia.pitch_acceleration);
+	
+
+
+}
+
 void denoise_inertia_with_complementary_filter() {
 	current_mpu_inertia.roll_gyroscope = previous_mpu_inertia.roll_gyroscope
 			* DENOISE_PREVIOUS_INERTIA_RATE

servo-control.cpp

@@ -10,17 +10,66 @@
 int angle_to_dxl_servo_position(const int deg);
 int rpm_to_dxl_servo_speed(const int rpm);
 
+
+Mood mood;
+
+float matrix_weights[SERVO_COUNT][3]; //mapping outputs, sensors/moods
+#define W_PITCH 0
+#define W_ROLL 1
+#define W_NOISE 2
+
+
+void matrix_weights_update(){
+  //0=pitch
+  //1=roll
+  
+
+  float map_mode=constrain(mapfloat(mood.wakefulness,0,0.2, 1,0),0,1); //0=pitchroll control, 1=noise
+  
+  
+  
+
+  //testing on servo
+  matrix_weights[0][W_NOISE]=90.0*map_mode;
+  matrix_weights[0][W_ROLL]=1.0* (1-map_mode);
+
+  matrix_weights[1][W_NOISE]=90.0*map_mode;
+  matrix_weights[1][W_PITCH]=1.0* (1-map_mode);
+
+  matrix_weights[2][W_NOISE]=90.0*map_mode;
+  matrix_weights[2][W_ROLL]=1.0* (1-map_mode);
+
+  matrix_weights[3][W_NOISE]=90.0*map_mode;
+  matrix_weights[3][W_PITCH]=1.0* (1-map_mode);
+
+  matrix_weights[4][W_NOISE]=90.0*map_mode;
+  matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
+
+
+  
+  float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 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;
+  }
+  
+
+	
+  
+}
+
 dxl_servo servos[SERVO_COUNT];
 
 void servos_set_current_initial_position() {
 	for(int i = 0; i < SERVO_COUNT ; i++){
-		servos[i].intial_position = dxlGetPosition(servos[i].id);
+		servos[i].initial_position = dxlGetPosition(servos[i].id);
 	}
 }
 
 void servos_setup() {
 	servos[0].id = 1;
-	servos[0].intial_position = DXL_NOSE_MOTOR_INITIAL_POSITION;
+	servos[0].initial_position = DXL_NOSE_MOTOR_INITIAL_POSITION;
 	servos[0].initial_speed = DXL_NOSE_MOTOR_INITIAL_SPEED;
 	servos[0].max_position = DXL_NOSE_MOTOR_MAX_POSITION_CCW;
 	servos[0].min_position = DXL_NOSE_MOTOR_MIN_POSITION_CW;
@@ -28,7 +77,7 @@ void servos_setup() {
 	servos[0].joint_orientation = - 1;
 
 	servos[1].id = 2;
-	servos[1].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
+	servos[1].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
 	servos[1].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
 	servos[1].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
 	servos[1].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
@@ -36,7 +85,7 @@ void servos_setup() {
 	servos[1].joint_orientation = - 1;
 
 	servos[2].id = 3;
-	servos[2].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
+	servos[2].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
 	servos[2].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
 	servos[2].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
 	servos[2].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
@@ -44,7 +93,7 @@ void servos_setup() {
 	servos[2].joint_orientation = + 1;
 
 	servos[3].id = 4;
-	servos[3].intial_position = DXL_MID_MOTOR_INITIAL_POSITION;
+	servos[3].initial_position = DXL_MID_MOTOR_INITIAL_POSITION;
 	servos[3].initial_speed = DXL_MID_MOTOR_INITIAL_SPEED;
 	servos[3].max_position = DXL_MID_MOTOR_MAX_POSITION_CCW;
 	servos[3].min_position = DXL_MID_MOTOR_MIN_POSITION_CW;
@@ -52,7 +101,7 @@ void servos_setup() {
 	servos[3].joint_orientation = + 1;
 
 	servos[4].id = 5;
-	servos[4].intial_position = DXL_BASE_MOTOR_INITIAL_POSITION;
+	servos[4].initial_position = DXL_BASE_MOTOR_INITIAL_POSITION;
 	servos[4].initial_speed = DXL_BASE_MOTOR_INITIAL_SPEED;
 	servos[4].max_position = DXL_BASE_MOTOR_MAX_POSITION_CCW;
 	servos[4].min_position = DXL_BASE_MOTOR_MIN_POSITION_CW;
@@ -78,7 +127,7 @@ void servos_init() {
 		dxlSetGoalSpeed(servos[i].id, servos[i].initial_speed);
 		dxlTorqueOn(servos[i].id);
 		dxlSetRunningTorqueLimit(servos[i].id,MOTOR_TORQUE_LIMIT / MOTOR_TORQUE_RATIO);
-		dxlSetGoalPosition(servos[i].id, servos[i].intial_position);
+		dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
 	}
 }
 
@@ -86,7 +135,7 @@ void servo_init(const int servo_id) {
 	int index = servo_id - 1;
 	dxlSetJointMode(servos[index].id, servos[index].min_position, servos[index].max_position);
 	dxlSetGoalSpeed(servos[index].id, servos[index].initial_speed);
-	dxlSetGoalPosition(servos[index].id, servos[index].intial_position);
+	dxlSetGoalPosition(servos[index].id, servos[index].initial_position);
 }
 
 void servo_to_initial_position(const int servo_id) {
@@ -95,7 +144,7 @@ void servo_to_initial_position(const int servo_id) {
 
 void servos_to_initial_position() {
 	for(int i = 0; i < SERVO_COUNT; i++) {
-		dxlSetGoalPosition(servos[i].id, servos[i].intial_position);
+		dxlSetGoalPosition(servos[i].id, servos[i].initial_position);
 	}
 }
 
@@ -135,7 +184,7 @@ void servo_move_to_angle(const int servo_id, const int deg) {
 }
 
 void map_angle_to_servo_with_initial_position(const int servo_id, const int deg) {
-	dxlSetGoalPosition(servo_id, servos[servo_id -1].intial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg));
+	dxlSetGoalPosition(servo_id, servos[servo_id -1].initial_position + servos[servo_id - 1].joint_orientation * angle_to_dxl_servo_position(deg));
 }
 
 void servos_move_to_angle(const int deg) {
@@ -166,18 +215,38 @@ void map_pitch_to_servos_and_move(const int pitch){
 }
 
 void map_roll_to_servos_with_initial_position_and_move(const int roll){
-	dxlSetGoalPosition(servos[0].id, servos[0].intial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll)));
+	dxlSetGoalPosition(servos[0].id, servos[0].initial_position + (servos[0].joint_orientation * angle_to_dxl_servo_position(roll)));
-	dxlSetGoalPosition(servos[2].id, servos[2].intial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll)));
+	dxlSetGoalPosition(servos[2].id, servos[2].initial_position + (servos[2].joint_orientation * angle_to_dxl_servo_position(roll)));
-	dxlSetGoalPosition(servos[4].id, servos[4].intial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll)));
+	dxlSetGoalPosition(servos[4].id, servos[4].initial_position + (servos[4].joint_orientation * angle_to_dxl_servo_position(roll)));
   delay(1);
 }
 
 void map_pitch_to_servos_with_initial_position_and_move(const int pitch){
-	dxlSetGoalPosition(servos[1].id, servos[1].intial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)));
+	dxlSetGoalPosition(servos[1].id, servos[1].initial_position + (servos[1].joint_orientation * angle_to_dxl_servo_position(pitch)));
-	dxlSetGoalPosition(servos[3].id, servos[3].intial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)));
+	dxlSetGoalPosition(servos[3].id, servos[3].initial_position + (servos[3].joint_orientation * angle_to_dxl_servo_position(pitch)));
   delay(1);
 }
 
+void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch){
+  
+  static unsigned long millis_add;
+  #define MILLISADD_MAX 100
+  millis_add+=constrain(mapfloat(mood.shakiness, 0.05,1 ,0,MILLISADD_MAX),0,MILLISADD_MAX);
+  //Serial.print(mood.shakiness);
+  //Serial.print(", ");
+  //Serial.println(millis_add);
+
+
+  for (uint8_t i=0;i<=SERVO_COUNT;i++) {
+    float pnoise=PerlinNoise2((millis()+millis_add)/1000.0,i*10,0.25,3); //x,y,persistance,octaves
+    int angle=0;
+    angle=pitch*matrix_weights[i][0]+roll*matrix_weights[i][1]+pnoise*matrix_weights[i][2];
+    dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));   
+  }
+  delay(1);
+}
+
+
 int servo_calculate_torque_with_direction(const int servo_id) {
 	int current_torque = dxlGetTorque(servo_id);
 	int direction = current_torque & MOTOR_TORQUE_DIRECTION_MASK;

servo-control.hpp

@@ -6,12 +6,19 @@
  */
 
 #include <ax12.h>
+#include "noise.hpp"
 
 #ifndef SERVO_CONTROL_HPP_
 #define SERVO_CONTROL_HPP_
 
 #define SERVO_COUNT 5
 
+struct Mood{
+  float shakiness;
+  float wakefulness;
+};
+extern Mood mood;
+
 #define RPM_TO_SPEED_RATIO 0.111
 #define ANGLE_PER_DIGIT_RATIO .2925
 //#define ANGLE_PER_DIGIT_RATIO 0.29			// From datasheet
@@ -60,7 +67,7 @@
 struct dxl_servo {
 	int id;
 	int initial_speed;
-	int intial_position;
+	int initial_position;
 	int min_position;
 	int max_position;
 	int torque_limit;
@@ -68,6 +75,8 @@ struct dxl_servo {
 	int joint_orientation;
 };
 
+void matrix_weights_update();
+
 void servos_setup();
 void servos_set_current_initial_position();
 
@@ -101,5 +110,6 @@ void map_angle_to_servo_with_initial_position(const int servo_id, const int deg)
 void map_roll_to_servos_with_initial_position_and_move(const int roll);
 void map_pitch_to_servos_with_initial_position_and_move(const int pitch);
 
+void map_servos_by_weights_with_initial_position_and_move(const int roll, const int pitch);
 
 #endif /* SERVO_CONTROL_HPP_ */