add helix movement

servo-control.cpp

@@ -7,17 +7,20 @@
 
 #include "servo-control.hpp"
 
+
 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][4]; //mapping outputs, sensors/moods
+float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
 #define W_PITCH 0
 #define W_ROLL 1
 #define W_NOISE 2
 #define W_NOISESLOW 3
+#define W_SIN 4
+#define W_COS 5
 
 
 void matrix_weights_update(){
@@ -26,31 +29,45 @@ void matrix_weights_update(){
   
 
   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
+
   
   
   
 
   //testing on servo
+  matrix_weights[0][W_COS]=30 *map_mode;
   matrix_weights[0][W_NOISE]=90.0 *map_mode;
-  matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
+  matrix_weights[1][W_SIN]=30 *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_NOISESLOW]=90.0*map_mode;
+  matrix_weights[2][W_COS]=-30 *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_NOISESLOW]=90.0*map_mode;
+  matrix_weights[3][W_SIN]=-30 *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_NOISESLOW]=90.0*map_mode;
+  matrix_weights[4][W_COS]=30 *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_NOISESLOW]=90.0*map_mode;
+
+
+  matrix_weights[0][W_ROLL]=1.0* (1-map_mode); 
+  matrix_weights[1][W_PITCH]=1.0* (1-map_mode);  
+  matrix_weights[2][W_ROLL]=1.0* (1-map_mode);  
+  matrix_weights[3][W_PITCH]=1.0* (1-map_mode);  
   matrix_weights[4][W_ROLL]=1.0* (1-map_mode);
 
+  /*
+  matrix_weights[0][W_NOISE]=90.0*map_mode;
+  matrix_weights[0][W_NOISESLOW]=90.0*map_mode;
+  matrix_weights[1][W_NOISE]=90.0*map_mode;
+  matrix_weights[1][W_NOISESLOW]=90.0*map_mode;
+  matrix_weights[2][W_NOISE]=90.0*map_mode;
+  matrix_weights[2][W_NOISESLOW]=90.0*map_mode;
+  matrix_weights[3][W_NOISE]=90.0*map_mode;
+  matrix_weights[3][W_NOISESLOW]=90.0*map_mode;
+  matrix_weights[4][W_NOISE]=90.0*map_mode;
+  matrix_weights[4][W_NOISESLOW]=90.0*map_mode;
+
+  
+  */
 
   
   float sleeping=constrain(mapfloat(mood.wakefulness,0.005,0.02, 0,1),0,1);
@@ -241,13 +258,21 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
   //Serial.print(mood.shakiness);
   //Serial.print(", ");
   //Serial.println(millis_add);
-
+  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++) {
     float pnoise=PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
     float pnoiseslow=PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
+
     int angle=0;
-    angle=pitch*matrix_weights[i][W_PITCH]+roll*matrix_weights[i][W_ROLL]+pnoise*matrix_weights[i][W_NOISE]+pnoiseslow*matrix_weights[i][W_NOISESLOW];
+    angle=
+      pitch*matrix_weights[i][W_PITCH]
+      +roll*matrix_weights[i][W_ROLL]
+      +pnoise*matrix_weights[i][W_NOISE]
+      +pnoiseslow*matrix_weights[i][W_NOISESLOW]
+      +vsin*matrix_weights[i][W_SIN]
+      +vcos*matrix_weights[i][W_COS];
     dxlSetGoalPosition(servos[i].id, servos[i].initial_position + (servos[i].joint_orientation * angle_to_dxl_servo_position(angle)));   
   }
   delay(1);

servo-control.hpp

@@ -23,7 +23,7 @@ extern Mood mood;
 #define ANGLE_PER_DIGIT_RATIO .2925
 //#define ANGLE_PER_DIGIT_RATIO 0.29			// From datasheet
 
-#define MOTOR_TORQUE_LIMIT 75
+#define MOTOR_TORQUE_LIMIT 100
 #define MOTOR_TORQUE_RATIO 0.1
 #define MOTOR_TORQUE_DIRECTION_MASK 0x00000200
 #define MOTOR_TORQUE_VALUE_MASK 0x000001ff