Skip to content

Commit f2e91ee

Browse files
RoozkiRoozki
committed
commit cbs panel code
1 parent d747c08 commit f2e91ee

File tree

6 files changed

+329
-86
lines changed

6 files changed

+329
-86
lines changed

‎arduino_firmware/arm_joystick_panel/arm_joystick_panel.ino‎

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
2+
3+
//Joystick right Pins
4+
const int xAxisPin2 = A1;
5+
const int yAxisPin2 = A0;
6+
const int zAxisPin2 = A2;
7+
const int buttonPin2 = 2;
8+
9+
//Joystick left Pins
10+
const int xAxisPin1 = A4;
11+
const int yAxisPin1 = A3;
12+
const int zAxisPin1 = A5;
13+
const int buttonPin1 = 3;
14+
15+
const int centerValue = 512; // Analog midpoint
16+
const int deadZoneMin = 440; // Lower dead zone threshold
17+
const int deadZoneMax = 583; // Upper dead zone threshold
18+
const int minAnalog = 0; // Min Analog value
19+
const int maxAnalog = 1023; // Max Analog value
20+
21+
//#define minAnalogZ 0
22+
//#define maxAnalogZ 600
23+
24+
#define RATE_HZ 100
25+
26+
//#define TEST //Uncomment to print raw values instead of adj values
27+
28+
long interval_ms = 99;
29+
long last_time = 0;
30+
long current_time = 0;
31+
void setup() {
32+
pinMode(buttonPin1, INPUT_PULLUP);
33+
pinMode(buttonPin2, INPUT_PULLUP);
34+
Serial.begin(19200);
35+
interval_ms = 1/RATE_HZ * 1000; //interval in ms
36+
37+
}
38+
39+
void loop() {
40+
current_time = millis();
41+
if(current_time - last_time > interval_ms){
42+
last_time = millis();
43+
44+
45+
//Read raw values
46+
int rawX1 = analogRead(xAxisPin1);
47+
int rawY1 = analogRead(yAxisPin1);
48+
int rawZ1 = analogRead(zAxisPin1);
49+
int buttonState1 = digitalRead(buttonPin1);
50+
51+
int rawX2 = analogRead(xAxisPin2);
52+
int rawY2 = analogRead(yAxisPin2);
53+
int rawZ2 = analogRead(zAxisPin2);
54+
int buttonState2 = digitalRead(buttonPin2);
55+
56+
#ifdef TEST
57+
Serial.print("TESING: ");
58+
// Print joystick values with adjusted dead zone
59+
Serial.print(rawX1);
60+
Serial.print(",");
61+
Serial.print(rawY1);
62+
Serial.print(",");
63+
Serial.print(rawZ1);
64+
Serial.print(",");
65+
Serial.print(buttonState1 == LOW ? "1" : "0");
66+
67+
Serial.print(",");
68+
Serial.print(rawX2);
69+
Serial.print(",");
70+
Serial.print(rawY2);
71+
Serial.print(",");
72+
Serial.print(rawZ2);
73+
Serial.print(",");
74+
Serial.print(buttonState2 == LOW ? "1" : "0");
75+
Serial.println(")");
76+
77+
#else
78+
int adjX1 = mapJoystickToPercentage(rawX1);
79+
int adjY1 = mapJoystickToPercentage(rawY1);
80+
int adjZ1 = mapJoystickToPercentage(rawZ1);
81+
82+
int adjX2 = mapJoystickToPercentage(rawX2);
83+
int adjY2 = mapJoystickToPercentage(rawY2);
84+
int adjZ2 = mapJoystickToPercentage(rawZ2);
85+
86+
87+
// Print joystick values with adjusted dead zone
88+
Serial.print("$arm_joy_panel(");
89+
Serial.print(adjX1, DEC);
90+
Serial.print(",");
91+
Serial.print(adjY1);
92+
Serial.print(",");
93+
Serial.print(adjZ1);
94+
Serial.print(",");
95+
Serial.print(buttonState1 == LOW ? "1" : "0");
96+
97+
Serial.print(",");
98+
Serial.print(adjX2, DEC);
99+
Serial.print(",");
100+
Serial.print(adjY2);
101+
Serial.print(",");
102+
Serial.print(adjZ2);
103+
Serial.print(",");
104+
Serial.print(buttonState2 == LOW ? "1" : "0");
105+
Serial.println(")");
106+
107+
#endif
108+
// Function to map values to 1-100 percentage with deadzone
109+
}else{
110+
delay(10); //delay not really needed but eh
111+
}
112+
113+
}
114+
115+
// Function to scale joystick input to 1-100 with deadzone
116+
int mapJoystickToPercentage(int rawValue) {
117+
if (rawValue >= deadZoneMin && rawValue <= deadZoneMax) {
118+
return 50; // Keep the dead zone at 50%
119+
}
120+
else if (rawValue < deadZoneMin) {
121+
return map(rawValue, minAnalog, deadZoneMin - 1, 1, 49); // Scale below dead zone
122+
}
123+
else {
124+
return map(rawValue, deadZoneMax + 1, maxAnalog, 51, 100); // Scale above dead zone
125+
}
126+
}

‎arduino_firmware/general_left_0/general_left_0.ino‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
//MCU for general left panel
2+
3+
#define BAUDRATE 9600
4+
#define TX_RATE_HZ 30
5+
6+
7+
#define SW_T00_PIN 2
8+
//T0 is just one flat switch currently
9+
10+
#define SW_T10_PIN 3
11+
#define SW_T11_PIN 4
12+
13+
#define SW_T20_PIN 5
14+
#define SW_T21_PIN 6
15+
16+
#define BT_T30_PIN 7
17+
#define BT_T31_PIN 8
18+
19+
#define BT_T40_PIN 9
20+
#define BT_T41_PIN 10
21+
22+
#define BT_T50_PIN 11 //This Button Needs replacement
23+
#define BT_T51_PIN 12
24+
25+
26+
long interval_ms = 99;
27+
long last_time = 0;
28+
long current_time = 0;
29+
30+
void setup() {
31+
pinMode(SW_T00_PIN, INPUT_PULLUP);
32+
pinMode(SW_T10_PIN, INPUT_PULLUP);
33+
pinMode(SW_T11_PIN, INPUT_PULLUP);
34+
pinMode(SW_T20_PIN, INPUT_PULLUP);
35+
pinMode(SW_T21_PIN, INPUT_PULLUP);
36+
pinMode(BT_T30_PIN, INPUT_PULLUP);
37+
pinMode(BT_T31_PIN, INPUT_PULLUP);
38+
pinMode(BT_T40_PIN, INPUT_PULLUP);
39+
pinMode(BT_T41_PIN, INPUT_PULLUP);
40+
pinMode(BT_T50_PIN, INPUT_PULLUP);
41+
pinMode(BT_T51_PIN, INPUT_PULLUP);
42+
interval_ms = 1000/TX_RATE_HZ;
43+
44+
Serial.begin(BAUDRATE);
45+
46+
}
47+
48+
void loop() {
49+
current_time = millis();
50+
if(current_time - last_time > interval_ms){
51+
last_time = millis();
52+
//might wanna use arrays but ehh our main problem is gonna be not mixing up the inputs
53+
int SW_T00 = digitalRead(SW_T00_PIN) == LOW ? 1 : 0;
54+
int SW_T10 = digitalRead(SW_T10_PIN) == LOW ? 1 : 0;
55+
int SW_T11 = digitalRead(SW_T11_PIN) == LOW ? 0 : 1;
56+
int SW_T20 = digitalRead(SW_T20_PIN) == LOW ? 1 : 0;
57+
int SW_T21 = digitalRead(SW_T21_PIN) == LOW ? 1 : 0;;
58+
int BT_T30 = digitalRead(BT_T30_PIN);
59+
int BT_T31 = digitalRead(BT_T31_PIN);
60+
int BT_T40 = digitalRead(BT_T40_PIN);
61+
int BT_T41 = digitalRead(BT_T41_PIN);
62+
int BT_T50 = digitalRead(BT_T50_PIN);
63+
int BT_T51 = digitalRead(BT_T51_PIN);
64+
65+
66+
// int SW_T30 = digitalRead(SW_T00_PIN);
67+
68+
String outmsg = "$gen_left_A(";
69+
outmsg = outmsg + SW_T00 + ","; // for some reason += messes up the string
70+
outmsg = outmsg + SW_T10 + ",";
71+
outmsg = outmsg + SW_T11 + ",";
72+
outmsg = outmsg + SW_T20 + ",";
73+
outmsg = outmsg + SW_T21 + ",";
74+
outmsg = outmsg + BT_T30 + ",";
75+
outmsg = outmsg + BT_T31 + ",";
76+
outmsg = outmsg + BT_T40 + ",";
77+
outmsg = outmsg + BT_T41 + ",";
78+
outmsg = outmsg + BT_T50 + ",";
79+
outmsg = outmsg + BT_T51;
80+
81+
// outmsg += SW_T10 + "";
82+
outmsg = outmsg + ")";
83+
Serial.println(outmsg);
84+
85+
}
86+
delay(1);
87+
}

‎zephyr_projects/arm_firmware_teensy41/include/armFirmware.h‎

Lines changed: 18 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ this is not easy
4545
#define DEBUG_MSGS true
4646

4747
// general parameters
48-
#define NUM_AXES 6
48+
#define NUM_AXES 7
4949
#define ON 0
5050
#define OFF 1
5151
#define SW_ON 0
@@ -59,6 +59,7 @@ this is not easy
5959

6060
#define ARM_DEG_RESOLUTION 0.01
6161
#define ARM_DEG_VELOCITY_RESOLUTION 0.05
62+
#define EE_INDEX 6
6263

6364
#define HOME 1
6465
#define ABSOLUTE_TARGET_POSITION 2
@@ -97,33 +98,35 @@ this is not easy
9798

9899
//weird pin mappings
99100
//all the usefull documentation I could find: https://docs.zephyrproject.org/latest/boards/arm/teensy4/doc/index.html
100-
101+
// https://docs.zephyrproject.org/latest/boards/pjrc/teensy4/doc/index.html
101102

102103
// Motor pins
103-
inline int stepPins[6] = {6, 8, 2, 10, 12, 25};
104-
inline int dirPins[6] = {5, 7, 1, 9, 11, 24};
104+
// inline int stepPins[6] = {6, 8, 2, 10, 12, 25};
105+
// inline int dirPins[6] = {5, 7, 1, 9, 11, 24};
105106

106107
//To GPIO devs //not working: 3 //working one dir: 1 // working: 2, 4, 5, 6
107-
inline int stepGPIO_PIN[6][2] = {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {1,13}};
108-
inline int dirGPIO_PIN[6][2] = {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {1,12}};
108+
inline int stepGPIO_PIN[NUM_AXES][2] = {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {1,13}, {2,18}};
109+
inline int dirGPIO_PIN[NUM_AXES][2] = {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {1,12}, {2,19}};
110+
// inline int stepGPIO_PIN[NUM_AXES][2] = {{2,10}, {2,16}, {1,3}, {2,0}, {2,1}, {2,18}, {1, 13}};
111+
// inline int dirGPIO_PIN[NUM_AXES][2] = {{1,30}, {2,17}, {1,2}, {2,11}, {2,2}, {2,19}, {1, 12}};
109112
inline int arpo = 0;
110113

111114
// Encoder pins
112-
inline int encPinA[6] = {17, 38, 40, 36, 13, 15};
113-
inline int encPinB[6] = {16, 37, 39, 35, 41, 14};
115+
// inline int encPinA[6] = {17, 38, 40, 36, 13, 15};
116+
// inline int encPinB[6] = {16, 37, 39, 35, 41, 14};
114117

115118
// limit switch pins
116-
inline int limPins[6] = {18, 19, 20, 21, 23, 22};
117-
inline int limGPIO_PIN[6][6] = {{1,17}, {1,16}, {1,26}, {1,27}, {1,25}, {1,24}};
119+
// inline int limPins[6] = {18, 19, 20, 21, 23, 22};
120+
inline int limGPIO_PIN[NUM_AXES][2] = {{1,17}, {1,16}, {1,26}, {1,27}, {1,25}, {1,24}, {1, 25}}; //4,8
118121

119122
// pulses per revolution for motors
120-
inline int ppr[6] = {400, 400, 400, 400, 400, 400};
123+
inline int ppr[NUM_AXES] = {400, 400, 400, 400, 400, 400, 400};
121124

122125
// Gear Reductions
123126
// inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 5.18};
124-
inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 150.00}; //? Double check with arm
127+
inline float red[NUM_AXES] = {50.0, 160.0, 92.3077, 250, 57.0, 170.00, 100.0}; //? Double check with arm
125128
// inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 20.00}; //? Double check with arm -> August 11th
126-
129+
//?A6 10:17 for belts -> 170 to one
127130

128131
//from old driver
129132
// // Motor pins
@@ -145,7 +148,7 @@ inline float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 150.00}; //? Double c
145148

146149
// End effector variables
147150
const int closePos = 0;
148-
const int openPos = 90000; //TODO: needs to be set with proper step value for gear redcution and ppr
151+
const int openPos = 9000; //TODO: needs to be set with proper step value for gear redcution and ppr
149152
const int EEstepPin = 4;
150153
const int EEdirPin = 3;
151154
const int speedEE = 1000;
@@ -161,7 +164,7 @@ inline bool arm_inited = false;
161164
// Encoder Variables
162165
inline int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES];
163166
const int pprEnc = 512;
164-
const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12};
167+
// const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12};
165168
inline float ENC_STEPS_PER_DEG[NUM_AXES];
166169

167170
inline int control_mode = VELOCITY_CONTROL;
@@ -231,4 +234,3 @@ int get_gpio(int dev, int pin);
231234

232235

233236

234-

‎zephyr_projects/arm_firmware_teensy41/load_hex.sh‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
echo LOADING HEX
2-
teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex
2+
sudo teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex
33
sleep 1.0
4-
teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex
5-
4+
sudo teensy_loader_cli -w -v --mcu TEENSY41 build/zephyr/zephyr.hex

0 commit comments

Comments
 (0)