add position capture and simple viewer processing software

positionCapture/positionCapture.pde

@@ -0,0 +1,161 @@
+import processing.serial.*;
+String serial_port="COM3";
+Serial serial;
+String serialString=""; //last read string
+int serial_endchar=10; //10=ASCII Linefeed
+
+static int SERVO_COUNT=5;
+int servoPositionsActual[]=new int[SERVO_COUNT];
+
+PrintWriter fileoutput;
+String recorddir="recordings/";
+
+boolean recording=false;
+String filename="";
+int recordingStartTime;
+int linesWritten;
+
+void setup() {
+  size(800,600);
+  background(0);
+  
+
+  printArray(Serial.list());
+  // Open the port you are using at the rate you want:
+  serial = new Serial(this, serial_port, 57600);
+  
+  serial.clear();
+  // Throw out the first reading, in case we started reading 
+  // in the middle of a string from the sender.
+  println("readUntil");
+  serialString = serial.readStringUntil(serial_endchar);
+  println("read:"+serialString);
+  serialString = null;
+
+
+  
+}
+
+void draw() {
+  clear();
+  int valRange=1024; //maximum value for bargraph scale
+  
+  String[] list=readSerial();
+  if (list!=null) {
+    
+    if (list[0].equalsIgnoreCase("dxlgp")) { //is position message
+        
+      boolean firstValueWritten=false;
+      for (int i=1;i<list.length;i++) {
+        String _number=list[i].replaceAll("[^0-9.]", ""); //remove non digit characters
+        print(i); print("="); print(_number); print(" parsed="); println(parseInt(_number));
+        servoPositionsActual[i-1]=parseInt(_number);
+         
+         
+        if (recording) {
+          if (!firstValueWritten) {
+            fileoutput.print( (millis()-recordingStartTime)/1000.0); //write time
+          }else{
+            fileoutput.print(","); 
+          }
+          fileoutput.print(_number);
+        }
+        firstValueWritten=true;
+        
+      }
+      
+      if (recording) {
+        linesWritten++;
+        fileoutput.println();
+        fileoutput.flush();
+      }
+      
+        
+      
+      
+      
+    }
+  }
+  
+
+  //Draw Recording Info
+  if (recording) {
+    textSize(16);
+    textAlign(RIGHT);
+    text(filename,width-10,10+50+20);
+    text("t="+nf((millis()-recordingStartTime)/1000.0, 1,3) ,width-10,10+50+20+20);
+    text("lines="+linesWritten ,width-10,10+50+20+20+20);
+    
+    fill(255,0,0);
+  }else{
+    fill(50,50,50);
+  }
+  rect(width-50-10,10,50,50);
+  
+  
+  for (int i=0;i<servoPositionsActual.length;i++) {
+    
+    drawBargraph(servoPositionsActual[i],valRange,10,30+50*i);
+  }
+  
+}
+
+void keyPressed() {
+  if (key == CODED) {
+    
+  }else{
+    //print("Code=");println(keyCode);
+    if (keyCode==82) { //r
+      if (!recording) {
+        filename=year()+"-"+month()+"-"+day()+"_"+hour()+"-"+minute()+"-"+second()+".txt";
+        fileoutput = createWriter(recorddir+""+filename);
+        print("Recording started. Filename: "); println(filename);
+        recording=true;
+        recordingStartTime=millis();
+        
+        //Write header
+        fileoutput.println("time,servo0,servo1,servo2,servo3,servo4");
+      }
+      
+    }else if(keyCode==83) { //s
+      if (recording) {
+        fileoutput.close();
+        recording=false;
+        println("Stopped Recording");
+      }
+    }
+  }
+}
+
+
+
+
+void drawBargraph(float val, float maxvalue, int posX, int posY) {
+  int graphW=300;
+  int graphH=20;
+  
+  fill(255);
+  rect(posX,posY,val*graphW/maxvalue,graphH); //graph
+  
+  noFill();
+  stroke(127);
+  rect(posX,posY,graphW,graphH); //border
+  
+  textAlign(CENTER);
+  textSize(20);
+  text(val,posX+graphW/2,posY);
+}
+
+
+String[] readSerial() {
+  if (serial.available() > 0) {
+      serialString = serial.readStringUntil(serial_endchar);
+      //println("read:"+serialString);
+      if (serialString != null) {
+        println(serialString);
+        String[] list = split(serialString, ',');
+        return list;
+      }
+  }
+  return null;
+}

protesis_03_dxl/protesis_03_dxl.ino

@@ -18,7 +18,6 @@ inertia previous_mpu_inertia;
 measurement_double_precission previous_mpu_measurement_dp;
 measurement_double_precission current_mpu_measurement_dp;
 
-unsigned long current_timestamp_us;
 unsigned long loop_counter = 0;
 int servo_id;
 int roll = 125;
@@ -45,7 +44,7 @@ void print_servo_torques();
 
 
 void setup() {
-	Serial.begin(115200);
+	Serial.begin(57600);
   
   Serial.println("booting");
   
@@ -65,7 +64,6 @@ void setup() {
 	servos_in_position_wait();
 	pinMode(switch_programm, INPUT_PULLUP);
 	
-	current_timestamp_us = micros();
 
 
 
@@ -75,23 +73,32 @@ void setup() {
 }
 
 void loop() {
+  unsigned long loopmillis=millis();
+  static unsigned long last_loop_movement_millis;
+  static unsigned long last_loop_readpos_millis;
 
+  static unsigned long last_servo_send_millis;
 
-  int state_switch_programm = digitalRead(switch_programm);
+  if (loopmillis-last_loop_movement_millis>50) {
+    last_loop_movement_millis=loopmillis;
+    //int state_switch_programm = digitalRead(switch_programm);  //currently not used
+    //Serial.println(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(); //old movement function
+    //slither();
+
     update_mood();
     move_protesis_organic();
-   
-	}
+    last_servo_send_millis=millis();
 
-	while (micros() - current_timestamp_us < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
-	//print_loop_time_micros();
-	current_timestamp_us = micros();                      //Reset the loop timer
+    //Serial.println("moved servos");
+  }
+
+  if (loopmillis-last_loop_readpos_millis>100 && loopmillis-last_servo_send_millis>=4) { //limit frequency and minimum delay after send positions
+    last_loop_readpos_millis=loopmillis;
+    read_servos_positions(); //takes around 8000us for all servos
+  }
+  
 
 }
 
@@ -102,11 +109,12 @@ void update_mood() {
   #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(gyro);
   Serial.print(", ");
   Serial.print(mood.shakiness);
   Serial.print(", ");
   Serial.println(mood.wakefulness);  
+  */
 
   matrix_weights_update();
 }
@@ -255,11 +263,12 @@ void print_mpu_offset() {
 	Serial.println("]");
 }
 
-void print_loop_time_micros() {
+/*void print_loop_time_micros() {
 	Serial.print(loop_counter++);
 	Serial.print(" diff ms: ");
 	Serial.println(micros()-current_timestamp_us);
 }
+*/
 
 void print_servo_torques() {
 	servos_print_current_torque();

protesis_03_dxl/servo-control.cpp

@@ -33,9 +33,12 @@ void matrix_weights_update(){
 
   
   
+  matrix_weights[0][W_COS]=40.0;
+  matrix_weights[1][W_SIN]=-20.0;
   
 
   //testing on servo
+  /*
   matrix_weights[0][W_COS]=30 *map_mode;
   matrix_weights[0][W_NOISE]=90.0 *map_mode;
   matrix_weights[1][W_SIN]=30 *map_mode;
@@ -53,6 +56,7 @@ void matrix_weights_update(){
   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;
@@ -260,10 +264,12 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
   //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
+  
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    //unsigned long blastart=micros();
+    float pnoise=0;//PerlinNoise2((millis()+millis_add)/500.0,i*10,0.25,3); //x,y,persistance,octaves
+    float pnoiseslow=0;//PerlinNoise2((millis()+millis_add)/10000.0,i*10,0.20,3); //x,y,persistance,octaves
+    //Serial.print("perlin micros="); Serial.println(micros()-blastart);
 
     int angle=0;
     angle=
@@ -273,9 +279,29 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
       +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);
+  //delay(1);
+  
+}
+
+
+void read_servos_positions()
+{
+  Serial.print("dxlgp");
+  for (uint8_t i=0;i<SERVO_COUNT;i++) {
+    //Serial.print("servo "); Serial.print(i); Serial.print( "(id="); Serial.print(servos[i].id); Serial.print(")"); Serial.print("="); Serial.println(dxlGetPosition(servos[i].id));
+    
+    Serial.print(",");
+    
+    Serial.print(dxlGetPosition(servos[i].id)); 
+    
+  }
+  Serial.println();
 }
 
 

protesis_03_dxl/servo-control.hpp

@@ -112,4 +112,6 @@ 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);
 
+void read_servos_positions();
+
 #endif /* SERVO_CONTROL_HPP_ */