add matrix visualization

positionCapture/positionCapture.pde

@@ -15,6 +15,11 @@ String filename="";
 int recordingStartTime;
 int linesWritten;
 
+//Matrix Weights
+String mwheader[] = new String[32];
+float matrixweights[][] = new float[32][32]; //[servo][weight]
+int matrixrows=0; //when matrix received this will be the number of rows
+
 void setup() {
   size(800,600);
   background(0);
@@ -77,6 +82,32 @@ void draw() {
       }
       
       
+    }else if (list[0].equalsIgnoreCase("mwheader")) { //is matrix weights header message
+      for (int i=0;i<mwheader.length;i++) {
+        mwheader[i]=""; //clear header array
+      }
+      for (int i=1;i<list.length;i++) {
+        String entry=list[i].strip();
+        mwheader[i-1]=entry;        
+      }
+      matrixrows=list.length-1;
+      
+      print("mwheader size="); println(calcMWHeaderLength());
+      
+      
+      
+    }else if (list[0].equalsIgnoreCase("mw")) { //is matrix weights header message
+      int mwheaderlength=calcMWHeaderLength();
+      int s=0;
+      for (int i=1;i<list.length;i++) {        
+        float _number=parseFloat(list[i].replaceAll("[^0-9.]", "")); //remove non digit characters
+        int num=i-1; //position
+        int w=num%mwheaderlength; //weight array pos
+        s=num/mwheaderlength;
+        matrixweights[s][w]=_number;
+      }
+      matrixrows=s+1;
+      print("Matrixrows="); println(matrixrows);
     }
   }
   
@@ -85,22 +116,60 @@ void draw() {
   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);
+    text(filename,width-10,height-200+10+50+20);
+    text("t="+nf((millis()-recordingStartTime)/1000.0, 1,3) ,width-10,height-200+10+50+20+20);
+    text("lines="+linesWritten ,width-10,height-200+10+50+20+20+20);
     
     fill(255,0,0);
   }else{
     fill(50,50,50);
   }
-  rect(width-50-10,10,50,50);
+  ellipse(width-50-10,height-200+10,50,50);
   
   
+  //Bargraphs
   for (int i=0;i<servoPositionsActual.length;i++) {
     
     drawBargraph(servoPositionsActual[i],valRange,10,30+50*i);
   }
   
+  //Matrix Weights
+  int matrixPosX=10;
+  int matrixPosY=300;
+  int matrixCellW=100;
+  int matrixCellH=20;
+  int mwheaderlength=calcMWHeaderLength();
+  color(255);
+  textAlign(LEFT);
+  for (int s=0;s<matrixrows+1;s++){ //horizontal lines
+    line(matrixPosX,matrixPosY+matrixCellH*s,matrixPosX+matrixCellW*mwheaderlength ,matrixPosY+matrixCellH*s);
+  }
+  for (int w=0;w<mwheaderlength+1;w++){ //vertical lines
+    line(matrixPosX+matrixCellW*w,matrixPosY-matrixCellH,matrixPosX+matrixCellW*w ,matrixPosY+matrixCellH*matrixrows);
+  }
+  
+  for (int h=0;h<mwheaderlength;h++){
+    text(mwheader[h],matrixPosX+matrixCellW*h,matrixPosY); 
+  }
+  for (int s=0;s<matrixrows;s++){
+     for (int w=0;w<mwheaderlength;w++){
+       text(matrixweights[s][w],matrixPosX+matrixCellW*w,matrixPosY+matrixCellH*(s+1));
+     }
+     
+  }
+  
+    
+  
+}
+
+int calcMWHeaderLength() {
+  int count=0;
+  for (int i=0;i<mwheader.length;i++) {
+    if (mwheader[i]!="" && mwheader[i]!=null) {
+      count++;
+    }
+  }
+  return count;
 }
 
 void keyPressed() {
@@ -126,7 +195,21 @@ void keyPressed() {
         recording=false;
         println("Stopped Recording");
       }
+    }else if(keyCode==84){ //t
+      //Matrix
+      int mwheaderlength=calcMWHeaderLength();
+      for (int h=0;h<mwheaderlength;h++){
+        print(","); print(mwheader[h]); 
+      }
+      println();
+      for (int s=0;s<matrixrows;s++){
+         for (int w=0;w<mwheaderlength;w++){
+           print(","); print(matrixweights[s][w]);
+         }
+         println();
+      }
     }
+    
   }
 }
 

protesis_03_dxl/protesis_03_dxl.ino

@@ -77,8 +77,8 @@ 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;
+  static unsigned long last_loop_sendmatrix_millis;
 
   
 
@@ -103,6 +103,12 @@ void loop() {
     last_loop_readpos_millis=loopmillis;
     read_servos_positions(); //takes around 8000us for all servos. read and send over serial.
   }
+
+
+  if (loopmillis-last_loop_sendmatrix_millis>1000) {
+    last_loop_sendmatrix_millis=loopmillis;
+    transmit_matrix_weights();
+  }
   
 
 }

protesis_03_dxl/servo-control.cpp

@@ -16,14 +16,20 @@ int rpm_to_dxl_servo_speed(const int rpm);
 
 Mood mood;
 
-float matrix_weights[SERVO_COUNT][6]; //mapping outputs, sensors/moods
+#define WEIGHT_COUNT 6 //enter number of weights here for array size
+float matrix_weights[SERVO_COUNT][WEIGHT_COUNT]; //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
+String weightNames[] = {"Pitch","Roll","Noise","NoiseSlow","Sin","Cos"};
 
+unsigned long last_overloaderror_millis;
+
+
+dxl_servo servos[SERVO_COUNT];
 
 void matrix_weights_update(){
   //0=pitch
@@ -101,13 +107,32 @@ void matrix_weights_update(){
     matrix_weights[i][W_NOISESLOW]*=constrain(sleeping,0.1,1);
   }
   */
-  
-
-	
-  
 }
 
-dxl_servo servos[SERVO_COUNT];
+
+bool trasmitMatrixWeightsHeaderFlag=false;
+void transmit_matrix_weights() {
+  if (!trasmitMatrixWeightsHeaderFlag) { //transmit header only one time
+    trasmitMatrixWeightsHeaderFlag=true;
+    Serial.print("mwheader"); //matrixweight header
+    for (int i=0;i<WEIGHT_COUNT;i++) {
+      Serial.print(",");
+      Serial.print(weightNames[i]);
+    }    
+    Serial.println();
+  }
+
+  Serial.print("mw"); //matrixweights
+  for (int s=0;s<SERVO_COUNT;s++) {
+    for (int w=0;w<WEIGHT_COUNT;w++) {
+      Serial.print(",");
+      Serial.print(matrix_weights[s][w]);
+    }
+  }  
+  Serial.println();
+
+}
+
 
 void servos_set_current_initial_position() {
 	for(int i = 0; i < SERVO_COUNT ; i++){
@@ -349,19 +374,31 @@ void read_servos_positions()
       Serial.print("Error present on "); Serial.print(i); Serial.print("errorcode="); Serial.println(errorcode);
     }
     if (CHECK_BIT(errorcode,5)) { //overload error
-      Serial.print("Init Servo "); Serial.println(i);
-      delay(1000);
-      dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
-      dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
-      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].initial_position);
-
-      
-      
+      if (last_overloaderror_millis==0) { //error just appeared
+        last_overloaderror_millis=millis();
+      }
     }
   }
+
+  #define DELAY_OVERLOADERROR_RESET 2000
+  if (millis()-last_overloaderror_millis>DELAY_OVERLOADERROR_RESET) { //wait before resetting
+    for(int i = 0; i< SERVO_COUNT; i++) {
+      int errorcode=errors[i];
+      if (CHECK_BIT(errorcode,5)) { //overload error
+        Serial.print("Init Servo "); Serial.println(i);
+        
+        dxlSetStatusReturnLevel(servos[i].id, AX_RETURN_READ); // Incorporated to avoid the colapsing of data found it with the osciloscope when the motors are sending data to te board
+        dxlSetJointMode(servos[i].id, servos[i].min_position, servos[i].max_position);
+        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].initial_position);
+        last_overloaderror_millis=0; //reset flag
+      
+      }
+    }
+  }
+
 }
 
 

protesis_03_dxl/servo-control.hpp

@@ -118,4 +118,7 @@ void map_servos_by_weights_with_initial_position_and_move(const int roll, const
 
 void read_servos_positions();
 
+void transmit_matrix_weights();
+
+
 #endif /* SERVO_CONTROL_HPP_ */