final VRPuppets merge

.gitmodules

@@ -22,9 +22,11 @@
 [submodule "hardware/stepper_control/kicad_db"]
 	path = hardware/stepper_control/kicad_db
 	url = https://github.com/Roboy/kicad_db
-[submodule "roboy_rqt_plugins"]
-	path = roboy_rqt_plugins
-	url = https://github.com/NexusReflex/roboy_rqt_plugins.git
+[submodule "hardware/m3"]
+	path = hardware/m3
+	url = https://github.com/NexusReflex/m3
 [submodule "src/cardsflow_rqt"]
 	path = src/cardsflow_rqt
-	url = https://github.com/CARDSflow/cardsflow_rqt
+	url = https://github.com/CARDSflow/cardsflow_rqt/tree/bad72bc588639221b638152ef9e11f5f302248d0
+
+

README.md

@@ -1,2 +1,42 @@
 # VRpuppet
 An actor struggles to die onstage, but a puppet has to struggle to live
+
+## Installation
+Clone the repo into your catkin workspace and build it   
+
+    $ cd path_to_catkin_ws/src
+    $ git clone --recurse-submodules https://github.com/Roboy/VRpuppet 
+    $ cd ..
+    $ catkin_make  
+
+Source your workspace  
+
+    $ . devel/setup.bash
+
+## Before you start
+Make sure the Host IP as well as the Wifi network and password in VRPuppet/hardware/m3/esp/m3_conrol/src/main.c are set to the correct network (e.g. roboy).   
+
+Then flash VRPuppet/hardware/m3/esp/m3_conrol/src/main.c on to all M3 boards. See M3 Readme.md for how to flash.
+
+## Running the VRCage GUI
+In a terminal window start a ROScore  
+
+    $ roscore
+
+In a second terminal window navigate to your catkin workspace, source it as seen above and then start up the VRPuppet Gui by runnung
+
+    $ rosrun rqt_gui rqt_gui
+
+and then from the dropdown menu *Plugins->Roboy* choose *vr puppets demo*.  
+
+![alt text](https://github.com/NexusReflex/VRpuppet/blob/master/docs/imgs/gui.png "gui")  
+
+When operating together with OUI team you might also need to set up a **rosbridge server**, if OUI doesn't have a Linux partition on hand.  
+
+To send commands to the linear actuators  press the GUI's serial node button.  
+
+(**Attention**: the Arduino Mega as well as the motorcontrol shield suffered greatly during the hackathon and should be replaced before resuming operation!)
+
+
+
+

arduino/linear_actuator/linear_actuator.ino

@@ -8,148 +8,145 @@
 ros::NodeHandle nh;
 
 class StepperMotorShield {
-public:
+  public:
     StepperMotorShield(byte id, uint16_t period)
-            : id_(id), period_(period),
-              motor_command_subscriber_("/stepper_motor_shield/MotorCommand",
-                                        &StepperMotorShield::motor_command_callback, this),
-              emergency_server_("/stepper_motor_shield/emergency", &StepperMotorShield::emergency_callback, this),
-              zero_server_("/stepper_motor_shield/zero", &StepperMotorShield::zero_callback, this),
-
-              e_stop_client_("/m3/emergency_stop")
+      : id_(id), period_(period),
+        motor_command_subscriber_("/stepper_motor_shield/MotorCommand",
+                                  &StepperMotorShield::motor_command_callback, this),
+        emergency_server_("/stepper_motor_shield/emergency", &StepperMotorShield::emergency_callback, this),
+        zero_server_("/stepper_motor_shield/zero", &StepperMotorShield::zero_callback, this),
+        e_stop_client_("/m3/emergency_stop")
     {}
     void init(ros::NodeHandle &nh) {
-        // set emergency interupt button
-        pinMode(e_stop_pin, INPUT_PULLUP);
-
-
-        for (int i = 0; i < 10; i++) {
-            pinMode(step_pin[i], OUTPUT);
-            pinMode(dir_pin[i], OUTPUT);
-            pinMode(release_pin[i], INPUT_PULLUP);
-            pinMode(pull_pin[i], INPUT_PULLUP);
-        }
-
-        nh.subscribe(motor_command_subscriber_);
-        nh.advertiseService(emergency_server_);
-        nh.advertiseService(zero_server_);
-        nh.serviceClient(e_stop_client_);
-
-        pixels = new Adafruit_NeoPixel(11, neopixel_pin, NEO_GBR + NEO_KHZ800);
-        pixels->begin();
-        for (int i = 0; i < 11; i++) {
-            pixels->setPixelColor(i, pixels->Color(0, 150, 0));
-            pixels->show();   // Send the updated pixel colors to the hardware.
-        }
+      // set emergency interupt button
+      pinMode(e_stop_pin, INPUT_PULLUP);
+
+
+      for (int i = 0; i < 6; i++) {
+        pinMode(step_pin[i], OUTPUT);
+        pinMode(dir_pin[i], OUTPUT);
+        pinMode(endstop_upper_pin[i], INPUT_PULLUP);
+        pinMode(endstop_lower_pin[i], INPUT_PULLUP);
+      }
+
+      nh.subscribe(motor_command_subscriber_);
+      nh.advertiseService(emergency_server_);
+      nh.advertiseService(zero_server_);
+      nh.serviceClient(e_stop_client_);
+
+      pixels = new Adafruit_NeoPixel(11, neopixel_pin, NEO_GBR + NEO_KHZ800);
+      pixels->begin();
+      for (int i = 0; i < 11; i++) {
+        pixels->setPixelColor(i, pixels->Color(0, 150, 0));
+        pixels->show();   // Send the updated pixel colors to the hardware.
+      }
     }
 
     void run() {
 
-        e_stop = !digitalRead(e_stop_pin);
-        if(e_stop && !e_stop_lastState){
-            std_srvs::SetBool::Request msg;
-            std_srvs::SetBool::Response res;
-            e_stop_lastState = true;
-            msg.data = e_stop;
-            e_stop_client_.call(msg, res);
-        }
-        else if(e_stop && e_stop_lastState){
-
-        }
-        else if(!e_stop && e_stop_lastState){
-                e_stop_lastState = e_stop;
-                std_srvs::SetBool::Request msg;
-                std_srvs::SetBool::Response res;
-                msg.data = e_stop;
-                e_stop_client_.call(msg, res);
-
+      e_stop = !digitalRead(e_stop_pin);
+      if (e_stop && !e_stop_lastState) {
+        std_srvs::SetBool::Request msg;
+        std_srvs::SetBool::Response res;
+        e_stop_lastState = true;
+        msg.data = e_stop;
+        e_stop_client_.call(msg, res);
+      }
+      else if (e_stop && e_stop_lastState) {
+
+      }
+      else if (!e_stop && e_stop_lastState) {
+        e_stop_lastState = e_stop;
+        std_srvs::SetBool::Request msg;
+        std_srvs::SetBool::Response res;
+        msg.data = e_stop;
+        e_stop_client_.call(msg, res);
+
+      }
+      else {
+        period_ = millis() - last_time_;
+        if (period_ > 200) {
+          last_time_ = millis();
+          blink = !blink;
         }
-        else{
-            period_ = millis() - last_time_;
-            if (period_ > 200) {
-                last_time_ = millis();
-                blink = !blink;
-            }
-            if (blink)
-                pixels->setPixelColor(0, pixels->Color(status_color[0], status_color[1], status_color[2]));
-            else
-                pixels->setPixelColor(0, pixels->Color(0, 0, 0));
-            for (int i = 0; i < 10; i++) {
-                int dif = set_point[i] - current_position[i];
-                if (dif < 0) {
-                    do_step(false, i, -dif);
-                    current_position[i] += dif;
-                } else if (dif > 0) {
-                    do_step(true, i, dif);
-                    current_position[i] += dif;
-                } else {
-                    pixels->setPixelColor(i + 1, pixels->Color(10, 10, 10));
-                    pixels->show();
-                }
-            }
+        if (blink)
+          pixels->setPixelColor(0, pixels->Color(status_color[0], status_color[1], status_color[2]));
+        else
+          pixels->setPixelColor(0, pixels->Color(0, 0, 0));
+        for (int i = 0; i < 6; i++) {
+          int dif = set_point[i] - current_position[i];
+          if (dif < 0) {
+            do_step(false, i, -dif);
+            current_position[i] += dif;
+          } else if (dif > 0) {
+            do_step(true, i, dif);
+            current_position[i] += dif;
+          } else {
+            pixels->setPixelColor(i + 1, pixels->Color(10, 10, 10));
+            pixels->show();
+          }
         }
+      }
     }
 
     void do_step(byte dir, int motor, int steps) {
-        digitalWrite(dir_pin[motor], dir);
-        if (dir)
-            pixels->setPixelColor(motor + 1, pixels->Color(0, 150, 0));
-        else
-            pixels->setPixelColor(motor + 1, pixels->Color(0, 0, 150));
-        pixels->show();
-        for (int i = 0; i < steps; i++) {
-            digitalWrite(step_pin[motor], HIGH);
-            delayMicroseconds(500);
-            digitalWrite(step_pin[motor], LOW);
-            delayMicroseconds(500);
-            if (!dir && !digitalRead(pull_pin[motor])) {
-                current_position[motor] = 0;
-                return;
-            }
-            if (dir && !digitalRead(release_pin[motor])) {
-                return;
-            }
+      digitalWrite(dir_pin[motor], dir);
+      if (dir)
+        pixels->setPixelColor(motor + 1, pixels->Color(0, 150, 0));
+      else
+        pixels->setPixelColor(motor + 1, pixels->Color(0, 0, 150));
+      pixels->show();
+      for (int i = 0; i < steps; i++) {
+        digitalWrite(step_pin[motor], HIGH);
+        delayMicroseconds(500);
+        digitalWrite(step_pin[motor], LOW);
+        delayMicroseconds(500);
+        if (!dir && !digitalRead(endstop_lower_pin[motor])) {
+          current_position[motor] = 0;
+          return;
+        }
+        if (dir && !digitalRead(endstop_upper_pin[motor])) {
+          return;
         }
+      }
     }
 
     void init_positions() {
-        pixels->setPixelColor(0, pixels->Color(0, 150, 0));
-        pixels->show();
-        for (int motor = 0; motor < 6; motor++) {
-            digitalWrite(dir_pin[motor], false);
-            while (digitalRead(pull_pin[motor])) {
-                digitalWrite(step_pin[motor], HIGH);
-                delayMicroseconds(500);
-                digitalWrite(step_pin[motor], LOW);
-                delayMicroseconds(500);
-            }
-            current_position[motor] = 0;
-            set_point[motor] = 0;
+      pixels->setPixelColor(0, pixels->Color(0, 150, 0));
+      pixels->show();
+      for (int motor = 0; motor < 6; motor++) {
+        digitalWrite(dir_pin[motor], false); // move down
+        while (digitalRead(endstop_lower_pin[motor])) {
+          digitalWrite(step_pin[motor], HIGH);
+          delayMicroseconds(500);
+          digitalWrite(step_pin[motor], LOW);
+          delayMicroseconds(500);
         }
+        current_position[motor] = 0;
+        set_point[motor] = 0;
+      }
     }
 
     void motor_command_callback(const roboy_middleware_msgs::MotorCommand &msg) {
-        for (int i = 0; i < 10; i++) {
-            set_point[i] = msg.set_points[i];
-        }
+      for (int i = 0; i < 6; i++) {
+        set_point[i] = msg.set_points[i];
+      }
     }
 
     void emergency_callback(const std_srvs::SetBool::Request &req,
                             std_srvs::SetBool::Response &res) {
-        active_ = req.data;
+      active_ = req.data;
     }
 
     void zero_callback(const std_srvs::Empty::Request &req,
                        std_srvs::Empty::Response &res) {
-        init_positions();
+      init_positions();
     }
 
-private:
-    const byte id_;
-    const byte step_pin[10] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, dir_pin[10] = {24, 25, 26, 27, 28, 29, 30, 31, 32, 33},
-            enable_pin[10] = {14, 15, 16, 17, 18, 19, 40, 41, 42, 43}, pull_pin[10] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9},
-
-            release_pin[10] = {44, 45, 46, 47, 48, 49, 50, 51, 52, 53}, neopixel_pin = 12, e_stop_pin = 50;
+  private:
+    const byte id_;const byte step_pin[6] = {2, 4, 6, 7, 8, 10}, dir_pin[6] = {24, 26, 28, 29, 30, 32},
+            enable_pin[6] = {14, 16, 18, 19, 40, 42}, endstop_lower_pin[6] = {A0, A1, A2, A3, A4, A5},
+            endstop_upper_pin[6] = {44, 45, 46, 47, 48, 49}, neopixel_pin = 12, e_stop_pin = 50;
     int set_point[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, current_position[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
     uint16_t period_;
     bool active_ = true;
@@ -169,12 +166,12 @@ private:
 StepperMotorShield stepper_motor_shield(69, 50);
 
 void setup() {
-    nh.initNode();
-    stepper_motor_shield.init(nh);
+  nh.initNode();
+  stepper_motor_shield.init(nh);
 }
 
 void loop() {
-    stepper_motor_shield.run();
-    nh.spinOnce();
-    delay(1);
+  stepper_motor_shield.run();
+  nh.spinOnce();
+  delay(1);
 }