auto close

Gripper/Gripper.ino

@@ -19,9 +19,10 @@ int input = 0;
 XYZrobotServo servo1(servoSerial, 2);
 
 const uint8_t playtime = 100;
+const int max_angle = 712,min_angle = 312;
 
-int button_oben = 2;
-int button_unten = 3;
+const byte button_oben = 20;
+const byte button_unten = 21;
 int ain1 = 4;
 int ain2 = 5;
 int pwma = 8;
@@ -31,21 +32,23 @@ int val[10] = {0,0,0,0,0};
 int choice;
 
 void ax12_1(int position){
-     if((position<300) || (position>700)){
+     if((position<min_angle) || (position>max_angle)){
     Serial.print("invalide position. Try again");
-    return;}
+    return;
+    }
   ax12a.moveSpeed(ID1,position, playtime);
 }
 
 void ax12_2(int position){
-     if((position<300) || (position>700)){
+     if((position<min_angle) || (position>max_angle)){
     Serial.print("invalide position. Try again");
-    return;}
+    return;
+    }
   ax12a.moveSpeed(ID2,position, playtime);
 }
 
 void a116(int position){
-    if((position<300) || (position>700)){
+    if((position<min_angle) || (position>max_angle)){
     Serial.print("invalide position. Try again");
     return;
   }
@@ -53,7 +56,7 @@ void a116(int position){
 }
 
 void motor(int speed, int dir){
-  if (dir == 1){
+  if (dir == 0){
     digitalWrite(ain1,HIGH);
     digitalWrite(ain2,LOW);
     }
@@ -64,6 +67,26 @@ void motor(int speed, int dir){
     analogWrite(pwma,speed);
   }
 
+void auto_close(){
+  //  Serial.print("Servo 1:  ");
+//  Serial.println(ax12a.readLoad(ID1));
+//  Serial.print("Servo 2:  ");
+ // Serial.println(ax12a.readLoad(ID2));
+ // delay(300);
+ while(((ax12a.readLoad(ID1)<1450)) && (ax12a.readLoad(ID2)<1400)){
+    Serial.print("Servo 1:  ");
+Serial.println(ax12a.readLoad(ID1));
+ Serial.print("Servo 2:  ");
+ Serial.println(ax12a.readLoad(ID2));
+    ax12a.move(ID1,max_angle);
+    ax12a.move(ID2,min_angle);
+    }
+  
+  ax12a.move(ID1,ax12a.readPosition(ID1)+10);
+  ax12a.move(ID2,ax12a.readPosition(ID2)-10);
+  }
+  
+
 void setup() {
   Serial.begin(9600);
   Serial.setTimeout(1);
@@ -71,6 +94,8 @@ void setup() {
   ax12a.begin(BaudRate, DirectionPin, &Serial3);
   ax12a.setEndless(ID1, OFF);
   ax12a.setEndless(ID2, OFF);
+  ax12a.setMaxTorque(ID1, 1000);
+  ax12a.setMaxTorque(ID2, 1000);
 
   servoSerial.begin(115200);
 
@@ -85,6 +110,11 @@ void setup() {
   attachInterrupt(digitalPinToInterrupt(button_unten),interrupt2,LOW);
 }
 void loop() {
+   //Serial.print("Servo 1:  ");
+ //Serial.println(ax12a.readLoad(ID1));
+ //Serial.print("Servo 2:  ");
+ //Serial.println(ax12a.readLoad(ID2));
+ //delay(300);
  if (Serial.available()){
    //Serial.print(test);
    for(int n = 0; n < 10 ; n++){
@@ -117,19 +147,26 @@ void loop() {
     motor(position,val[1]);
     Serial.print("Ready");
     break;
+    case 5:
+    auto_close();
+    Serial.print("Ready");
+    break;
   }
  }
 }
 
 void interrupt1(){
-    Serial.println("interrupt button oben");
-    motor(254,' ');
-    delay(200);
-    motor(0,' ');
+    //Serial.println("interrupt button oben");
+     digitalWrite(36,HIGH);
+    motor(254,1);
+    motor(0,1);
+       digitalWrite(36,LOW);
   }
 void interrupt2(){
-    Serial.println("interrupt button unten");
-    motor(254,'a');
-    delay(200);
-    motor(0,'a');
+    //Serial.println("interrupt button unten");
+    
+    digitalWrite(36,HIGH);
+    motor(254,0);
+    motor(0,0);
+    digitalWrite(36,LOW);
     }

Gripper_gui_test.py

@@ -7,17 +7,20 @@ slider_hight = 40
 button_width = 10
 button_hight = 3
 
+open_value = 650
+close_value = 350
+
 
 layout = [
     [sg.Text('Servomotor 1')],
-    [sg.Slider(range=(300, 700), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER1-'), sg.Button('Reset', key='-BUTTON1-',size=(button_width,button_hight))],    #enable_events=True,
+    [sg.Text('CLOSE'),sg.Slider(range=(312, 712), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER1-'),sg.Text('OPEN'), sg.Button('Reset', key='-BUTTON1-',size=(button_width,button_hight))],    #enable_events=True,
     [sg.Text('Servomotor 2')],
-    [sg.Slider(range=(300, 700), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER2-'), sg.Button('Reset', key='-BUTTON2-',size=(button_width,button_hight))],     #enable_events=True,
+    [sg.Text('CLOSE'),sg.Slider(range=(312, 712), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER2-'),sg.Text('OPEN'), sg.Button('Reset', key='-BUTTON2-',size=(button_width,button_hight))],     #enable_events=True,
     [sg.Text('Servomotor 3')],
-    [sg.Slider(range=(300, 700), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER3-'), sg.Button('Reset', key='-BUTTON3-',size=(button_width,button_hight))], #enable_events=True,
+    [sg.Text('CLOSE'),sg.Slider(range=(312, 712), size=(slider_width, slider_hight), default_value=512, orientation='h', key='-SLIDER3-'),sg.Text('OPEN'), sg.Button('Reset', key='-BUTTON3-',size=(button_width,button_hight))], #enable_events=True,
     [sg.Text('Motor')],
-    [sg.Slider(range=(-254, 254), size=(slider_width, slider_hight), default_value=0, orientation='h', key='-SLIDER4-'), sg.Button('Reset', key='-BUTTON4-',size=(button_width,button_hight))],
-    [sg.Button('OK',size=(button_width,button_hight)),sg.Button('Reset',key='-BUTTON5-',size=(button_width,button_hight))]
+    [sg.Text('DOWN'),sg.Slider(range=(-254, 254), size=(slider_width, slider_hight), default_value=0, orientation='h', key='-SLIDER4-'),sg.Text('   UP  '), sg.Button('Reset', key='-BUTTON4-',size=(button_width,button_hight))],
+    [sg.Button('OK',key='-BUTTON9-',size=(button_width,button_hight)),sg.Button('Reset',key='-BUTTON5-',size=(button_width,button_hight)),sg.Button('Open',key='-BUTTON6-',size=(button_width,button_hight)),sg.Button('Close',key='-BUTTON7-',size=(button_width,button_hight)),sg.Button('auto Close',key='-BUTTON8-',size=(button_width,button_hight))]
 ]
 arduino = serial.Serial(port='/dev/ttyUSB0', baudrate=9600, timeout=.1)
 def write_read(x):
@@ -28,27 +31,58 @@ def write_read(x):
     return data
 # GUI erstellen
 window = sg.Window('Slider GUI', layout,resizable=True,ttk_theme='classic')
+def write():
+        # Werte der Schieberegler abrufen
+    slider1_value = 512+(512-values['-SLIDER1-'])
+    slider2_value = 512+(512-values['-SLIDER2-'])
+    slider3_value = values['-SLIDER3-']
+    slider4_value = values['-SLIDER4-']
+
+    # Beispiel: Ausgabe der aktuellen Werte der Schieberegler
+    print("Slider 1:", slider1_value)
+    print("Slider 2:", slider2_value)
+    print("Slider 3:", slider3_value)
+    print("Slider 4:", slider4_value)
+    val_1 = "0"
+    if slider4_value < 0:
+        slider4_value = numpy.abs(slider4_value)
+        val_1 = "1"
+    print("1 " + str(int(slider1_value)))
+    print("2 " + str(int(slider2_value)))
+    print( "3 " + str(int(slider3_value)))
+    print("4" +val_1+ str(int(slider4_value)))
+    write_read(str("1 "+str(int(slider1_value))))
+    write_read(str("2 "+str(int(slider2_value))))
+    write_read(str("3 "+str(int(slider3_value))))
+    write_read(str("4"+val_1+str(int(slider4_value))))
 # GUI-Ereignisschleife
 while True:
-    event, values = window.read()       
+    event, values = window.read()  
     if event == sg.WINDOW_CLOSED:
         break
+    if event == '-BUTTON9-':
+        print('Button OK was clicked')
+        write()
     if event == '-BUTTON1-':
         print('Button 1 was clicked')
         window['-SLIDER1-'].update(512)
         values['-SLIDER1-']=512.0
+        write()
     elif event == '-BUTTON2-':
         print('Button 2 was clicked')
         window['-SLIDER2-'].update(512)
         values['-SLIDER2-']=512.0
+        write()
     elif event == '-BUTTON3-':
         print('Button 3 was clicked')
         window['-SLIDER3-'].update(512)
         values['-SLIDER3-']=512.0
+        write()
     elif event == '-BUTTON4-':
         print('Button 4 was clicked')
-        window['-SLIDER4-'].update(512)
-        values['-SLIDER4-']=512.0
+        window['-SLIDER4-'].update(0)
+        values['-SLIDER4-']=0
+        write()
     elif event == '-BUTTON5-':
         print('Button 5 was clicked')
         window['-SLIDER1-'].update(512)
@@ -57,31 +91,39 @@ while True:
         values['-SLIDER2-']=512.0
         window['-SLIDER3-'].update(512)
         values['-SLIDER3-']=512.0
-        window['-SLIDER4-'].update()
+        window['-SLIDER4-'].update(0)
         values['-SLIDER4-']=0
-    # Werte der Schieberegler abrufen
-    slider1_value = values['-SLIDER1-']
-    slider2_value = values['-SLIDER2-']
-    slider3_value = values['-SLIDER3-']
-    slider4_value = values['-SLIDER4-']
+        write()
+    elif event == '-BUTTON6-':
+        print('Button 6 was clicked')
+        window['-SLIDER1-'].update(open_value)
+        values['-SLIDER1-']=open_value
+        window['-SLIDER2-'].update(open_value)
+        values['-SLIDER2-']=open_value
+        window['-SLIDER3-'].update(open_value)
+        values['-SLIDER3-']=open_value
+        window['-SLIDER4-'].update(0)
+        values['-SLIDER4-']=0
+        write()
+    elif event == '-BUTTON7-':
+        print('Button 7 was clicked')
+        window['-SLIDER1-'].update(close_value)
+        values['-SLIDER1-']=close_value
+        window['-SLIDER2-'].update(close_value)
+        values['-SLIDER2-']=close_value
+        window['-SLIDER3-'].update(close_value)
+        values['-SLIDER3-']=close_value
+        window['-SLIDER4-'].update(0)
+        values['-SLIDER4-']=0
+        write()
+    elif event == '-BUTTON8-':
+        print('Button 8 was clicked')
+        value = "50000" 
+        write_read(str(value))
+        while(arduino.readline() != b"" ):
+            print(arduino.readline()) 
+            print(arduino.readline())       
 
-    # Beispiel: Ausgabe der aktuellen Werte der Schieberegler
-    print("Slider 1:", slider1_value)
-    print("Slider 2:", slider2_value)
-    print("Slider 3:", slider3_value)
-    print("Slider 4:", slider4_value)
-    val_1 = "0"
-    if slider4_value < 0:
-        slider4_value = numpy.abs(slider4_value)
-        val_1 = "1"
-    print("1 " + str(int(slider1_value)))
-    print("2 " + str(int(slider2_value)))
-    print( "3 " + str(int(slider3_value)))
-    print("4 " + str(int(slider4_value)))
-    write_read(str("1 "+str(int(slider1_value))))
-    write_read(str("2 "+str(int(slider2_value))))
-    write_read(str("3 "+str(int(slider3_value))))
-    write_read(str("4"+val_1+str(int(slider4_value))))
 
 # GUI schließen
 window.close()