diff --git a/README.md b/README.md index fc10065..a1d5133 100644 --- a/README.md +++ b/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/) \ No newline at end of file + +## 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. diff --git a/extra/carcomm/carcomm.py b/extra/carcomm/carcomm.py new file mode 100644 index 0000000..5f9b918 --- /dev/null +++ b/extra/carcomm/carcomm.py @@ -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) + + diff --git a/extra/carcomm/testcarcom.py b/extra/carcomm/testcarcom.py new file mode 100644 index 0000000..b3bb229 --- /dev/null +++ b/extra/carcomm/testcarcom.py @@ -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) + + + + + diff --git a/extra/factory_10011.zip b/extra/factory_10011.zip new file mode 100755 index 0000000..a1293b7 Binary files /dev/null and b/extra/factory_10011.zip differ diff --git a/figures/NexusOmni4WD.png b/figures/NexusOmni4WD.png new file mode 100644 index 0000000..e7ff619 Binary files /dev/null and b/figures/NexusOmni4WD.png differ diff --git a/libraries/NativeDDS/README.md b/libraries/NativeDDS/README.md new file mode 100644 index 0000000..8f2f2e0 --- /dev/null +++ b/libraries/NativeDDS/README.md @@ -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) diff --git a/libraries/TrueRMS/README.md b/libraries/TrueRMS/README.md new file mode 100644 index 0000000..7f06e94 --- /dev/null +++ b/libraries/TrueRMS/README.md @@ -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) diff --git a/sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino b/sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino new file mode 100644 index 0000000..17c3798 --- /dev/null +++ b/sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino @@ -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 inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; } + +#include +#include +#include +#include +#include +//#include + +/* +************************************************************************************ + 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(); +} +