commit cbs panel code and merge latest old arm code

arduino_firmware/arm_joystick_panel/arm_joystick_panel.ino

@@ -0,0 +1,126 @@
+
+
+//Joystick right Pins
+const int xAxisPin2 = A1; 
+const int yAxisPin2 = A0;
+const int zAxisPin2 = A2;
+const int buttonPin2 = 2;
+
+//Joystick left Pins 
+const int xAxisPin1 = A4;
+const int yAxisPin1 = A3; 
+const int zAxisPin1 = A5; 
+const int buttonPin1 = 3; 
+
+const int centerValue = 512; // Analog midpoint
+const int deadZoneMin = 440; // Lower dead zone threshold
+const int deadZoneMax = 583; // Upper dead zone threshold
+const int minAnalog = 0;     // Min Analog value
+const int maxAnalog = 1023;  // Max Analog value
+
+//#define minAnalogZ 0
+//#define maxAnalogZ 600
+
+#define RATE_HZ 100
+
+//#define TEST //Uncomment to print raw values instead of adj values
+
+long interval_ms = 99;
+long last_time = 0;
+long current_time = 0;
+void setup() {
+  pinMode(buttonPin1, INPUT_PULLUP);
+  pinMode(buttonPin2, INPUT_PULLUP); 
+  Serial.begin(19200);
+  interval_ms = 1/RATE_HZ * 1000; //interval in ms
+
+}
+
+void loop() {
+  current_time = millis();
+  if(current_time - last_time > interval_ms){
+      last_time = millis();
+
+
+  //Read raw values
+  int rawX1 = analogRead(xAxisPin1); 
+  int rawY1 = analogRead(yAxisPin1); 
+  int rawZ1 = analogRead(zAxisPin1); 
+  int buttonState1 = digitalRead(buttonPin1); 
+
+  int rawX2 = analogRead(xAxisPin2);
+  int rawY2 = analogRead(yAxisPin2); 
+  int rawZ2 = analogRead(zAxisPin2); 
+  int buttonState2 = digitalRead(buttonPin2); 
+
+#ifdef TEST
+Serial.print("TESING: ");
+  // Print joystick values with adjusted dead zone
+  Serial.print(rawX1);
+  Serial.print(",");
+  Serial.print(rawY1);
+  Serial.print(",");
+  Serial.print(rawZ1);
+  Serial.print(",");
+  Serial.print(buttonState1 == LOW ? "1" : "0");
+
+  Serial.print(",");
+  Serial.print(rawX2);
+  Serial.print(",");
+  Serial.print(rawY2);
+  Serial.print(",");
+  Serial.print(rawZ2);
+  Serial.print(",");
+  Serial.print(buttonState2 == LOW ? "1" : "0");
+  Serial.println(")");
+
+#else
+  int adjX1 = mapJoystickToPercentage(rawX1);
+  int adjY1 = mapJoystickToPercentage(rawY1);
+  int adjZ1 = mapJoystickToPercentage(rawZ1);
+
+  int adjX2 = mapJoystickToPercentage(rawX2);
+  int adjY2 = mapJoystickToPercentage(rawY2);
+  int adjZ2 = mapJoystickToPercentage(rawZ2);
+
+
+  // Print joystick values with adjusted dead zone
+  Serial.print("$arm_joy_panel(");
+  Serial.print(adjX1, DEC);
+  Serial.print(",");
+  Serial.print(adjY1);
+  Serial.print(",");
+  Serial.print(adjZ1);
+  Serial.print(",");
+  Serial.print(buttonState1 == LOW ? "1" : "0");
+
+  Serial.print(",");
+  Serial.print(adjX2, DEC);
+  Serial.print(",");
+  Serial.print(adjY2);
+  Serial.print(",");
+  Serial.print(adjZ2);
+  Serial.print(",");
+  Serial.print(buttonState2 == LOW ? "1" : "0");
+  Serial.println(")");
+
+#endif
+  // Function to map values to 1-100 percentage with deadzone 
+  }else{
+   delay(10); //delay not really needed but eh
+  }
+
+}
+
+// Function to scale joystick input to 1-100 with deadzone 
+int mapJoystickToPercentage(int rawValue) {
+  if (rawValue >= deadZoneMin && rawValue <= deadZoneMax) {
+    return 50; // Keep the dead zone at 50%
+  } 
+  else if (rawValue < deadZoneMin) {
+    return map(rawValue, minAnalog, deadZoneMin - 1, 1, 49); // Scale below dead zone
+  } 
+  else {
+    return map(rawValue, deadZoneMax + 1, maxAnalog, 51, 100); // Scale above dead zone
+  }
+}

arduino_firmware/general_left_0/general_left_0.ino

@@ -0,0 +1,87 @@
+//MCU for general left panel
+
+#define BAUDRATE 9600
+#define TX_RATE_HZ 30
+
+
+#define SW_T00_PIN 2
+//T0 is just one flat switch currently
+
+#define SW_T10_PIN 3
+#define SW_T11_PIN 4
+
+#define SW_T20_PIN 5
+#define SW_T21_PIN 6
+
+#define BT_T30_PIN 7
+#define BT_T31_PIN 8
+
+#define BT_T40_PIN 9
+#define BT_T41_PIN 10
+
+#define BT_T50_PIN 11 //This Button Needs replacement
+#define BT_T51_PIN 12
+
+
+long interval_ms = 99;
+long last_time = 0;
+long current_time = 0;
+
+void setup() {
+  pinMode(SW_T00_PIN, INPUT_PULLUP);
+  pinMode(SW_T10_PIN, INPUT_PULLUP);
+  pinMode(SW_T11_PIN, INPUT_PULLUP);
+  pinMode(SW_T20_PIN, INPUT_PULLUP);
+  pinMode(SW_T21_PIN, INPUT_PULLUP);
+  pinMode(BT_T30_PIN, INPUT_PULLUP);
+  pinMode(BT_T31_PIN, INPUT_PULLUP);
+  pinMode(BT_T40_PIN, INPUT_PULLUP);
+  pinMode(BT_T41_PIN, INPUT_PULLUP);
+  pinMode(BT_T50_PIN, INPUT_PULLUP);
+  pinMode(BT_T51_PIN, INPUT_PULLUP);
+  interval_ms = 1000/TX_RATE_HZ;
+
+  Serial.begin(BAUDRATE);
+
+}
+
+void loop() {
+current_time = millis();
+  if(current_time - last_time > interval_ms){
+      last_time = millis();
+      //might wanna use arrays but ehh our main problem is gonna be not mixing up the inputs
+  int SW_T00 = digitalRead(SW_T00_PIN) == LOW ? 1 : 0;
+  int SW_T10 = digitalRead(SW_T10_PIN) == LOW ? 1 : 0;
+  int SW_T11 = digitalRead(SW_T11_PIN) == LOW ? 0 : 1;
+  int SW_T20 = digitalRead(SW_T20_PIN) == LOW ? 1 : 0;
+  int SW_T21 = digitalRead(SW_T21_PIN) == LOW ? 1 : 0;;
+  int BT_T30 = digitalRead(BT_T30_PIN);
+  int BT_T31 = digitalRead(BT_T31_PIN);
+  int BT_T40 = digitalRead(BT_T40_PIN);
+  int BT_T41 = digitalRead(BT_T41_PIN);
+  int BT_T50 = digitalRead(BT_T50_PIN);
+  int BT_T51 = digitalRead(BT_T51_PIN);
+
+
+//  int SW_T30 = digitalRead(SW_T00_PIN);
+
+  String outmsg = "$gen_left_A(";
+  outmsg = outmsg + SW_T00 + ","; // for some reason += messes up the string
+  outmsg = outmsg + SW_T10 + ",";
+  outmsg = outmsg + SW_T11 + ",";
+  outmsg = outmsg + SW_T20 + ",";
+  outmsg = outmsg + SW_T21 + ",";
+  outmsg = outmsg + BT_T30 + ",";
+  outmsg = outmsg + BT_T31 + ",";
+  outmsg = outmsg + BT_T40 + ",";
+  outmsg = outmsg + BT_T41 + ",";
+  outmsg = outmsg + BT_T50 + ",";
+  outmsg = outmsg + BT_T51;
+
+//  outmsg += SW_T10 + "";
+  outmsg = outmsg + ")";
+  Serial.println(outmsg);
+
+  }
+  delay(1);
+}

zephyr_projects/arm_firmware_teensy41/include/armFirmware.h

@@ -45,7 +45,7 @@ this is not easy
 #define DEBUG_MSGS true
 
 // general parameters
-#define NUM_AXES 6
+#define NUM_AXES 7
 #define ON 0
 #define OFF 1
 #define SW_ON 0
@@ -59,6 +59,7 @@ this is not easy
 
 #define ARM_DEG_RESOLUTION 0.01
 #define ARM_DEG_VELOCITY_RESOLUTION 0.05
+#define EE_INDEX 6
 
 #define HOME 1
 #define ABSOLUTE_TARGET_POSITION 2
@@ -97,33 +98,35 @@ this is not easy
 
 //weird pin mappings
 //all the usefull documentation I could find: https://docs.zephyrproject.org/latest/boards/arm/teensy4/doc/index.html 
-
+// https://docs.zephyrproject.org/latest/boards/pjrc/teensy4/doc/index.html
 
 // Motor pins     
-inline int stepPins[6] =   {6, 8, 2, 10, 12, 25}; 
-inline int dirPins[6] =    {5, 7, 1, 9, 11, 24}; 
+// inline int stepPins[6] =   {6, 8, 2, 10, 12, 25}; 
+// inline int dirPins[6] =    {5, 7, 1, 9, 11, 24}; 
 
 //To GPIO devs                  //not working: 3 //working one dir: 1   // working: 2, 4, 5, 6 
-inline int stepGPIO_PIN[6][2] =   {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {1,13}}; 
-inline int dirGPIO_PIN[6][2] =    {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {1,12}}; 
+inline int stepGPIO_PIN[NUM_AXES][2] =   {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {1,13}, {2,18}}; 
+inline int dirGPIO_PIN[NUM_AXES][2] =    {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {1,12}, {2,19}}; 
+// inline int stepGPIO_PIN[NUM_AXES][2] =   {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {2,18}, {1, 13}}; 
+// inline int dirGPIO_PIN[NUM_AXES][2] =    {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {2,19}, {1, 12}}; 
 inline int arpo = 0;
 
 // Encoder pins
-inline int encPinA[6] = {17, 38, 40, 36, 13, 15};
-inline int encPinB[6] = {16, 37, 39, 35, 41, 14};
+// inline int encPinA[6] = {17, 38, 40, 36, 13, 15};
+// inline int encPinB[6] = {16, 37, 39, 35, 41, 14};
 
 // limit switch pins
-inline int limPins[6] = {18, 19, 20, 21, 23, 22};
-inline int limGPIO_PIN[6][6] = {{1,17}, {1,16}, {1,26}, {1,27}, {1,25}, {1,24}};
+// inline int limPins[6] = {18, 19, 20, 21, 23, 22};
+inline int limGPIO_PIN[NUM_AXES][2] = {{1,17}, {1,16}, {1,26}, {1,27}, {1,25}, {1,24}, {1, 25}}; //4,8
 
 // pulses per revolution for motors
-inline int ppr[6] = {400, 400, 400, 400, 400, 400};
+inline int ppr[NUM_AXES] = {400, 400, 400, 400, 400, 400, 400};
 
 // Gear Reductions
 // inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 5.18}; 
-inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 150.00}; //? Double check with arm 
+inline float red[NUM_AXES] = {50.0, 160.0, 92.3077, 250, 57.0, 170.00, 100.0}; //? Double check with arm 
 // inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 20.00}; //? Double check with arm -> August 11th
-
+//?A6 10:17 for belts -> 170 to one
 
 //from old driver
 // // Motor pins      
@@ -145,7 +148,7 @@ inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 150.00}; //? Double c
 
 // End effector variables
 const int closePos = 0; 
-const int openPos = 90000; //TODO: needs to be set with proper step value for gear redcution and ppr
+const int openPos = 9000; //TODO: needs to be set with proper step value for gear redcution and ppr
 const int EEstepPin = 4;
 const int EEdirPin = 3;
 const int speedEE = 1000;
@@ -161,7 +164,7 @@ inline bool arm_inited = false;
 // Encoder Variables
 inline int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES];
 const int pprEnc = 512;
-const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12};
+// const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12};
 inline float ENC_STEPS_PER_DEG[NUM_AXES];
 
 inline int control_mode = VELOCITY_CONTROL;
@@ -231,4 +234,3 @@ int get_gpio(int dev, int pin);
 
 
 
-

zephyr_projects/arm_firmware_teensy41/load_hex.sh

@@ -1,5 +1,4 @@
 echo LOADING HEX
-teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex 
+sudo teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex 
 sleep 1.0
-teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex 
-
+sudo teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex