added libraries and Nexus Omni4WD
This commit is contained in:
M. Stokroos
parent
b03a42ba89
commit
63f03cfd86
31
README.md
31
README.md
|
|
@ -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 @@
|
|||
#!/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 @@
|
|||
#!/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.
Binary file not shown.
|
After Width: | Height: | Size: 190 KiB |
|
|
@ -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 @@
|
|||
# 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 @@
|
|||
/*
|
||||
* 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