Booster

 Un programma misto Arduino/Python per controllare da un joystick collegato ad un portatile dei servo motori pilotati da Arduino

Parte Arduino Ethernet
--------------------------------------------------------------------
#include <SPI.h>        
#include <Ethernet.h>
#include <Udp.h>       
#include <Servo.h>
#include <string.h>
#include <stdlib.h>


byte mac[] = {0xDE, 0xAD, 0xBF, 0xEF, 0xFE, 0xED };
byte ip[] = {192,168,1,3 };

unsigned int localPort = 8888;      // local port to listen on

byte remoteIp[4];        // holds received packet's originating IP
unsigned int remotePort; // holds received packet's originating port
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet,

Servo m1; //destra
Servo m2; //sinistra
Servo m3; //prua
Servo m4; //poppa

int val;

void setup() {
  Ethernet.begin(mac,ip);
  Udp.begin(localPort);
  m1.attach(3); // collega motore destro
  m2.attach(5); // collega motore sinistro
  m3.attach(6); // collega motore prua;
  m4.attach(9); // collega motore poppa 

  pinMode(7,OUTPUT); // collega Luce 1
  pinMode(8,OUTPUT); // collega Luce 1
  //pinMode(4,OUTPUT); // collega Luce 1
  //pinMode(7,OUTPUT); // collega Luce 1
 
  Serial.begin(9600);
}

void loop() {
  char *p, *i;
  int potenza1, potenza2, potenza3, potenza4;
  int l1, l2, l3, l4;

  int packetSize = Udp.available();

  if(packetSize)
  {
    packetSize = packetSize - 8;    
    Udp.readPacket(packetBuffer,UDP_TX_PACKET_MAX_SIZE, remoteIp, remotePort);
    //Serial.println(packetBuffer);
    p = strtok_r(packetBuffer,":",&i);
    int asse = atoi(p);
    Serial.print(asse);
    Serial.print(" = ");
  
   
    p = strtok_r(NULL,":",&i);
    potenza1 = 0;
    potenza1 = atoi(p);
    Serial.print("M1 = ");
    Serial.println(potenza1);

    p = strtok_r(NULL,":",&i);
    potenza2 = 0;
    potenza2 = atoi(p);
    Serial.print("M2 = ");
    Serial.println(potenza2);

    p = strtok_r(NULL,":",&i);
    potenza3 = 0;
    potenza3 = atoi(p);
    Serial.print("M3 = ");
    Serial.println(potenza3);

    p = strtok_r(NULL,":",&i);
    potenza4 = 0;
    potenza4 = atoi(p);
    Serial.print("M4 = ");
    Serial.println(potenza4);

    //luci
    p = strtok_r(NULL,":",&i);
    l1 = 0;
    l1 = atoi(p);
    Serial.print("L1 = ");
    Serial.println(l1);   
    if (l1 == 1)
      {
        val = digitalRead(7);
        if (val == HIGH)
            {
              digitalWrite(7,LOW);
            }
            else
            {
              digitalWrite(7,HIGH);
            }
           
      }

    p = strtok_r(NULL,":",&i);
    l2 = 0;
    l2 = atoi(p);
    Serial.print("L2 = ");
    Serial.println(l2);
    if (l2 == 1)
      {
        val = digitalRead(7);
        if (val == HIGH)
            {
              digitalWrite(7,LOW);
            }
            else
            {
              digitalWrite(7,HIGH);
            }
           
      }

    p = strtok_r(NULL,":",&i);
    l3 = 0;
    l3 = atoi(p);
    Serial.print("L3 = ");
    Serial.println(l3);
    if (l3 == 1)
      {
        val = digitalRead(8);
        if (val == HIGH)
            {
              digitalWrite(8,LOW);
            }
            else
            {
              digitalWrite(8,HIGH);
            }
           
      }

   
    p = strtok_r(NULL,":",&i);
    l4 = 0;
    l4 = atoi(p);
    Serial.print("L4 = ");
    Serial.println(l4);
    if (l4 == 1)
      {
        val = digitalRead(8);
        if (val == HIGH)
            {
              digitalWrite(8,LOW);
            }
            else
            {
              digitalWrite(8,HIGH);
            }
           
      }

   
    m1.write(potenza1);
    delay(50);
    m2.write(potenza2);
    delay(50);
    m3.write(potenza2);
    delay(50);
    m4.write(potenza4);
    delay(50);


  }
 
  delay(100);
}
--------------------------------------------------------------------

Parte PC
--------------------------------------------------------------------
import pygame
import pygcurse
import socket
from time import sleep

pygame.init()
win = pygcurse.PygcurseWindow(40, 25, 'Rov Controller')

sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)

j = pygame.joystick.Joystick(0)
j.init()

speed = 10

try:
    while True:
        pygame.event.pump()

    l1 = int(j.get_button(4))
    l2 = int(j.get_button(5))
    l3 = int(j.get_button(6))
    l4 = int(j.get_button(7))

    if (int(j.get_button(0)) == 1):
        speed = 10
    if (int(j.get_button(1)) == 1):
        speed = 20
    if (int(j.get_button(2)) == 1):
        speed = 30


    win.write("Mult speed :     ", x=2, y=9)
    win.write("Mult speed : "+str(speed), x=2, y=9)

    win.write("Luce 1 : "+str(l1), x=2,y=10)
    win.write("Luce 2 : "+str(l2), x=2,y=11)
    win.write("Luce 3 : "+str(l3), x=2,y=12)
    win.write("Luce 4 : "+str(l4), x=2,y=13)

    #sinistra/destra
    if (int(j.get_button(3)) == 1):
        m1 = 90-int((j.get_axis(0)*speed))
        m2 = 90+int((j.get_axis(0)*speed))
    else:
    #avanti/indietro
        m1 = 90+int((j.get_axis(2)*speed))
        m2 = 90+int((j.get_axis(2)*speed))

    #alto/basso
    m3 = 90+int((j.get_axis(1)*speed))
    m4 = 90+int((j.get_axis(1)*speed))


    win.write("M 1 :    ", x=2,y=14)
    win.write("M 2 :    ", x=2,y=15)
    win.write("M 3 :    ", x=2,y=16)
    win.write("M 4 :    ", x=2,y=17)
    win.write("M 1 : "+str(m1), x=2,y=14)
    win.write("M 2 : "+str(m2), x=2,y=15)
    win.write("M 3 : "+str(m3), x=2,y=16)
    win.write("M 4 : "+str(m4), x=2,y=17)


    sock.sendto("1:"+str(m1)+":"+str(m2)+":"+str(m3)+":"+str(m4)+":"+str(l1)+":"+str(l2)+":"+str(l3)+":"+str(l4)+":0",("192.168.1.3",8888))


    sleep(0.3)
   
except KeyboardInterrupt:
    j.quit()
------------------------------------------