M. Stokroos
4 years ago
8 changed files with 639 additions and 1 deletions
@ -1,5 +1,34 @@
@@ -1,5 +1,34 @@
|
||||
# Arduino |
||||
|
||||
This repository contains Arduino sketches and Arduino libraries maintained by the RUG/DTPA group. |
||||
[web link to DTPA](https://www.rug.nl/research/discrete-technology-production-automation/) |
||||
|
||||
[web link to DTPA](https://www.rug.nl/research/discrete-technology-production-automation/) |
||||
|
||||
## Sketches |
||||
|
||||
- Blinky |
||||
A hello world example. |
||||
|
||||
- Nexus_Omni4WD |
||||
|
||||
Firmware for the Nexus Omni4WD chassis for setting the wheel speeds independently. It is used to control the robot from ROS. |
||||
|
||||
 |
||||
|
||||
*sketches/Nexus_Omni4WD* |
||||
This folder contains the Arduino sketch of the firmware. The program works as a simple command interpreter for controlling the wheel speeds independently. To compile, use the libraries provided in the *extra/factory_10011.zip* archive. |
||||
|
||||
*extra/carcomm* |
||||
*testcarcom.py* is a small Python application to test the Nexus_Omni4WD firmware (sketch). The python application sends data packages to set the wheel speeds. |
||||
|
||||
The Arduino controller 10009 with io expansion shield on the Omni4WD is Duemilanove/ATMEGA328 compatible. |
||||
|
||||
|
||||
|
||||
## Libraries |
||||
|
||||
- NativeDDS |
||||
Arduino library for a software implemented Direct Digital Synthesizer. |
||||
|
||||
- TrueRMS |
||||
Arduino library that calculates the average, rms and power of DC+AC input signals, measured with the ADC. |
||||
|
||||
@ -0,0 +1,183 @@
@@ -0,0 +1,183 @@
|
||||
#!/usr/bin/env python2 |
||||
|
||||
# nog geen 3 omdat serial er nog niet bij zit. (nog steeds?) |
||||
|
||||
# To control Nexus robot car |
||||
# Via /dev/ttyUSB? |
||||
# |
||||
|
||||
import sys, os, serial, time, traceback |
||||
from struct import pack, unpack |
||||
import numpy as np |
||||
import re |
||||
|
||||
def key_description(character): |
||||
"""generate a readable description for a key""" |
||||
ascii_code = ord(character) |
||||
if ascii_code < 32: |
||||
return 'Ctrl+%c' % (ord('@') + ascii_code) |
||||
else: |
||||
return repr(character) |
||||
|
||||
# voorlopig alleen posix |
||||
if os.name == 'posix': |
||||
import termios, sys, os |
||||
class Console: |
||||
def __init__(self): |
||||
self.fd = sys.stdin.fileno() |
||||
|
||||
def setup(self): |
||||
self.old = termios.tcgetattr(self.fd) |
||||
new = termios.tcgetattr(self.fd) |
||||
# new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG |
||||
new[3] = new[3] & ~termios.ICANON # & ~termios.ISIG |
||||
new[6][termios.VMIN] = 1 |
||||
new[6][termios.VTIME] = 0 |
||||
termios.tcsetattr(self.fd, termios.TCSANOW, new) |
||||
|
||||
# Uiteindelijk willen we meer tegelijk lezen.. |
||||
# zeker als er grotere hoeveelheden data verstuurd worden door het target |
||||
def getkey(self): |
||||
c = os.read(self.fd, 1) |
||||
return c |
||||
|
||||
def cleanup(self): |
||||
termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old) |
||||
|
||||
console = Console() |
||||
|
||||
def cleanup_console(): |
||||
console.cleanup() |
||||
|
||||
console.setup() |
||||
sys.exitfunc = cleanup_console #terminal modes have to be restored on exit... |
||||
|
||||
else: |
||||
raise NotImplementedError( "Sorry no implementation for your platform (%s) available." % sys.platform) |
||||
|
||||
|
||||
CONVERT_CRLF = 2 |
||||
CONVERT_CR = 1 |
||||
CONVERT_LF = 0 |
||||
NEWLINE_CONVERISON_MAP = ('\n', '\r', '\r\n') |
||||
LF_MODES = ('LF', 'CR', 'CR/LF') |
||||
|
||||
REPR_MODES = ('raw', 'some control', 'all control', 'hex') |
||||
|
||||
class CarControl: |
||||
def __init__(self, port, baudrate, parity, rtscts, xonxoff, echo=False, convert_outgoing=CONVERT_CRLF, repr_mode=0): |
||||
self.serial = serial.Serial(port, baudrate, parity=parity, rtscts=rtscts, xonxoff=xonxoff, timeout = 1) |
||||
self.echo = echo |
||||
self.repr_mode = repr_mode |
||||
self.convert_outgoing = convert_outgoing |
||||
self.newline = NEWLINE_CONVERISON_MAP[self.convert_outgoing] |
||||
self.dtr_state = True |
||||
self.rts_state = True |
||||
self.break_state = False |
||||
|
||||
# Het lijkt er op of er na een power up van de Nexus twee characters (waarde 0) gezonden worden. FTDI chip? |
||||
# De writer pas wat laten doen als we daar geen last meer van hebben. |
||||
tmp = self.serial.read(1) |
||||
#print 'pretmp', len(tmp) |
||||
#if len(tmp) > 0: |
||||
# print ord(tmp[0]) |
||||
time.sleep(1) |
||||
tmp = self.serial.read(1) |
||||
#print 'prettmp', len(tmp) |
||||
#if len(tmp) > 0: |
||||
# print ord(tmp[0]) |
||||
|
||||
def stop(self): |
||||
print 'Waar is de stop?' |
||||
self.serial.close() |
||||
|
||||
def getinfopacket(self): |
||||
try: |
||||
d = self.serial.read(24) |
||||
print len(d) #, ord(d[0]) |
||||
if ord(d[0]) <> 2 or ord(d[23]) <> 3: # STX and ETX |
||||
print 'Packet framing error!' |
||||
else: |
||||
print 'New info packet received.' |
||||
if ord(d[1]) == 1: # scope data packet (try that first) |
||||
status_control = ord(d[3]); |
||||
error_code = ord(d[4]); |
||||
status_car = ord(d[5]); |
||||
#(sensor,) = unpack('h', d[10:12]); |
||||
(speed,) = unpack('h', d[6:8]); |
||||
(debtime,) = unpack('h', d[10:12]); |
||||
# we gebruiken dit in de write thread en bij autoreply. |
||||
# evt. korte lock bij uitpakken data |
||||
waitreq = 1 |
||||
else: |
||||
print 'Packet type %x received, not implemented.' % d[1] |
||||
print 'Info packet received with error code %x %x %d and %d' % (error_code, status_car, speed, debtime) |
||||
except: |
||||
print 'Wanneer hier?' |
||||
traceback.print_exc(file=sys.stdout) |
||||
|
||||
|
||||
""" |
||||
Packets are 8 bytes. |
||||
{ stx, command, par's ..., etx } |
||||
parameters dependant upon the command |
||||
|
||||
Nexus robot functions of type void(*function[11])(unsigned int speedMMPS) : |
||||
{goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop, backOff, upperLeft, lowerLeft, upperRight, lowerRight}; |
||||
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } |
||||
this wil go in command. |
||||
Parameter speedMMPS in par |
||||
|
||||
-700 < speedMMPS < 700 |
||||
speedMMPS = 200 komt overeen met 0.21 m/sec |
||||
maximale snelheid 0.735 m/sec |
||||
van m/s naar speedMMPS : maal 952 |
||||
|
||||
|
||||
Other commands: |
||||
12: setWheelspeedcommand (set speed of 4 wheels) |
||||
13: set speedMMPS on Nexus |
||||
20: ask info packet |
||||
a 24 byte packet will be received |
||||
21: autoshutdown off (default) |
||||
22: autoshutdown on |
||||
23: autoreply off (default) |
||||
24: autoreply on. reply with a infopacket |
||||
25: start standalone? |
||||
26: stop standalone |
||||
27: keep alive |
||||
28: repeat mode info packets off (default) |
||||
29: repeat mode info packete on |
||||
|
||||
""" |
||||
|
||||
def sendpacket(self, command, par): |
||||
outpkt = bytearray(16) |
||||
outpkt[0] = 2 # STX |
||||
outpkt[1] = command |
||||
outpkt[2:4] = pack('h', par) |
||||
outpkt[15] = 3 # ETX |
||||
self.serial.write(outpkt) # send packet |
||||
print 'sendpacket', outpkt[0], outpkt[1], outpkt[2], outpkt[3], outpkt[4], outpkt[5], outpkt[6], ' ... ', outpkt[15] |
||||
|
||||
def sendwheelscommand(self, frontleft, backleft, frontright, backright): |
||||
outpkt = bytearray(16) |
||||
outpkt[0] = 2 # STX |
||||
outpkt[1] = 12 # setwheels command |
||||
outpkt[2:4] = pack('h', frontleft) |
||||
outpkt[4:6] = pack('h', backleft) |
||||
outpkt[6:8] = pack('h', frontright) |
||||
outpkt[8:10] = pack('h', backright) |
||||
|
||||
outpkt[15] = 3 # ETX |
||||
self.serial.write(outpkt) # send packet |
||||
#print 'outpkt: ', unpack('h', outpkt[2:4]), unpack('h', outpkt[4:6]), unpack('h', outpkt[6:8]), unpack('h',outpkt[8:10]) |
||||
|
||||
def setspeed(self, lin,rot): |
||||
# lineaire (m/sec) and rotatie (rad/sec) snelheid |
||||
# convert to nexus (calibrated with nexus1) |
||||
speed = lin * 1024 |
||||
rot = rot * 300 |
||||
self.sendwheelscommand(speed -rot, speed - rot, speed + rot , speed + rot) |
||||
|
||||
|
||||
@ -0,0 +1,64 @@
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env python |
||||
|
||||
import sys, time |
||||
import carcomm |
||||
|
||||
if __name__ == "__main__": |
||||
|
||||
try: |
||||
# voorlopig vast |
||||
port = "/dev/ttyUSB0" |
||||
baudrate = 28800 |
||||
parity = 'N' |
||||
rtscts = False |
||||
xonxoff = False |
||||
echo = False |
||||
convert_outgoing = carcomm.CONVERT_CRLF |
||||
repr_mode = 0 |
||||
|
||||
carcon = carcomm.CarControl( |
||||
port, |
||||
baudrate, |
||||
parity, |
||||
rtscts, |
||||
xonxoff, |
||||
echo, |
||||
convert_outgoing, |
||||
repr_mode, |
||||
) |
||||
time.sleep(2) # de nexus robot is niet zo snel.. |
||||
carcon.sendpacket(20,0) |
||||
carcon.getinfopacket() |
||||
carcon.sendpacket(20,22) |
||||
|
||||
|
||||
|
||||
speed = 400 |
||||
for x in range(0, 50): |
||||
# carcon.setspeed(0.5, 0) |
||||
carcon.sendwheelscommand(speed, speed, speed, speed) |
||||
# carcon.getinfopacket() |
||||
time.sleep(0.05) |
||||
carcon.sendwheelscommand(0, 0, 0, 0) |
||||
# carcon.setspeed( 0, 0) |
||||
|
||||
time.sleep(1) # de nexus robot is niet zo snel.. |
||||
|
||||
for x in range(0, 50): |
||||
# carcon.setspeed(0.5, 0) |
||||
carcon.sendwheelscommand(-speed, -speed, -speed, -speed) |
||||
# carcon.getinfopacket() |
||||
time.sleep(0.05) |
||||
carcon.sendwheelscommand(0, 0, 0, 0) |
||||
# carcon.setspeed( 0, 0) |
||||
|
||||
|
||||
|
||||
except carcomm.serial.SerialException as e: |
||||
sys.stderr.write("could not open port %r: %s\n" % (port, e)) |
||||
sys.exit(1) |
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 190 KiB |
@ -0,0 +1,7 @@
@@ -0,0 +1,7 @@
|
||||
# NativeDDS |
||||
|
||||
An Arduino library for a software implemented Direct Digital Synthesizer. |
||||
|
||||
Please follow: |
||||
|
||||
[https://github.com/MartinStokroos/NativeDDS](https://github.com/MartinStokroos/NativeDDS) |
||||
@ -0,0 +1,7 @@
@@ -0,0 +1,7 @@
|
||||
# TrueRMS |
||||
|
||||
An Arduino library that calculates the average, rms and power of DC+AC input signals, measured with the ADC. |
||||
|
||||
Please follow: |
||||
|
||||
[https://github.com/MartinStokroos/TrueRMS](https://github.com/MartinStokroos/TrueRMS) |
||||
@ -0,0 +1,348 @@
@@ -0,0 +1,348 @@
|
||||
/*
|
||||
* Sensors cannot be used together with serial interface! |
||||
* Build for Duemilanove with 328.
|
||||
*
|
||||
* Changes to version 30-10-2018:
|
||||
* - The Sonar code has been completely stripped out because the serial port is not available for the Sonars. |
||||
* - Brackets has been placed in the two code lines for setting the PWM frequency, because it was causing compiler warnings. |
||||
* - The function allStop has been modified according the solution provided by asantiago@bnl.gov |
||||
*/ |
||||
|
||||
template<class T> inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; } |
||||
|
||||
#include <MotorWheel.h> |
||||
#include <Omni4WD.h> |
||||
#include <PID_Beta6.h> |
||||
#include <PinChangeInt.h> |
||||
#include <PinChangeIntConfig.h> |
||||
//#include <SONAR.h>
|
||||
|
||||
/*
|
||||
************************************************************************************ |
||||
Sonar:0x12 |
||||
------------------------------ |
||||
| | |
||||
M3| |M2 |
||||
| | |
||||
| | |
||||
| | |
||||
Sonar:0x13| |Sonar:0x11 |
||||
| | |
||||
| |Power Switch |
||||
| | |
||||
| | |
||||
------------------------------- |
||||
| | |
||||
M4| |M1 |
||||
| | |
||||
------------------------------- |
||||
Sonar:0x14
|
||||
************************************************************************************ |
||||
*/ |
||||
|
||||
|
||||
int speedRampTime = 1000; //speed ramping time in ms.
|
||||
|
||||
irqISR(irq1,isr1); |
||||
MotorWheel wheel1(3,2,4,5,&irq1); |
||||
|
||||
irqISR(irq2,isr2); |
||||
MotorWheel wheel2(11,12,14,15,&irq2); |
||||
|
||||
irqISR(irq3,isr3); |
||||
MotorWheel wheel3(9,8,16,17,&irq3); |
||||
|
||||
irqISR(irq4,isr4); |
||||
MotorWheel wheel4(10,7,18,19,&irq4); |
||||
|
||||
Omni4WD Omni(&wheel1,&wheel2,&wheel3,&wheel4); |
||||
|
||||
|
||||
|
||||
// er zitten delays in deze functies.
|
||||
void goAhead(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_ADVANCE) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarAdvance(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void turnLeft(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_LEFT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarLeft(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void turnRight(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_RIGHT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarRight(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void rotateRight(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_ROTATERIGHT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarRotateRight(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void rotateLeft(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_ROTATELEFT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarRotateLeft(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void allStop(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_STOP) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarStop(); |
||||
//this sets the PWM pins Low to enforce a stop. Solution from asantiago@bnl.gov via email conversation.
|
||||
digitalWrite(3, LOW);digitalWrite(11, LOW); |
||||
digitalWrite(9, LOW);digitalWrite(10, LOW); |
||||
} |
||||
|
||||
void backOff(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_BACKOFF) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarBackoff(0); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void upperLeft(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_UPPERLEFT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarUpperLeft(speedMMPS); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void lowerLeft(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_LOWERLEFT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarLowerLeft(speedMMPS); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void upperRight(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_UPPERRIGHT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarUpperRight(speedMMPS); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
void lowerRight(int speedMMPS){ |
||||
if(Omni.getCarStat()!=Omni4WD::STAT_LOWERRIGHT) Omni.setCarSlow2Stop(speedRampTime); |
||||
Omni.setCarLowerRight(speedMMPS); |
||||
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime); |
||||
} |
||||
|
||||
/*
|
||||
backoff = achteruit |
||||
turnLeft = opzij links |
||||
upperLeft = diagonaal links naar voren |
||||
*/ |
||||
|
||||
void(*function[11])(int speedMMPS) = |
||||
{goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop, |
||||
backOff, upperLeft, lowerLeft, upperRight, lowerRight}; |
||||
|
||||
|
||||
#define NULLCHAR 0 |
||||
#define SPACE 32 |
||||
#define ESC 27 |
||||
#define BACKSPACE 8 |
||||
#define TAB 9 |
||||
#define LF 10 |
||||
#define CR 13 |
||||
|
||||
// Last active time
|
||||
unsigned long int lastactime, currMillis, debtime; |
||||
|
||||
char autoshutdown = 0; |
||||
char autoreply = 0; |
||||
char repeatmode = 0; |
||||
|
||||
char sendpacket[24]; |
||||
char error_code = 0; |
||||
|
||||
int speedMMPS = 0; |
||||
|
||||
void setup() { |
||||
delay(2000);
|
||||
TCCR1B = (TCCR1B & 0xf8) | 0x01; // Pin9,Pin10 PWM 31250Hz
|
||||
TCCR2B = (TCCR2B & 0xf8) | 0x01; // Pin3,Pin11 PWM 31250Hz
|
||||
|
||||
//SONAR::init(13);
|
||||
//Omni.switchMotors();
|
||||
Omni.PIDEnable(0.35,0.02,0,10); |
||||
|
||||
Serial.begin(28800); // opens serial port, sets data rate to 28800 bps.
|
||||
// Faster does not work with all motor running fast.
|
||||
lastactime = currMillis = millis(); |
||||
|
||||
sendpacket[0] = 2; // STX
|
||||
sendpacket[23] = 3;// ETX
|
||||
} |
||||
|
||||
void sendinfo(void) { |
||||
int i; |
||||
|
||||
sendpacket[1] = 1; // info packet
|
||||
sendpacket[2] = 1; // version
|
||||
sendpacket[3] = 1; |
||||
sendpacket[4] = error_code; |
||||
sendpacket[5] = Omni.getCarStat(); |
||||
sendpacket[6] = (speedMMPS ) & 0xff ; |
||||
sendpacket[7] = (speedMMPS >> 8) & 0xff; |
||||
|
||||
Serial.write(sendpacket, 24); |
||||
|
||||
/* getCarStat --> info
|
||||
Serial.print("Info: "); |
||||
Serial.print(" "); |
||||
Serial.print((char)Omni.getCarStat()); |
||||
Serial.print(" "); |
||||
Serial.print(distBuf[0]); |
||||
Serial.print(" "); |
||||
Serial.print(distBuf[0]); |
||||
Serial.print(" "); |
||||
Serial.print(distBuf[0]); |
||||
Serial.print(" "); |
||||
Serial.print(distBuf[0]); |
||||
Serial.println(); |
||||
*/ |
||||
} |
||||
|
||||
|
||||
|
||||
// number of received chars from next packet
|
||||
unsigned char nmbr = 0;
|
||||
unsigned char packet[16]; |
||||
char dosendinfo = 0; |
||||
|
||||
void loop() { |
||||
char c; |
||||
//unsigned char sonarcurrent = 0;
|
||||
short frontleft, backleft, frontright, backright; |
||||
|
||||
// try to complete getting a packet and then process it.
|
||||
if (Serial.available() > 0) { |
||||
c = Serial.read(); |
||||
packet[nmbr++] = c; |
||||
if (nmbr > 15) { // process next packet
|
||||
if (packet[0] != 2 || packet[15] != 3) { |
||||
// set error in info packet
|
||||
error_code = 1; |
||||
//Serial.println("Packet framing error");
|
||||
} |
||||
/* Serial.println((int)packet[0]);
|
||||
Serial.println((int)packet[1]); |
||||
Serial.println((int)packet[2]); |
||||
Serial.println((int)packet[3]); |
||||
Serial.println((int)packet[4]); |
||||
Serial.println((int)packet[5]); |
||||
Serial.println((int)packet[6]); |
||||
Serial.println((int)packet[7]); |
||||
Serial.println(); |
||||
|
||||
Serial.println("Process Command"); |
||||
*/ |
||||
if (packet[1] < 11) { |
||||
speedMMPS = (int)((unsigned int)packet[2]+((unsigned int)packet[3])*256); |
||||
(*function[packet[1]])(speedMMPS); |
||||
//Serial.println(packet[1]);
|
||||
//Serial.println(speedMMPS);
|
||||
} |
||||
else { |
||||
switch (packet[1]) { |
||||
case 12: // set speed of all four motors
|
||||
//debtime = millis();
|
||||
frontleft = (short)((unsigned short)packet[2]+((unsigned short)packet[3])*256); |
||||
backleft = (short)((unsigned short)packet[4]+((unsigned short)packet[5])*256); |
||||
frontright = (short)((unsigned short)packet[6]+((unsigned short)packet[7])*256); |
||||
backright = (short)((unsigned short)packet[8]+((unsigned short)packet[9])*256); |
||||
if (frontleft > 700) frontleft = 700; |
||||
else if (frontleft < -700) frontleft = -700; |
||||
if (frontright > 700) frontright = 700; |
||||
else if (frontright < -700) frontright = -700; |
||||
if (backleft > 700) backleft = 700; |
||||
else if (backleft < -700) backleft = -700; |
||||
if (backright > 700) backright = 700; |
||||
else if (backright < -700) backright = -700; |
||||
|
||||
if ( (frontleft==0) && (backleft==0) ) { |
||||
allStop(0); |
||||
} |
||||
else { |
||||
|
||||
Omni.wheelULSetSpeedMMPS(frontleft); |
||||
Omni.wheelLLSetSpeedMMPS(backleft); |
||||
Omni.wheelLRSetSpeedMMPS(-frontright); |
||||
Omni.wheelURSetSpeedMMPS(-backright); |
||||
} |
||||
|
||||
/*
|
||||
debtime = millis()- debtime;
|
||||
debtime = (short int) debtime; |
||||
sendpacket[10] = (debtime ) & 0xff ; |
||||
sendpacket[11] = (debtime >> 8) & 0xff; |
||||
sendinfo(); |
||||
*/ |
||||
break; |
||||
case 13: // set speedMMPS
|
||||
speedMMPS = (int)((unsigned short)packet[2]+((unsigned short)packet[3])*256); |
||||
if (speedMMPS > 700) speedMMPS = 700; |
||||
else if (speedMMPS < -700) speedMMPS = -700; |
||||
break; |
||||
case 20: // ask info packet
|
||||
dosendinfo = 1; |
||||
break; |
||||
case 21: |
||||
autoshutdown = 0; |
||||
break; |
||||
case 22: |
||||
autoshutdown = 1; |
||||
break; |
||||
case 23: |
||||
autoreply = 0; |
||||
break; |
||||
case 24: |
||||
autoreply = 1; |
||||
break; |
||||
case 25: |
||||
error_code = 2; |
||||
//Serial.println("25 Not implemented");
|
||||
break; |
||||
case 26: |
||||
error_code = 3; |
||||
//Serial.println("Not implemented");
|
||||
break; |
||||
case 27: // keep alive
|
||||
break; |
||||
case 28: |
||||
repeatmode = 0; |
||||
break; |
||||
case 29: |
||||
repeatmode = 1; |
||||
break; |
||||
default: |
||||
error_code = 3; |
||||
//Serial.println("Unrecognized command");
|
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (autoreply || dosendinfo) sendinfo(); |
||||
dosendinfo = 0; |
||||
|
||||
// clear packet buffer
|
||||
nmbr = 0; |
||||
lastactime = millis();
|
||||
} |
||||
} |
||||
|
||||
|
||||
if (autoshutdown) { |
||||
if ((millis() - lastactime) > 3000) { |
||||
//Serial.print("Connection lost?");
|
||||
//Serial.print(millis());
|
||||
//Serial.println();
|
||||
allStop(0); |
||||
lastactime = millis(); |
||||
} |
||||
} |
||||
Omni.PIDRegulate(); |
||||
} |
||||
|
||||
Loading…
Reference in new issue