Skip to content

Commit 643a573

Browse files
elector102elector102
authored
Merge pull request #32 from Ekumen-OS/gripper_hardware
Fix issues to make gripper work on the real robot arm.
2 parents f817e25 + 9b6fb5f commit 643a573

File tree

14 files changed

+894
-68
lines changed

14 files changed

+894
-68
lines changed

‎ar4_hardware_interface/ar4_firmware/firmware_gripper_arduino.ino‎

Lines changed: 252 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,252 @@
1+
/* AR4 Annin Robot Control Software Arduino Nano sketch
2+
Copyright (c) 2021, Chris Annin
3+
All rights reserved.
4+
5+
You are free to share, copy and redistribute in any medium
6+
or format. You are free to remix, transform and build upon
7+
this material.
8+
9+
Redistribution and use in source and binary forms, with or without
10+
modification, are permitted provided that the following conditions are met:
11+
12+
Redistributions of source code must retain the above copyright
13+
notice, this list of conditions and the following disclaimer.
14+
Redistribution of this software in source or binary forms shall be
15+
free of all charges or fees to the recipient of this software.
16+
Redistributions in binary form must reproduce the above copyright
17+
notice, this list of conditions and the following disclaimer in the
18+
documentation and/or other materials provided with the distribution.
19+
you must give appropriate credit and indicate if changes were made.
20+
You may do so in any reasonable manner, but not in any way that suggests the
21+
licensor endorses you or your use.
22+
Selling AR2 software, robots, robot parts, or any versions of robots
23+
or software based on this work is strictly prohibited.
24+
25+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28+
ARE DISCLAIMED. IN NO EVENT SHALL CHRIS ANNIN BE LIABLE FOR ANY DIRECT,
29+
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
30+
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31+
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
32+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
33+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
34+
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
35+
36+
chris.annin@gmail.com
37+
*/
38+
39+
#include <Servo.h>
40+
41+
// Firmware version
42+
const char* VERSION = "0.1.0";
43+
44+
String inData;
45+
46+
Servo servo0;
47+
Servo servo1;
48+
Servo servo2;
49+
Servo servo3;
50+
Servo servo4;
51+
Servo servo5;
52+
Servo servo6;
53+
54+
const int Input2 = 2;
55+
const int Input3 = 3;
56+
const int Input4 = 4;
57+
const int Input5 = 5;
58+
const int Input6 = 6;
59+
const int Input7 = 7;
60+
const int CurrentSensorPin = A7; // Change to use A7 for current sensing
61+
62+
const int Output8 = 8;
63+
const int Output9 = 9;
64+
const int Output10 = 10;
65+
const int Output11 = 11;
66+
const int Output12 = 12;
67+
const int Output13 = 13;
68+
69+
void setup() {
70+
// run once:
71+
Serial.begin(115200);
72+
73+
pinMode(A0, OUTPUT);
74+
pinMode(A1, OUTPUT);
75+
pinMode(A2, OUTPUT);
76+
pinMode(A3, OUTPUT);
77+
pinMode(A4, OUTPUT);
78+
pinMode(A5, OUTPUT);
79+
pinMode(A6, OUTPUT);
80+
pinMode(CurrentSensorPin, INPUT); // Set current sensor pin as input
81+
82+
pinMode(Input2, INPUT_PULLUP);
83+
pinMode(Input3, INPUT_PULLUP);
84+
pinMode(Input4, INPUT_PULLUP);
85+
pinMode(Input5, INPUT_PULLUP);
86+
pinMode(Input6, INPUT_PULLUP);
87+
pinMode(Input7, INPUT_PULLUP);
88+
89+
pinMode(Output8, OUTPUT);
90+
pinMode(Output9, OUTPUT);
91+
pinMode(Output10, OUTPUT);
92+
pinMode(Output11, OUTPUT);
93+
pinMode(Output12, OUTPUT);
94+
pinMode(Output13, OUTPUT);
95+
96+
servo0.attach(A0);
97+
servo1.attach(A1);
98+
servo2.attach(A2);
99+
servo3.attach(A3);
100+
servo4.attach(A4);
101+
servo5.attach(A5);
102+
servo6.attach(A6);
103+
104+
// Make servo0 (the servo gripper) go to an arbitrary initial position,
105+
// otherwise it goes to some unknow position beyond the acceptable range
106+
servo0.write(30);
107+
}
108+
109+
float readCurrent() {
110+
int sensorValue = analogRead(CurrentSensorPin);
111+
// Convert analog reading to current
112+
// For ACS712 5A version: sensitivity is 185mV/A
113+
float voltage = sensorValue * (5.0 / 1024.0);
114+
float current = (voltage - 2.5) / 0.185; // 2.5V is the offset at 0A
115+
return current;
116+
}
117+
118+
void loop() {
119+
// start loop
120+
while (Serial.available() > 0) {
121+
char received = Serial.read();
122+
inData += received;
123+
// Process message when new line character is received
124+
if (received == '\n') {
125+
String function = inData.substring(0, 2);
126+
127+
if (function == "ST") {
128+
Serial.println(String(VERSION));
129+
}
130+
131+
//-----COMMAND TO MOVE SERVO-----
132+
if (function == "SV") {
133+
int SVstart = inData.indexOf('V');
134+
int POSstart = inData.indexOf('P');
135+
int servoNum = inData.substring(SVstart + 1, POSstart).toInt();
136+
int servoPOS = inData.substring(POSstart + 1).toInt();
137+
if (servoNum == 0) {
138+
servo0.write(servoPOS);
139+
}
140+
if (servoNum == 1) {
141+
servo1.write(servoPOS);
142+
}
143+
if (servoNum == 2) {
144+
servo2.write(servoPOS);
145+
}
146+
if (servoNum == 3) {
147+
servo3.write(servoPOS);
148+
}
149+
if (servoNum == 4) {
150+
servo4.write(servoPOS);
151+
}
152+
if (servoNum == 5) {
153+
servo5.write(servoPOS);
154+
}
155+
if (servoNum == 6) {
156+
servo6.write(servoPOS);
157+
}
158+
Serial.println("Done");
159+
}
160+
161+
//-----COMMAND TO READ SERVO POSITION-----
162+
if (function == "SP") {
163+
int SPstart = inData.indexOf('P');
164+
int servoNum = inData.substring(SPstart + 1).toInt();
165+
if (servoNum == 0) {
166+
Serial.println(servo0.read());
167+
}
168+
if (servoNum == 1) {
169+
Serial.println(servo1.read());
170+
}
171+
if (servoNum == 2) {
172+
Serial.println(servo2.read());
173+
}
174+
if (servoNum == 3) {
175+
Serial.println(servo3.read());
176+
}
177+
if (servoNum == 4) {
178+
Serial.println(servo4.read());
179+
}
180+
if (servoNum == 5) {
181+
Serial.println(servo5.read());
182+
}
183+
if (servoNum == 6) {
184+
Serial.println(servo6.read());
185+
}
186+
}
187+
188+
//-----COMMAND IF INPUT THEN JUMP-----
189+
if (function == "JF") {
190+
int IJstart = inData.indexOf('X');
191+
int IJTabstart = inData.indexOf('T');
192+
int IJInputNum = inData.substring(IJstart + 1, IJTabstart).toInt();
193+
if (digitalRead(IJInputNum) == HIGH) {
194+
Serial.println("T");
195+
}
196+
if (digitalRead(IJInputNum) == LOW) {
197+
Serial.println("F");
198+
}
199+
}
200+
201+
//-----COMMAND SET OUTPUT ON-----
202+
if (function == "ON") {
203+
int ONstart = inData.indexOf('X');
204+
int outputNum = inData.substring(ONstart + 1).toInt();
205+
digitalWrite(outputNum, HIGH);
206+
Serial.println("Done");
207+
}
208+
//-----COMMAND SET OUTPUT OFF-----
209+
if (function == "OF") {
210+
int ONstart = inData.indexOf('X');
211+
int outputNum = inData.substring(ONstart + 1).toInt();
212+
digitalWrite(outputNum, LOW);
213+
Serial.println("Done");
214+
}
215+
//-----COMMAND TO WAIT INPUT ON-----
216+
if (function == "WI") {
217+
int WIstart = inData.indexOf('N');
218+
int InputNum = inData.substring(WIstart + 1).toInt();
219+
while (digitalRead(InputNum) == LOW) {
220+
delay(100);
221+
}
222+
Serial.println("Done");
223+
}
224+
//-----COMMAND TO WAIT INPUT OFF-----
225+
if (function == "WO") {
226+
int WIstart = inData.indexOf('N');
227+
int InputNum = inData.substring(WIstart + 1).toInt();
228+
229+
// String InputStr = String("Input" + InputNum);
230+
// uint8_t Input = atoi(InputStr.c_str ());
231+
while (digitalRead(InputNum) == HIGH) {
232+
delay(100);
233+
}
234+
Serial.println("Done");
235+
}
236+
237+
//-----COMMAND TO READ CURRENT SENSOR-----
238+
if (function == "CR") {
239+
float current = readCurrent();
240+
Serial.println(current);
241+
}
242+
243+
//-----COMMAND ECHO TEST MESSAGE-----
244+
if (function == "TM") {
245+
String echo = inData.substring(2);
246+
Serial.println(echo);
247+
}
248+
249+
inData = ""; // Clear received buffer
250+
}
251+
}
252+
}

0 commit comments

Comments
 (0)