Browse Source

added libraries and Nexus Omni4WD

master
M. Stokroos 4 years ago
parent
commit
63f03cfd86
  1. 31
      README.md
  2. 183
      extra/carcomm/carcomm.py
  3. 64
      extra/carcomm/testcarcom.py
  4. BIN
      extra/factory_10011.zip
  5. BIN
      figures/NexusOmni4WD.png
  6. 7
      libraries/NativeDDS/README.md
  7. 7
      libraries/TrueRMS/README.md
  8. 348
      sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino

31
README.md

@ -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.
![ ](/local/martin/Git.dtpa/Arduino/figures/NexusOmni4WD.png "Nexus Omni 4WD")
*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.

183
extra/carcomm/carcomm.py

@ -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)

64
extra/carcomm/testcarcom.py

@ -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)

BIN
extra/factory_10011.zip

Binary file not shown.

BIN
figures/NexusOmni4WD.png

Binary file not shown.

After

Width:  |  Height:  |  Size: 190 KiB

7
libraries/NativeDDS/README.md

@ -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)

7
libraries/TrueRMS/README.md

@ -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)

348
sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino

@ -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…
Cancel
Save