Skip to content

Instantly share code, notes, and snippets.

@artizirk
Created October 8, 2014 19:17
Show Gist options
  • Save artizirk/39de7eb159e96f373e48 to your computer and use it in GitHub Desktop.
Save artizirk/39de7eb159e96f373e48 to your computer and use it in GitHub Desktop.
int ees_mootor_parem_enable = 12;
int ees_mootor_parem_pwm = 3;
int ees_mootor_vasak_enable = 8;
int ees_mootor_vasak_pwm = 11;
int taga_mootor_parem_enable = 7;
int taga_mootor_parem_pwm = 9;
int taga_mootor_vasak_enable = 5;
int taga_mootor_vasak_pwm = 10;
int kaugus_parem = A2;
int kaugus_vasak = A0;
int kaugus_kesk = A1;
void setup() {
pinMode(ees_mootor_parem_enable, OUTPUT);
pinMode(ees_mootor_vasak_enable, OUTPUT);
pinMode(taga_mootor_parem_enable, OUTPUT);
pinMode(taga_mootor_vasak_enable, OUTPUT);
pinMode(ees_mootor_parem_pwm, OUTPUT);
pinMode(ees_mootor_vasak_pwm, OUTPUT);
pinMode(taga_mootor_parem_pwm, OUTPUT);
pinMode(taga_mootor_vasak_pwm, OUTPUT);
Serial.begin(9600);
}
// the loop routine runs over and over again forever:
void loop() {
// buffer for the data
int buf[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
// wait for first byte
while((buf[0] = Serial.read()) == -1);
// test if it's good, and if not send it back as a error
if (buf[0] != 0xAA) {
Serial.print("ER ");
Serial.println(buf[0], HEX);
return;
}
//wait for second byte
while((buf[1] = Serial.read()) == -1);
// test if it's good, and if not send it back as a error
if (buf[1] != 0xAA) {
Serial.print("ER ");
for (int i = 0; i < 2; i++) {
Serial.print(buf[i], HEX);
Serial.print(":");
}
Serial.println();
return;
}
//wait for third and the final byte of the packet start
while((buf[2] = Serial.read()) == -1);
// test if it's good, and if not send it back as a error
if (!((buf[2] == 1) || (buf[2] == 0))) {
Serial.print("ER ");
for (int i = 0; i < 3; i++) {
Serial.print(buf[i], HEX);
Serial.print(":");
}
Serial.println();
return;
}
// read rest of the packet
for (int i =3; i < 10; i++) {
while((buf[i] = Serial.read()) == -1);
}
// send back a confirm with all the recived data
Serial.print("OK ");
for (int i = 0; i < 10; i++) {
Serial.print(buf[i], HEX);
Serial.print(":");
}
Serial.println();
// configure motors
digitalWrite(ees_mootor_parem_enable, buf[2]);
digitalWrite(ees_mootor_vasak_enable, buf[3]);
digitalWrite(taga_mootor_parem_enable, buf[4]);
digitalWrite(taga_mootor_vasak_enable, buf[5]);
analogWrite(ees_mootor_parem_pwm, buf[6]);
analogWrite(ees_mootor_vasak_pwm, buf[7]);
analogWrite(taga_mootor_parem_pwm, buf[8]);
analogWrite(taga_mootor_vasak_pwm, buf[9]);
}
import serial
import struct
from time import sleep
ser = serial.Serial("/dev/ttyACM0")
"0xAA, 0xAA, 1, 1, 1, 1, 255, 255, 255, 255"
"start, start, dir, dir, dir, dir, speed, speed, speed, speed"
#p = struct.pack("10B", 0xAA, 0xAA, 1,0,0,0, 0,0,0,0)
#p = struct.pack("B", 170)
#print("send:", ":".join("{:02x}".format(c) for c in p))
#ser.write(p)
# v = ser.read(2)
# if v == b'OK':
# print(v, ":".join("{:02x}".format(c) for c in ser.read(10)))
# else:
# print(v)
#exit()
def speed(s, d):
data = [0xAA, 0xAA] + d + [s,s,s,s]
p = struct.pack("10B", *data)
retry=1
while retry:
print("->", ":".join("{:02x}".format(c) for c in p))
ser.write(p)
status_line = ser.readline().decode()
status, data = status_line.split(" ")
data = [int(x, 16) for x in data.split(":")[:-1]]
data = ":".join("{:02x}".format(c) for c in data)
if status.startswith("OK"):
print(status, data)
retry = 0
elif status.startswith("ER"):
print(status, data)
retry -= 1
speed(255, [0,1,1,0])
sleep(2)
speed(0, [0,0,0,0])
#exit()
speed(100, [0,1,1,0])
sleep(0.5)
speed(100, [0,0,0,0])
sleep(1)
speed(100, [1,0,0,1])
sleep(0.5)
speed(100, [1,1,1,1])
sleep(1)
speed(0, [0,0,0,0])
# while True:
# speed(40)
# continue
# for x in range(200):
# speed(x)
# for x in range(200, 0, -1):
# speed(x)
# import socket
# port = 5000
# s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# s.bind(("", port))
# print "waiting on port:", port
# while 1:
# data, addr = s.recvfrom(1024)
# print data
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment