Created
October 8, 2014 19:17
-
-
Save artizirk/39de7eb159e96f373e48 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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]); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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