Added applied robotics code to archive.

2025-11-09 21:51:15 +00:00 · 2018-08-22 14:49:49 -07:00
parent 5cbceb42d7
commit a56690ca18
89 changed files with 38750 additions and 0 deletions
--- a/Robotics/software/firmware/interface_controller/interface_controller.ino
+++ b/Robotics/software/firmware/interface_controller/interface_controller.ino
@@ -0,0 +1,192 @@
+#include <Servo.h>4
+
+////////// Pinout Variables //////////
+const int led_data_pin = 9;
+const int led2_data_pin = 8;
+const int shooting_servo_pin = 11;
+const int tamping_servo_pin = 10;
+const int relay_control_pin = 3;
+const int pressure_sensor_pin = A0;
+
+const int denso_sense_pin_2 = 4;
+const int denso_sense_pin_3 = 5;
+
+const int ball_sense_pwm_pin = 7;
+const int ball_sense_feedback_pin = 6;
+
+////////// Global Variables //////////
+String read_string = "";
+
+const int safety_psi_max = 80;
+float current_set_pressure = 0;
+float current_actual_pressure = 0;
+bool set_pressure_reached = false;
+float allowed_psi_error = 4.0;
+
+bool should_tamp = false;
+bool should_shoot = false;
+
+Servo firing_servo;
+Servo tamping_servo;
+
+void setup() {
+  Serial.begin(57600);
+
+  pinMode(pressure_sensor_pin, INPUT);
+  pinMode(ball_sense_feedback_pin, INPUT);
+  pinMode(denso_sense_pin_2, INPUT);
+  pinMode(denso_sense_pin_3, INPUT);
+  pinMode(led_data_pin, INPUT);
+  pinMode(led2_data_pin, INPUT);
+  pinMode(ball_sense_pwm_pin, INPUT);
+
+  pinMode(shooting_servo_pin, OUTPUT);
+  pinMode(tamping_servo_pin, OUTPUT);
+  pinMode(relay_control_pin, OUTPUT);
+
+  pinMode(5, OUTPUT);
+  digitalWrite(5, LOW);
+
+  digitalWrite(relay_control_pin, LOW);
+
+  firing_servo.attach(shooting_servo_pin);
+  firing_servo.write(170);
+
+  tamping_servo.attach(tamping_servo_pin);
+  tamping_servo.write(180);
+}
+
+void loop() {
+  check_serial_input();
+  get_current_actual_pressure();
+  pressurize_if_needed();
+  tamp_if_needed();
+  shoot_if_needed();
+  print_state();
+  delay(50);
+}
+
+// Set Pressure|Tamp?|Shoot?|
+// XXX|X|X|
+void check_serial_input() {
+  if (Serial.available() > 0) {
+    while (Serial.available() > 0) {
+      read_string += (char)Serial.read();
+    }
+
+    if (read_string.endsWith("\n")) {
+      // Vairables to break things up
+      int last_substring_breakpoint = 0;
+      int current_substring_breakpoint = 0;
+
+      // Get substring and turn into pressure
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      current_set_pressure = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+      // Get whether we should tamp
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      should_tamp = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+
+      // Get whether we should shoot
+      current_substring_breakpoint = read_string.substring(last_substring_breakpoint).indexOf('|');
+      should_shoot = read_string.substring(last_substring_breakpoint, current_substring_breakpoint + last_substring_breakpoint).toInt();
+      last_substring_breakpoint += current_substring_breakpoint + 1;
+
+      read_string = "";
+    }
+
+  }
+}
+
+void get_current_actual_pressure() {
+  current_actual_pressure = read_pressure();
+}
+
+void pressurize_if_needed() {
+  static bool keep_charging = false;
+
+  if (current_set_pressure > 0) {
+
+    if (keep_charging) {
+      digitalWrite(relay_control_pin, HIGH);
+
+      if (current_actual_pressure >= current_set_pressure) {
+        keep_charging = false;
+        digitalWrite(relay_control_pin, LOW);
+      }
+    }
+
+    if ((current_set_pressure - current_actual_pressure) > allowed_psi_error) {
+      keep_charging = true;
+    }
+
+  } else {
+    digitalWrite(relay_control_pin, LOW);
+  }
+}
+
+void tamp_if_needed() {
+  if (should_tamp) {
+
+    tamping_servo.write(60);
+
+    delay(1000);
+    tamping_servo.write(180);
+
+    should_tamp = false;
+  }
+}
+
+void shoot_if_needed() {
+  if (should_shoot) {
+    firing_servo.write(100);
+    delay(500);
+    firing_servo.write(170);
+
+    current_set_pressure = 0;
+    should_shoot = false;
+  }
+}
+
+void print_state() {
+  Serial.print(current_actual_pressure);
+  Serial.print("|");
+  Serial.print(get_set_pressure());
+  Serial.print("|");
+  Serial.print(get_compressor_on());
+  Serial.print("|");
+  Serial.print(ball_detected());
+  Serial.print("|");
+  Serial.print(arm_motor_power_on());
+  Serial.println();
+}
+
+bool arm_motor_power_on() {
+  return digitalRead(denso_sense_pin_2);
+}
+
+bool ball_detected() {
+  return digitalRead(ball_sense_feedback_pin);
+}
+
+bool get_compressor_on() {
+  return digitalRead(relay_control_pin);
+}
+
+float get_set_pressure() {
+  return current_set_pressure;
+}
+
+float read_pressure() {
+  analogRead(pressure_sensor_pin);
+  uint16_t raw_adc = analogRead(pressure_sensor_pin);
+
+  float voltage = 0.004887585532746823 * (float) raw_adc;  //ADC count (10 bit) to voltage conversion
+
+  float pressure = (250000.0 * voltage - 250000.0) * 0.000145038; //Convert voltage to pascals, then to PSI
+
+  return max(pressure, 0);
+}
--- a/Robotics/software/firmware/led_controller/led_controller.ino
+++ b/Robotics/software/firmware/led_controller/led_controller.ino
@@ -0,0 +1,74 @@
+#include "FastLED.h"
+#include <ArduinoJson.h>
+
+////////// Pinout Variables //////////
+const int led_power_status_pin = 8;
+const int led_general_status_pin = 9;
+const int power_on_status_pin = 7;
+const int led_pwm_pin = 6;
+
+////////// Global Variables //////////
+const int num_power_status_leds = 69;
+CRGB power_status_leds[num_power_status_leds];
+
+const int num_general_status_leds = 18;
+CRGB general_status_leds[num_general_status_leds];
+
+char read_buffer[1500];
+
+bool power_on = false;
+bool was_on = false;
+
+
+void setup() {
+  Serial.begin(57600);
+
+  FastLED.addLeds<WS2812, led_power_status_pin, GRB>(power_status_leds, num_power_status_leds).setCorrection(TypicalLEDStrip);
+  FastLED.addLeds<WS2812, led_general_status_pin, GRB>(general_status_leds, num_general_status_leds).setCorrection(TypicalLEDStrip);
+
+  FastLED.setBrightness(40);
+
+  pinMode(power_on_status_pin, INPUT);
+  pinMode(led_pwm_pin, OUTPUT);
+
+  analogWriteFrequency(led_pwm_pin, 38000);
+  analogWrite(led_pwm_pin, 128);
+}
+
+void loop() {
+  if (Serial.available() > 0) {
+    Serial.readBytesUntil('\n', read_buffer, 1500);
+  }
+
+  StaticJsonBuffer<1500> jsonBuffer;
+  JsonObject& root = jsonBuffer.parseObject(read_buffer);
+
+  if (root.success()) {
+    Serial.println("worked!");
+    JsonArray& leds = root["leds"];
+
+    int led_count = 0;
+    for (unsigned int i = 0 ; i < min(num_general_status_leds * 3, leds.size()) ; i += 3) {
+      general_status_leds[led_count].red = leds[i];
+      general_status_leds[led_count].green = leds[i + 1];
+      general_status_leds[led_count].blue = leds[i + 2];
+      led_count++;
+    }
+  }
+
+  EVERY_N_MILLISECONDS(250) {
+    if (digitalRead(power_on_status_pin)) {
+      if (was_on) {
+        fill_solid(power_status_leds, num_power_status_leds, CRGB::Black);
+        was_on = 0;
+      } else {
+        fill_solid(power_status_leds, num_power_status_leds, CRGB::Red);
+        was_on = 1;
+      }
+    } else {
+      fill_solid(power_status_leds, num_power_status_leds, CRGB::Green);
+    }
+  }
+
+  FastLED.show();
+}
--- a/Robotics/software/firmware/sensor_demo/sensor_demo.ino
+++ b/Robotics/software/firmware/sensor_demo/sensor_demo.ino
@@ -0,0 +1,148 @@
+#define PRESSURE_INPUT_PIN A5
+#define VALVE_CONTROL_PIN 9
+#define COMPRESSOR_CONTROL_PIN 2
+
+#define LED_PIN 13
+
+//Code safeties
+#define SAFETY_PSI_MAX 25
+
+String input;
+
+void setup() {
+
+  pinMode(COMPRESSOR_CONTROL_PIN, OUTPUT);
+  digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+  pinMode(LED_PIN, OUTPUT);
+  digitalWrite(LED_PIN, LOW);
+  pinMode(PRESSURE_INPUT_PIN, INPUT);
+
+  pinMode(VALVE_CONTROL_PIN, OUTPUT);
+  digitalWrite(VALVE_CONTROL_PIN, HIGH);
+
+  Serial.begin(9600);
+}
+
+void loop() {
+  uint16_t targetPSI = 0;
+  float    currentPSI = 0;
+  uint8_t  shouldCompress = 0;
+  digitalWrite(VALVE_CONTROL_PIN, HIGH) ;
+  
+  if(Serial.available()){
+    input = Serial.readStringUntil('\n');
+    Serial.flush();
+
+    if(isValidNumber(input)){
+      targetPSI = input.toInt();
+      
+      Serial.print("Target PSI: ");
+      Serial.println(targetPSI);
+
+      if(targetPSI > SAFETY_PSI_MAX){
+        Serial.print("Target PSI too high, bounding to safety max of ");
+        Serial.println(SAFETY_PSI_MAX);
+      }
+
+      targetPSI = constrain(targetPSI, 0, SAFETY_PSI_MAX);
+      
+      shouldCompress = 1;
+
+      //Start the compressor
+      digitalWrite(COMPRESSOR_CONTROL_PIN, HIGH);
+      digitalWrite(LED_PIN, HIGH);
+
+      while(shouldCompress){
+        currentPSI = readPressure();
+        if(currentPSI >= targetPSI){
+          shouldCompress = 0;
+        }
+        Serial.print("Current Pressure: ");
+        Serial.print(currentPSI);
+        Serial.print(" psi | Target Pressure: ");
+        Serial.print(targetPSI);
+        Serial.println(" psi");
+        delay(150);
+      }
+
+      //Stop the compressor
+      digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+      digitalWrite(LED_PIN, LOW);
+
+      //Wait for half a second
+      delay(500) ;
+      //Release Valve
+      digitalWrite(VALVE_CONTROL_PIN, LOW) ;
+      //Wait to let all the pressure out
+      delay(2500) ;
+    }
+    
+  }
+
+  delay(150);  // ¯\_(ツ)_/¯
+
+  Serial.print("Current Pressure: ");
+  Serial.print(readPressure());
+  Serial.println(" psi");
+  Serial.flush();
+
+
+  /*
+  //Turn off the compressor
+  digitalWrite(COMPRESSOR_CONTROL_PIN, LOW);
+  digitalWrite(LED_PIN, LOW);
+  for(int i = 0; i < 2; ++i){
+    delay(160);
+    Serial.print("Read Analog Value: ");
+    Serial.print(readPressure());
+    Serial.println(" psi");
+    Serial.flush();
+  }
+
+  */
+
+  /*
+  //Turn on the compressor
+  digitalWrite(COMPRESSOR_CONTROL_PIN, HIGH);
+  digitalWrite(LED_PIN, HIGH);
+  for(int i = 0; i < 6; ++i){
+    delay(160);
+    Serial.print("Read Analog Value: ");
+    Serial.print(readPressure());
+    Serial.println(" psi");
+    Serial.flush();
+  }
+  */
+
+
+}
+
+/*
+ * 
+ * Pressure is a signal from 1 - 5 volts with 5 volts signifying 1 MPa 
+ * See http://nickmccomb.ddns.net:8090/display/AR/Sensors#Sensors-ISE50-T2-62L
+ */
+float readPressure(){
+  analogRead(PRESSURE_INPUT_PIN);
+  uint16_t raw_adc = analogRead(PRESSURE_INPUT_PIN);
+
+  float voltage = 0.004887585532746823 * (float) raw_adc;  //ADC count (10 bit) to voltage conversion
+
+  float pressure = (250000.0 * voltage -250000.0) * 0.000145038; //Convert voltage to pascals, then to PSI
+
+  return pressure;
+}
+
+
+boolean isValidNumber(String str){
+   for(byte i=0;i<str.length();i++)
+   {
+      if(isDigit(str.charAt(i))) return true;
+        }
+   return false;
+} 
+
+
+
+
+