max output 30, seitpoint 0 on poweroff

Roboy · Jan 20, 2021 · 7912911 · 7912911
1 parent 5e097f3
commit 7912911
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 3 deletions.
diff --git a/include/roboy_plexus/roboyPlexus.hpp b/include/roboy_plexus/roboyPlexus.hpp
@@ -189,6 +189,10 @@ class RoboyPlexus {
      */
     void RoboyStatePublisher();
 
+    /**
+     * Checks if any of the motors were turned off and resets its setpoint to 0 if yes.
+     */
+    void MotorPowerTracker();
     /**
      * Service for system check
      * @param req
@@ -232,7 +236,7 @@ class RoboyPlexus {
     int32_t *power_control, *power_sense, *switches, *led;
     vector<A1335Ptr> a1335_elbow, a1335_knee;
     boost::shared_ptr<std::thread> elbowJointAngleThread, kneeJointAngleThread,
-        magneticsThread, motorInfoThread, motorStateThread, roboyStateThread;
+        magneticsThread, motorInfoThread, motorStateThread, roboyStateThread, powerTrackerThread;
     bool keep_publishing = true;
     vector<TLE493DPtr> balljoints;
     bool emergency_stop = false;

diff --git a/src/interfaces/iCEbusControl.cpp b/src/interfaces/iCEbusControl.cpp
@@ -228,7 +228,7 @@ void IcebusControl::GetDefaultControlParams(control_Parameters_t *params, int co
             params->Ki = 1;
             params->Kd = 0;
             params->deadband = 0;
-            params->PWMLimit = 50; // 10% of max pwm
+            params->PWMLimit = 30; // 30% of max pwm
           }else if(muscleType=="m3"){
             params->IntegralLimit = 100;
             params->Kp = 10;

diff --git a/src/roboyPlexus.cpp b/src/roboyPlexus.cpp
@@ -96,6 +96,7 @@ RoboyPlexus::RoboyPlexus(string robot_name, IcebusControlPtr icebusControl,
     roboyStateThread = boost::shared_ptr<std::thread>(new std::thread(&RoboyPlexus::RoboyStatePublisher, this));
     roboyStateThread->detach();
 
+
     neopixel_sub = nh->subscribe(robot_name + "middleware/Neopixel", 1, &RoboyPlexus::Neopixel, this);
     emergencyStop_srv = nh->advertiseService(robot_name + "middleware/EmergencyStop",
                                              &RoboyPlexus::EmergencyStopService,
@@ -139,6 +140,9 @@ RoboyPlexus::RoboyPlexus(string robot_name, IcebusControlPtr icebusControl,
       tli4970.reset(new TLI4970(tli4970_base));
     }
 
+    powerTrackerThread = boost::shared_ptr<std::thread>(new std::thread(&RoboyPlexus::MotorPowerTracker, this));
+    powerTrackerThread->detach();
+
     ROS_INFO("roboy plexus initialized");
 }
 
@@ -147,6 +151,8 @@ RoboyPlexus::~RoboyPlexus() {
         motorStateThread->join();
     if (motorInfoThread->joinable())
         motorInfoThread->join();
+    if (powerTrackerThread->joinable())
+        powerTrackerThread->join();
 }
 
 void RoboyPlexus::MotorStatePublisher() {
@@ -167,7 +173,9 @@ void RoboyPlexus::MotorStatePublisher() {
             msg.encoder1_pos.push_back(bus->GetEncoderPosition(m.first,ENCODER1_POSITION)*m.second->encoder1_conversion_factor);
             msg.displacement.push_back(bus->GetDisplacement(m.first));
             // msg.displacement.push_back(msg.encoder0_pos.back()-msg.encoder1_pos.back());
-            msg.current.push_back(bus->GetCurrent(m.first));
+            auto current = bus->GetCurrent(m.first);
+            msg.current.push_back(current);
+
           }
         }
       }
@@ -252,6 +260,28 @@ void RoboyPlexus::RoboyStatePublisher(){
   }
 }
 
+void RoboyPlexus::MotorPowerTracker(){
+    ros::Rate rate(200);
+    while(ros::ok()) {
+
+        for(auto &bus:motorControl){
+            for(auto motor:bus->motor_config->motor) {
+                if(bus->MyMotor(motor.first)) {
+                    if (bus->GetCommunicationQuality(motor.first) > 0) {
+                        motor.second->is_on = true;
+                    }
+                    if (motor.second->is_on && bus->GetCommunicationQuality(motor.first) <= 0) {
+                        bus->SetPoint(motor.first, 0);
+                        motor.second->is_on = false;
+                        ROS_INFO("motor %d on %s is off - resetting setpoint to 0.", motor.second->motor_id_global, bus->whoami().c_str());
+                    }
+                }
+            }
+        }
+        rate.sleep();
+    }
+}
+
 void RoboyPlexus::Neopixel(const roboy_middleware_msgs::Neopixel::ConstPtr &msg){
   uint i = 0;
   external_led_control = true;