Added IONI configs and mining firmware

electrical/documents/ioni/roll.drc

@@ -1,7 +1,7 @@
 [Header]
 DRCVersion=110
-GranityVersion=1.14.0
+GranityVersion=1.14.1
-GranityVersionInt=11400
+GranityVersionInt=11401
 
 [GDT3Params]
 1\name=GCFWVER
@@ -9,7 +9,7 @@ GranityVersionInt=11400
 1\scaling=1
 1\offset=0
 1\readonly=true
-1\value=10702
+1\value=10707
 1\min=2
 1\max=1
 2\name=HWTYPE
@@ -25,7 +25,7 @@ GranityVersionInt=11400
 3\scaling=1
 3\offset=0
 3\readonly=true
-3\value=110028853
+3\value=110028506
 3\min=2
 3\max=1
 4\name=BUILDREVISION
@@ -33,7 +33,7 @@ GranityVersionInt=11400
 4\scaling=1
 4\offset=0
 4\readonly=true
-4\value=163070257
+4\value=157269493
 4\min=2
 4\max=1
 5\name=CEI
@@ -49,7 +49,7 @@ GranityVersionInt=11400
 6\scaling=1
 6\offset=0
 6\readonly=true
-6\value=-314970192
+6\value=-261528363
 6\min=2
 6\max=1
 7\name=SMO
@@ -291,7 +291,7 @@ GranityVersionInt=11400
 36\readonly=false
 36\value=3
 36\min=0
-36\max=5.99999999999999
+36\max=6
 37\name=FOV
 37\addr=569
 37\scaling=100
@@ -441,7 +441,7 @@ GranityVersionInt=11400
 55\scaling=1
 55\offset=0
 55\readonly=false
-55\value=20
+55\value=2
 55\min=1
 55\max=32767
 56\name=CSD
@@ -673,7 +673,7 @@ GranityVersionInt=11400
 84\scaling=1
 84\offset=0
 84\readonly=true
-84\value=4194047
+84\value=62914303
 84\min=0
 84\max=0
 85\name=CAPS2

electrical/documents/ioni/shoulder.drc

@@ -1,7 +1,7 @@
 [Header]
 DRCVersion=110
-GranityVersion=1.14.0
+GranityVersion=1.14.1
-GranityVersionInt=11400
+GranityVersionInt=11401
 
 [GDT3Params]
 1\name=GCFWVER
@@ -9,7 +9,7 @@ GranityVersionInt=11400
 1\scaling=1
 1\offset=0
 1\readonly=true
-1\value=10702
+1\value=10707
 1\min=2
 1\max=1
 2\name=HWTYPE
@@ -25,7 +25,7 @@ GranityVersionInt=11400
 3\scaling=1
 3\offset=0
 3\readonly=true
-3\value=110028853
+3\value=110028896
 3\min=2
 3\max=1
 4\name=BUILDREVISION
@@ -33,7 +33,7 @@ GranityVersionInt=11400
 4\scaling=1
 4\offset=0
 4\readonly=true
-4\value=163070257
+4\value=157269493
 4\min=2
 4\max=1
 5\name=CEI
@@ -49,7 +49,7 @@ GranityVersionInt=11400
 6\scaling=1
 6\offset=0
 6\readonly=true
-6\value=-314970192
+6\value=-23384809
 6\min=2
 6\max=1
 7\name=SMO
@@ -441,7 +441,7 @@ GranityVersionInt=11400
 55\scaling=1
 55\offset=0
 55\readonly=false
-55\value=2
+55\value=1
 55\min=1
 55\max=32767
 56\name=CSD
@@ -673,7 +673,7 @@ GranityVersionInt=11400
 84\scaling=1
 84\offset=0
 84\readonly=true
-84\value=4194047
+84\value=62914303
 84\min=0
 84\max=0
 85\name=CAPS2

software/firmware/gripper/gripper.ino

@@ -21,7 +21,7 @@ enum HARDWARE {
 };
 
 ////////// Global Variables //////////
-// define user words for motor states
+// define nice words for motor states
 #define BRAKE     0
 #define FWD       1
 #define REV       2
@@ -48,11 +48,12 @@ double signed_setpoint_vel = -200;
 // Position
 double setpoint_pos = 0; //PID position variables
 double input_pos, output_pos;
-PID motor_pos(&input_pos, &output_pos, &setpoint_pos, 10, 0, 0, DIRECT); //position PID
+PID motor_pos(&input_pos, &output_pos, &setpoint_pos, 500, 0, 0, DIRECT); //position PID
-long current_pos = 1000;
+
+long current_pos = 0;
 
 // Homing
-int trip_threshold = 650;
+int trip_threshold = 550;
 bool home_flag = true;
 long prev_millis = 0;
 
@@ -72,68 +73,81 @@ void setup() {
 void loop() {
 
   // Position based homing
+  //Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
   // if (home_flag == true){
   //   Serial.println("homing");
-  //   Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
   //
-  //   unsigned long curr_millis = millis();
   //   // if it is time to move the motor and we haven't tripped the current yet:
+  //   unsigned long curr_millis = millis();
   //   if (curr_millis - prev_millis > interval){
   //     prev_millis = curr_millis;
-  //     current_pos += 5; // increment by counts
+  //     current_pos -= 10; // increment by counts
-  //
+  //     }
-  //     if (analogRead(HARDWARE::MOTOR_CURRENT_SENSE) < trip_threshold){
+  //   if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (curr_millis > 1000)){
-  //       current_pos = 0;
   //     enc.write(0);
+  //     current_pos = 0;
   //     home_flag = false;
+  //     Serial.println("homing complete");
   //   }
   // }
-  // }
-
-  // Position movement test
-  // unsigned long curr_millis = millis();
-  // // if it is time to move the motor and we haven't tripped the current yet:
-  // if (curr_millis - prev_millis > interval){
-  //   prev_millis = curr_millis;
-  //   current_pos += 1; // increment by counts
-  // }
-  //
-  // set_position(10000);
   //
+  // set_position(current_pos);
   // Serial.print("Set Pos:  ");
   // Serial.println(current_pos);
   // Serial.print("Curr Pos: ");
   // Serial.println(enc.read());
   // delay(1);
 
-  // Velocity based non blocking homing
+/*----------------------------------------------------------------------------*/
-  if (home_flag == true){
 
-    set_velocity(-150);
+  // Position movement test
+  // unsigned long curr_millis = millis();
+  // // if it is time to move the motor and we haven't tripped the current yet:
+  // if (curr_millis - prev_millis > interval){
+  //   prev_millis = curr_millis;
 
-    Serial.println("Homing");
+  //   current_pos += 10; // increment by counts
 
-    Serial.print("Current: ");
+  // }
-    Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
+  //
+  set_position(current_pos);
 
-    if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (millis() > 500)){
-      Serial.println("Tripped current sensor!");
-      set_motor_output(BRAKE, 0); //stop moving
-      home_flag = false; // no more homing
-      //set_motor_output(COAST, 0); //stop moving
-      enc.write(0); // reset encoder
-    }
-  }
-  else {
-    set_position(current_pos); // hold
   Serial.print("Set Pos:  ");
   Serial.println(current_pos);
   Serial.print("Curr Pos: ");
   Serial.println(enc.read());
-  }
-
   delay(1);
 
+/*----------------------------------------------------------------------------*/
+
+  // Velocity based non blocking homing
+  // if (home_flag == true){
+  //
+  //   set_velocity(-150);
+  //
+  //   Serial.println("Homing");
+  //
+  //   Serial.print("Current: ");
+  //   Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
+  //
+  //   if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (millis() > 500)){
+  //     Serial.println("Tripped current sensor!");
+  //     set_motor_output(BRAKE, 0); //stop moving
+  //     home_flag = false; // no more homing
+  //     //set_motor_output(COAST, 0); //stop moving
+  //     enc.write(0); // reset encoder
+  //   }
+  // }
+  // else {
+  //   set_position(current_pos); // hold
+  //   Serial.print("Set Pos:  ");
+  //   Serial.println(current_pos);
+  //   Serial.print("Curr Pos: ");
+  //   Serial.println(enc.read());
+  // }
+  //
+  // delay(1);
+
 }
 
 void setup_hardware(){

software/firmware/mining/mining.ino

@@ -0,0 +1,289 @@
+////////// Includes //////////
+#include "HX711.h"
+
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+  RS485_EN = 6,
+  RS485_RX = 9,
+  RS485_TX = 10,
+
+  MOTOR_LIFT_A = 27,
+  MOTOR_LIFT_B = 28,
+  MOTOR_LIFT_PWM = 25,
+  MOTOR_LIFT_CS = 31,
+  MOTOR_LIFT_EN = 24,
+  MOTOR_LIFT_FB = A10,
+
+  MOTOR_TILT_A = 30,
+  MOTOR_TILT_B = 29,
+  MOTOR_TILT_PWM = 32,
+  MOTOR_TILT_CS = 26,
+  MOTOR_TILT_EN = 33,
+  MOTOR_TILT_FB = A11,
+
+  LED_13 = 13,
+
+  LED_RED = 20,
+  LED_BLUE = 21,
+  LED_GREEN = 22,
+
+  SCALE_DOUT = 8,
+  SCALE_CLK = 7
+
+};
+
+enum MOTORS {
+  LIFT = 0,
+  TILT = 1,
+};
+
+enum MODBUS_REGISTERS {
+    // Inputs
+    SET_POSITION_LIFT = 0,
+    SET_POSITION_TILT = 1,
+    TARE = 2,
+    CALIBRATION_FACTOR = 3,
+
+    // Outputs
+    CURRENT_POSITION_LIFT = 4,
+    CURRENT_POSITION_TILT = 5,
+    MEASURED_WEIGHT = 6,
+};
+
+////////// Global Variables //////////
+int set_position_lift = 0;
+int set_position_tilt = 0;
+int tolerance = 20; //tolerance for position
+
+float calibration_factor = -120000; //for the scale
+
+// modbus stuff
+const uint8_t node_id = 2;
+const uint8_t mobus_serial_port_number = 3;
+uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0};
+uint8_t num_modbus_registers = 0;
+int8_t poll_state = 0;
+bool communication_good = false;
+uint8_t message_count = 0;
+
+// nice human words for motor states
+#define BRAKEVCC  0
+#define CW        1
+#define CCW       2
+#define BRAKEGND  3
+
+////////// Class Instantiations //////////
+Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
+
+void setup() {
+  Serial.begin(9600); // debug
+
+  setup_hardware();
+  num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+  slave.begin(115200); // baud-rate at 19200
+  slave.setTimeOut(150);
+}
+
+void loop() {
+  poll_modbus();
+  set_leds();
+  set_motors();
+  set_scale();
+  poll_scale();
+}
+
+void setup_hardware() {
+  pinMode(HARDWARE::RS485_EN, OUTPUT);
+
+  pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
+  pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
+  pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
+  pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
+  pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
+  pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
+
+  pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
+  pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
+  pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
+  pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
+  pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
+  pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
+
+  pinMode(HARDWARE::LED_13, OUTPUT);
+  pinMode(HARDWARE::LED_RED, OUTPUT);
+  pinMode(HARDWARE::LED_BLUE, OUTPUT);
+  pinMode(HARDWARE::LED_GREEN, OUTPUT);
+
+  // set defualt states
+  digitalWrite(HARDWARE::LED_RED, LOW);
+  digitalWrite(HARDWARE::LED_GREEN, HIGH);
+  digitalWrite(HARDWARE::LED_BLUE, HIGH);
+  digitalWrite(HARDWARE::LED_13, LOW);
+  digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
+  digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
+
+  // Change motor PWM frequency so it's not in the audible range
+  analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
+  analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
+
+  // set the current desired position to the current position
+  set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
+  set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
+
+  // setup scale
+  scale.set_scale();
+  scale.tare();	//Reset the scale to 0
+  scale.set_scale(calibration_factor);
+}
+
+void poll_modbus(){
+    poll_state = slave.poll(modbus_data, num_modbus_registers);
+    communication_good = !slave.getTimeOutState();
+}
+
+void set_leds(){
+    if(poll_state > 4){
+        message_count++;
+        if(message_count > 2){
+            digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
+            message_count = 0;
+        }
+
+        digitalWrite(HARDWARE::LED_GREEN, LOW);
+        digitalWrite(HARDWARE::LED_RED, HIGH);
+    }else if(!communication_good){
+        digitalWrite(HARDWARE::LED_13, LOW);
+        digitalWrite(HARDWARE::LED_GREEN, HIGH);
+        digitalWrite(HARDWARE::LED_RED, LOW);
+    }
+}
+
+void set_motors() {
+  set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT];
+  set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT];
+
+  current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
+  current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
+
+  if (abs(current_position_lift - set_position_lift) > tolerance) {
+    if (current_position_lift < set_position_lift) {
+      set_motor_output(MOTORS::LIFT, CCW, 255);
+    }
+    else {
+      set_motor_output(MOTORS::LIFT, CW, 255);
+    }
+  }
+  else {
+    motor_off(MOTORS::LIFT);
+  }
+
+  if (abs(current_position_tilt - set_position_tilt) > tolerance) {
+    if (current_position_tilt < set_position_tilt) {
+      set_motor_output(MOTORS::TILT, CCW, 255);
+    }
+    else {
+      set_motor_output(MOTORS::TILT, CW, 255);
+    }
+  }
+  else {
+    motor_off(MOTORS::TILT);
+  }
+}
+
+void set_scale(){
+    scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]);
+}
+
+void poll_scale(){
+    modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1
+}
+
+//---Set Motor Output---//
+/*
+  Inputs: motor number, direction, pwm value
+  Returns: nothing
+
+  Will set a motor going in a specific direction the motor will continue
+  going in that direction, at that speed until told to do otherwise.
+
+  direct: Should be between 0 and 3, with the following result
+  0: Brake to VCC
+  1: Clockwise
+  2: CounterClockwise
+  3: Brake to GND
+
+  pwm: should be a value between 0 and 255, higher the number, the faster
+  it'll go
+  ----------------
+  Control Logic:
+  ----------------
+	         	 A | B
+  Brake VCC: 1   1
+	       CW: 1   0
+	      CCW: 0   1
+  Brake GND: 0   0
+  ----------------
+*/
+void set_motor_output(int motor, int direction, int pwm_input) {
+
+  int a;
+  int b;
+  int pwm;
+
+  if (motor == MOTORS::LIFT) {
+    a = HARDWARE::MOTOR_LIFT_A;
+    b = HARDWARE::MOTOR_LIFT_B;
+    pwm = HARDWARE::MOTOR_LIFT_PWM;
+  }
+  else if (motor == MOTORS::TILT) {
+    a = HARDWARE::MOTOR_TILT_A;
+    b = HARDWARE::MOTOR_TILT_B;
+    pwm = HARDWARE::MOTOR_TILT_PWM;
+  }
+  else {
+    return;
+  }
+
+  if (direction <= 4) {
+    // Set A
+    if (direction <= 1) {
+      digitalWrite(a, HIGH);
+    }
+    else {
+      digitalWrite(a, LOW);
+    }
+
+    // Set B
+    if ((direction == 0) || (direction == 2)) {
+      digitalWrite(b, HIGH);
+    }
+    else {
+      digitalWrite(b, LOW);
+    }
+    analogWrite(pwm, pwm_input);
+  }
+}
+
+void motor_off(int motor) {
+  int a;
+  int b;
+  int pwm;
+
+  if (motor == MOTORS::LIFT) {
+    a = HARDWARE::MOTOR_LIFT_A;
+    b = HARDWARE::MOTOR_LIFT_B;
+    pwm = HARDWARE::MOTOR_LIFT_PWM;
+  }
+  else if (motor == MOTORS::TILT) {
+    a = HARDWARE::MOTOR_TILT_A;
+    b = HARDWARE::MOTOR_TILT_B;
+    pwm = HARDWARE::MOTOR_TILT_PWM;
+  }
+  else {
+    return;
+  }
+
+  digitalWrite(a, LOW);
+  digitalWrite(b, LOW);
+  analogWrite(pwm, 0);
+}

software/testing/modbus/mining/mining_tester.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+
+# from std_msgs.msg import UInt8, UInt16
+
+# Custom Imports
+# from rover_control.msg import TowerPanTiltControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "chassis_pan_tilt_control"
+
+DEFAULT_PORT = "/dev/rover/ttyChassisPanTilt"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_PAN_TILT_CONTROL_TOPIC = "chassis/pan_tilt/control"
+
+PAN_TILT_NODE_ID = 1
+
+COMMUNICATIONS_TIMEOUT = 0.01  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+DEFAULT_HERTZ = 20
+
+PAN_TILT_MODBUS_REGISTERS = {
+    "CENTER_ALL": 0,
+
+    "PAN_ADJUST_POSITIVE": 1,
+    "PAN_ADJUST_NEGATIVE": 2,
+    "TILT_ADJUST_POSITIVE": 3,
+    "TILT_ADJUST_NEGATIVE": 4
+}
+
+PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
+    0,  # No centering
+    0,  # No pan positive adjustment
+    0,  # No pan negative adjustment
+    0,  # No tilt positive adjustment
+    0  # No tilt negative adjustement
+]
+
+self.mining_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+self.mining_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                delay_before_rx=RX_DELAY,
+                                                                delay_before_tx=TX_DELAY)
+
+
+def run(self):
+    print(self.mining_node.read_registers(0, 7))
+
+
+def connect_to_pan_tilt_and_tower(self):
+    self.mining_node = minimalmodbus.Instrument(self.port, int(2))
+    self.__setup_minimalmodbus_for_485()
+
+
+NODE_LAST_SEEN_TIMEOUT = 2  # seconds
+
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class MiningControl(object):
+    def __init__(self):
+        self.port = "COM22"
+        self.baud = 115200
+
+        self.mining_node = None
+        self.tower_node = None
+
+        self.connect_to_pan_tilt_and_tower()
+
+        self.pan_tilt_control_message = None
+        self.new_pan_tilt_control_message = False
+
+        self.modbus_nodes_seen_time = time()
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+if __name__ == "__main__":
+    MiningControl()