Updated arm teensy code to publish current angles

Teensy Code/ArmControl/ArmControl.ino

@@ -17,28 +17,29 @@
 #include "config.h"
 #include "controller.h"
 
-// Settings
-bool initArmDown = true;
-
 // ROS
-ros::NodeHandle  nh;
+ros::NodeHandle nh;
+
+std_msgs::Float32MultiArray arm_values;
+
+ros::Publisher pub_arm_values("/arm_current_angles", &arm_values);
 
 // format: [base, arm1, arm2]
-float motor_data[3] = {30, 40, 40};
+float motor_data[3] = {0, 0, 0};
 
 void messageArm( const std_msgs::Float32MultiArray& msg){
   if (msg.data_length < 3) return;
+  
   for (int i = 0; i < 3; i++) {
     motor_data[i] = msg.data[i];
   }
-  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
 }
 
-ros::Subscriber<std_msgs::Float32MultiArray> subscriberArm("arm", &messageArm );
+ros::Subscriber<std_msgs::Float32MultiArray> subscriberArm("/arm_demand_angles", &messageArm );
+
+int prevMillis;
 
 void setup() {
-  
-  //-Serial.begin(115200);
   nh.initNode();
   
   pinSetting();
@@ -48,41 +49,44 @@ void setup() {
   mainTime = millis();
   loopCountTime = micros();
   printTime = millis();
-
-  pinMode(13, OUTPUT);
-  digitalWrite(13, HIGH);
-  delay(500);
-  digitalWrite(13, LOW);
-
+  prevMillis = millis();
+
+  arm_values.layout.dim = (std_msgs::MultiArrayDimension *)
+  malloc(sizeof(std_msgs::MultiArrayDimension)*2);
+  arm_values.layout.dim[0].label = "height";
+  arm_values.layout.dim[0].size = 3;
+  arm_values.layout.dim[0].stride = 1;
+  arm_values.layout.data_offset = 0;
+  arm_values.data = (float *)malloc(sizeof(float)*8);
+  arm_values.data_length = 3;
+  
   nh.subscribe(subscriberArm);
 
-  
+  nh.advertise(pub_arm_values);
+
+  moveToZero();
 }
 
+
+
 void loop() {
   nh.spinOnce();
 
-  /*delay(1000);
-  if(initArmDown == true) {
-    // move arm down here 
-    moveToZero();
-
-    delay(3000);
-  }*/
-  
   float baseDemand = motor_data[0];  // demand for the base motor - in degrees
   float armDemand[2]; // demand for the two arm motors - in degrees
   armDemand[0] = motor_data[1];
   armDemand[1] = motor_data[2];
- 
-
-  // main loop
-  //while(true) {
-    
-    updateBasePosition(baseDemand);
-    updateMotorPower(armDemand);
-    //printStuff();  // uncomment and add what you want to print here
-  //}
-  
 
+  updateBasePosition(baseDemand);
+  updateMotorPower(armDemand);
+
+  if(millis() - prevMillis > 100) {
+    arm_values.data[0] = baseAngle;
+    arm_values.data[1] = encoderAngle[0];
+    arm_values.data[2] = encoderAngle[1];
+  
+    pub_arm_values.publish(&arm_values);
+  
+    prevMillis = millis();
+  }
 }

Teensy Code/ArmControl/controller.cpp

@@ -170,7 +170,8 @@ void updateMotorPower(float *demandAngle) {
     //if((digitalRead(openEndStop[i]) != 0 || dir[i] != forward)&& (digitalRead(closedEndStop[i]) != 0 && dir[i] != reverse)) {
         encoderAngle[i] = rawEncoderCount[i] * rawToReal; 
         encoderError[i] = demandAngle[i] - encoderAngle[i];
-      
+
+         
         power[i] = abs(kp * encoderError[i]);
     
         dir[i] = forward;
@@ -181,6 +182,9 @@ void updateMotorPower(float *demandAngle) {
         if(power[i] > basePower) {
           power[i] = basePower;
         }
+        else if(power[i] < 10) {
+          power[i] = 0;
+        }
 
 
         // control for the velocity