NXT plus Arduino als Motor- / Sensor-Multiplexer

Modelle zum Nachbauen oder wo gibt es etwas interessantes oder Projekte?

Moderator: Moderatoren

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 25. Jun 2014 16:47

teilw. wiederhergestellt aus Notfall-Backup des früheren Original-Topics:
NXT plus Arduino
(Original Topic verschwunden) :(


NXT plus Arduino als Motor- / Sensor-Multiplexer


Hier ein Foto wie es am Schluss mal aussehen kann (zusätzlich noch 16 Touch-Sensoren am Arduino möglich, nicht mit abegebildet):
NXT+Arduino Sensor+Motor-Multiplexer.jpg


Jetzt kommen aber erst einmal die Vorarbeiten:

Grundlagen:
Auslesen der Encoderwerte eines NXT-Encodermotors mit einem Arduino AVR (Uno oder Mega)
und Ansteuerung per pwm mit einem L293D Digital H-Bridge-Treiber:



LegoEncoder2+3_Steckplatine.jpg



Code: Alles auswählen

#define  encoderA   2
#define  encoderB   3

pinMode(encoderA, INPUT_PULLUP);
pinMode(encoderB, INPUT_PULLUP);



Variante 1:
Auslesen der Encoder per Arduino Uno / Mega :


Code: Alles auswählen

/************************************************************
*
* Demo-Programm zur Auswertung eines händisch betriebenen
* Drehencoders (Quadraturencoder) mit dem Arduino im
* Timer-Interrupt mit einer Abfragefrequenz von rd. 1kHz
*
* Kann von jederman frei verwendet werden, aber bitte den
* Hinweis: "Entnommen aus http://www.meinDUINO.de" einfügen
*
************************************************************/

// An die Pins 2 und 3 ist der Encoder angeschlossen
#define encoderA 2
#define encoderB 3

// Globale Variablen zur Auswertung in der
// Interrupt-Service-Routine (ISR)
volatile int8_t altAB = 0;
volatile int encoderWert = 0;

// Die beiden Schritt-Tabellen für volle oder 1/4-Auflösung
// 1/1 Auflösung
//int8_t schrittTab[16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};

// 1/2 Auflösung ergibt bei Lego-Motoren 1 tick pro Grad (standard wie bei Lego)
int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};


// 1/4 Auflösung
//int8_t schrittTab[16] = {0,0,0,0,0,0,0,-1,0,0,0,0,0,1,0,0};


/*************************************************************
*
* Interrupt Service Routine
*
* Wird aufgerufen, wenn der entsprechende Interrupt
* ausgelöst wird
*
*************************************************************/
ISR(TIMER1_COMPA_vect) {
  altAB <<= 2;
  altAB &= B00001100;
  altAB |= (digitalRead(encoderA) << 1) | digitalRead(encoderB);
  encoderWert += schrittTab[altAB];
}


/*************************************************************
*
* void setup()
*
* Wird einmal beim Programmstart ausgeführt
*
*************************************************************/
void setup() {
  pinMode(encoderA, INPUT);
  pinMode(encoderB, INPUT);
 
  noInterrupts(); // Jetzt keine Interrupts
  TIMSK1 |= (1<<OCIE1A);  // Timer 1 Output Compare A Match Interrupt Enable

  TCCR1A = 0; // "Normaler" Modus

  // WGM12: CTC-Modus einschalten (Clear Timer on Compare match)
  //        Stimmen OCR1A und Timer überein, wird der Interrupt
  //        ausgelöst
  // Bit CS12 und CS10 setzen = Vorteiler: 1024
  TCCR1B = (1<<WGM12) | (1<<CS12) | (1<<CS10);

  // Frequenz = 16000000 / 1024 / 15 = rd. 1kHz Abtastfrequenz;
  // Überlauf bei 14, weil die Zählung bei 0 beginnt
  OCR1A = 14;
 
  interrupts(); // Interrupts wieder erlauben

  Serial.begin(115200);
}


/*************************************************************
*
* void loop()
*
* Wird immer wieder durchlaufen
*
*************************************************************/
void loop() {
 
  while(true) {
    Serial.println(encoderWert);
    delay(100);
  }
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



Variante 2:
Quadratur-Encoder auslesen mit Arduino Due (per Due-Timer):


Code: Alles auswählen

/************************************************************
* Programm zur Auswertung eines manuell betriebenen
* Drehencoders (Quadraturencoder) mit dem Arduino Due
* per Due-Timer mit einer Abfragefrequenz von rd. 4-10kHz
* Entlehnt an http ://www.meinDUINO.de 
************************************************************/
#include <DueTimer.h>

char sbuf[100];

#define MAXMOTORS           6 // max number of encoder motors at Arduino Uno=2 // Due=6 // Mega=8



// motor 0
#define pinenc0A   22  // enc0A yellow
#define pinenc0B   23  // enc0B blue
#define pinmot0d1  24  // dir0-1   <<
#define pinmot0d2  25  // dir0-2
#define pinmot0pwm 10  // pwm enable0   

// motor 1
#define pinenc1A   26  // enc1A yellow
#define pinenc1B   27  // enc1B blue
#define pinmot1d1  28  // dir1-1   <<
#define pinmot1d2  29  // dir1-2
#define pinmot1pwm  9  // pwm enable1   


// motor 2
#define pinenc2A   30  // enc2A yellow
#define pinenc2B   31  // enc2B blue
#define pinmot2d1  32  // dir2-1   <<
#define pinmot2d2  33  // dir2-2
#define pinmot2pwm  8  // pwm enable2   

// motor 3
#define pinenc3A   34  // enc3A yellow
#define pinenc3B   35  // enc3B blue
#define pinmot3d1  36  // dir3-1   <<
#define pinmot3d2  37  // dir3-2
#define pinmot3pwm  7  // pwm enable3   

// motor 4
#define pinenc4A   38  // enc4A yellow
#define pinenc4B   39  // enc4B blue
#define pinmot4d1  40  // dir4-1   <<
#define pinmot4d2  41  // dir4-2
#define pinmot4pwm  6  // pwm enable4   

// motor 5
#define pinenc5A   42  // enc5A yellow
#define pinenc5B   43  // enc5B blue
#define pinmot5d1  47  // dir5-1   <<
#define pinmot5d2  48  // dir5-2
#define pinmot5pwm  5  // pwm enable5   





volatile long   motenc[MAXMOTORS]    = {0, 0, 0, 0, 0, 0},
                oldenc[MAXMOTORS]    = {0, 0, 0, 0, 0, 0};
               
byte pinmotdir[MAXMOTORS][ 2] = {
  {pinmot0d1, pinmot0d2},   // motor direction pin array
  {pinmot1d1, pinmot1d2},
  {pinmot2d1, pinmot2d2},
  {pinmot3d1, pinmot3d2},
  {pinmot4d1, pinmot4d2},
  {pinmot5d1, pinmot5d2},
};

int  pinmotpwm[MAXMOTORS] =      {pinmot0pwm, pinmot1pwm, pinmot2pwm,  // motor pwm pin array
                                  pinmot3pwm, pinmot4pwm, pinmot5pwm,
                                 };

volatile int8_t ISRab[MAXMOTORS]     = {0, 0, 0, 0, 0, 0};

// 1/1 Auflösung
//int8_t schrittTab[16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};

// 1/2 Auflösung ergibt bei Lego-Motoren 1 tick pro Grad (standard wie bei Lego)
int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};

// 1/4 Auflösung
//int8_t schrittTab[16] = {0,0,0,0,0,0,0,-1,0,0,0,0,0,1,0,0};


                               
                                 
/*************************************************************
* Interrupt Handler Routine
*************************************************************/

void encHandler() {
 
  ISRab [ 0] <<= 2;
  ISRab [ 0] &= B00001100;
  ISRab [ 0] |= (digitalRead(pinenc0A) << 1) | digitalRead(pinenc0B);
  motenc[ 0] += schrittTab[ISRab[0]];           //

  ISRab [ 1] <<= 2;
  ISRab [ 1] &= B00001100;
  ISRab [ 1] |= (digitalRead(pinenc1A) << 1) | digitalRead(pinenc1B);
  motenc[ 1] += schrittTab[ISRab[1]];           //

  ISRab [ 2] <<= 2;
  ISRab [ 2] &= B00001100;
  ISRab [ 2] |= (digitalRead(pinenc2A) << 1) | digitalRead(pinenc2B);
  motenc[ 2] += schrittTab[ISRab[2]];           //

  ISRab [ 3] <<= 2;
  ISRab [ 3] &= B00001100;
  ISRab [ 3] |= (digitalRead(pinenc3A) << 1) | digitalRead(pinenc3B);
  motenc[ 3] += schrittTab[ISRab[3]];           //

  ISRab [ 4] <<= 2;
  ISRab [ 4] &= B00001100;
  ISRab [ 4] |= (digitalRead(pinenc4A) << 1) | digitalRead(pinenc4B);
  motenc[ 4] += schrittTab[ISRab[4]];           //

  ISRab [ 5] <<= 2;
  ISRab [ 5] &= B00001100;
  ISRab [ 5] |= (digitalRead(pinenc5A) << 1) | digitalRead(pinenc5B);
  motenc[ 5] += schrittTab[ISRab[5]];           //

 
}


void setup() {
  // motor pin settings
  // setup for L293D motor driver

     // motor 0
     pinMode(pinenc0A, INPUT_PULLUP);   // enc0A    yellow
     pinMode(pinenc0B, INPUT_PULLUP);   // enc0B    blue
     pinMode(pinmot0d1, OUTPUT);        // dir0-1   
     pinMode(pinmot0d2, OUTPUT);        // dir0-2   
     pinMode(pinmot0pwm ,OUTPUT);       // enable0 
       
     // motor 1
     pinMode(pinenc1A, INPUT_PULLUP);   // enc1A    yellow
     pinMode(pinenc1B, INPUT_PULLUP);   // enc1B    blue
     pinMode(pinmot1d1, OUTPUT);        // dir1-1   
     pinMode(pinmot1d2, OUTPUT);        // dir1-2 
     pinMode(pinmot1pwm, OUTPUT);       // enable1
       
     // motor 2
     pinMode(pinenc2A, INPUT_PULLUP);   // enc2A    yellow
     pinMode(pinenc2B, INPUT_PULLUP);   // enc2B    blue
     pinMode(pinmot2d1, OUTPUT);        // dir2-1 
     pinMode(pinmot2d2, OUTPUT);        // dir2-2   
     pinMode(pinmot2pwm, OUTPUT);       // enable2 
       
     // motor 3
     pinMode(pinenc3A, INPUT_PULLUP);   // enc3A     yellow
     pinMode(pinenc3B, INPUT_PULLUP);   // enc3B     blue
     pinMode(pinmot3d1, OUTPUT);        // dir3-1   
     pinMode(pinmot3d2, OUTPUT);        // dir3-2 
     pinMode(pinmot3pwm, OUTPUT);       // enable3 
       
     // motor 4
     pinMode(pinenc4A, INPUT_PULLUP);   // enc4A     yellow
     pinMode(pinenc4B, INPUT_PULLUP);   // enc4B     blue
     pinMode(pinmot4d1, OUTPUT);        // dir4-1   
     pinMode(pinmot4d2, OUTPUT);        // dir4-2 
     pinMode(pinmot4pwm, OUTPUT);       // enable4 
       
     // motor 5
     pinMode(pinenc5A, INPUT_PULLUP);   // encA5     yellow
     pinMode(pinenc5B, INPUT_PULLUP);   // encB5     blue
     pinMode(pinmot5d1, OUTPUT);        // dir5-1   
     pinMode(pinmot5d2, OUTPUT);        // dir5-2   
     pinMode(pinmot5pwm, OUTPUT);       // enable5
       

 
   Timer1.attachInterrupt(encHandler);
   Timer1.start(100); // Calls every ...µs

   Serial.begin(115200);
   Serial.println( "safety delay before start");
   delay(1000);  // safety delay before start
   Serial.println();
}


void loop() {
 
  while(true) {
     sprintf(sbuf, " 0=%6d, 1=%6d, 2=%6d, 3=%6d, 4=%6d, 5=%6d",
             motenc[ 0], motenc[ 1], motenc[ 2], motenc[ 3], motenc[ 4], motenc[ 5]);
     Serial.println(sbuf);
     delay(100);
  }
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


Ansteuern von Lego-DC-Motoren per pwm:


Mit dem Arduino Mega oder Due kann man 6 pwm-Encoder-Motore ansteuern (je 3 digWrite + 2 digRead => 6*5 = 30 dig. Pins) und hätte dann sogar noch weitere pins für Sensoreingänge zur Verfügung!

NXT-Motoren werden dann über einen Motortreiber so angeschlossen (weiße und schwarze Litze an grün/gelb):
Bild
http://www.arduino-tutorial.de/2010/06/ ... bridge-ic/

Code: Alles auswählen

int motor1_A=11;
int motor1_B=10;
int motor1_Speed=9;
 
int motor2_A=5;
int motor2_B=4;
int motor2_Speed=3;
 
void setup(){
  pinMode(motor1_A,OUTPUT);
  pinMode(motor1_B,OUTPUT);
 
  pinMode(motor2_A,OUTPUT);
  pinMode(motor2_B,OUTPUT);
}
 
void loop(){
  // motor1
  for (int i=0; i>256; i+=5){
    digitalWrite(motor1_A,HIGH); // A = HIGH and B = LOW means the motor will turn right
    digitalWrite(motor1_B,LOW);
    analogWrite(motor1_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  for (int i=255; i>0; i-=5){
    digitalWrite(motor1_A,HIGH); // A = HIGH and B = LOW means the motor will turn right
    digitalWrite(motor1_B,LOW);
    analogWrite(motor1_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  // motor2
  for (int i=0; i<256; i+=5){
    digitalWrite(motor2_A,HIGH); // A = HIGH and B = LOW means the motor will turn right
    digitalWrite(motor2_B,LOW);
    analogWrite(motor2_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  for (int i=255; i>0; i-=5){
    digitalWrite(motor2_A,HIGH); // A = HIGH and B = LOW means the motor will turn right
    digitalWrite(motor2_B,LOW);
    analogWrite(motor2_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
 
  // turn vice versa
 
  // motor1
  for (int i=0; i<256; i+=5){
    digitalWrite(motor1_A,LOW); // A = LOW and B = HIGH means the motor will turn left
    digitalWrite(motor1_B,HIGH);
    analogWrite(motor1_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  for (int i=255; i>0; i-=5){
    digitalWrite(motor1_A,LOW); // A = LOW and B = HIGH means the motor will turn left
    digitalWrite(motor1_B,HIGH);
    analogWrite(motor1_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  // motor2
  for (int i=0; i<256; i+=5){
    digitalWrite(motor2_A,LOW); // A = LOW and B = HIGH means the motor will turn left
    digitalWrite(motor2_B,HIGH);
    analogWrite(motor2_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
  for (int i=255; i>0; i-=5){
    digitalWrite(motor2_A,LOW); // A = LOW and B = HIGH means the motor will turn left
    digitalWrite(motor2_B,HIGH);
    analogWrite(motor2_Speed,i); // speed counts from 0 to 255
    delay(20);
  }
}


Die Dokus zu den L293D sind ziemlich besch***** durcheinander im Web, jeder bezeichnet sie anders. Hier mal ganz super-ausführlich auch für komplette Neulinge:
enable1: pwm Signal Motor1
in1, in2: dig Richtungs-Pins für Motor1
out1, out2: Ausgänge für Motor1

enable2: pwm Signal Motor2
in3, in4: dig Richtungs-Pins für Motor2
out3, out4: Ausgänge für Motor2

Die Motortreiber-Steuer-ICs (H-Brücken) werden meist mit 3-5V mit nur sehr kleinen Steuer-Strömen angesteuert, während die Motoren über die Treiberendstufen der Motortreiber dann von einer externen Spannungsquelle mit deutlich höheren Strömen (je nach H-Brücke ca. 1A bis 40A oder mehr) versorgt werden (ähnlich wie bei einem Relais, bei dem man mit niedrigen Spannungen bei kleinen Steuerströmen deutlich höhere externe Leistungen schalten kann).

IC-Spannungversorgung (Vcc): 5V vom Arduino ("Vss" in einem der Bilder ist eigentlich die falsche Bezeichnung)
"Power-Stromquelle" ("Borne"): 9-12V für die Motoren von externer Batterie ("VS" in einem der Bilder)
GND-Arduino mit GND-(-)-Batterie verbinden;
im L293D sind alle 4 GND Leitungen bereits intern verbunden, es reicht auf dem Steckbrett also 1 einziges GND-Verbindungskabel

L293D.jpg

Quelle: http://www.daedalus.ei.tum.de/index.php ... 1/hardware

Bild
Quelle: http://www.google.de/imgres?imgurl=http ... 56&bih=816



Bild

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 11. Apr 2015 15:31

NXT plus Arduino als Sensor- und Motor-Multiplexer

Elektrische Verbindung des NXTs mit dem Arduino:

Code: Alles auswählen


Arduino      ________________ NXT ________________
  GND ------ GND(pin3)
  SCL ------ SCL(pin5)  --- R_47k --- NXT_pin4(+)
  SDA ------ SDA(pin6)  --- R_47k --- NXT_pin4(+)

statt 47k Pullups funktionieren auch erfahrungsgemäß alle anderen Pullup-Werte im Bereich 33k...82k.


Nun kommt der eigentliche Multiplexer-Teil:

Der Arduino steuert Motoren und Sensoren von alleine (an, aus, Richtung, inkl. PID-Steuerung für Drehung um bestimmte Gradzahlen),
der NXT schickt aber an den Arduino per I2C gezielt Motor-Steuer-Befehle und holt vom Arduino die dort gemessenen Sensorwerte per I2C ab.
Es werden daher laufend I2C-Nachrichten hin- und her geschickt, die Zusammenarbeit muss daher auch sehr gut getimed sein, und Nachrichten müssen auf ihre Vollständigkeit und Richtigkeit getestet werden. Dies passiert per Checksum und über Verifizierung der Sender- und Empfänger-IDs, die in dem Message-String immer mitgeschickt werden.
Hier ein Foto wie es am Schluss mal aussehen kann (zusätzlich noch 16 Touch-Sensoren am Arduino möglich, nicht mit abegebildet):

NXT+Arduino Sensor+Motor-Multiplexer.jpg


wichtig für die Verwendung des Mega mit zusammen mit dem NXT:
// der Mega ist in der Standardkonfiguration nicht i2c-Kompatibel zum NXT wegen eingebauter pullups.
// Man kann die aber softwaremäßig soweit disablen, dass es trotzdem wieder funktioniert wie beim Uno.
// Dazu die Datei twi.c im Arduino-Programmordner suchen und die beiden unteren Zeilen auskommentieren
// wie es hier steht, falls nicht schon geschehen:

Code: Alles auswählen

// file: twi.c  ( path: Arduino\hardware\arduino\avr\libraries\Wire\utility )         
           //  activate internal pullups for twi.
           //  digitalWrite(SDA, 1);
           //  digitalWrite(SCL, 1);




Code für NXT als I2C-Master:

Code: Alles auswählen

//***********************************************************************************
// I²C Muxer controller feat. PID controller
// NXT i2c master
// ver 1.122 NXT
// remote keyboard + syntheziser
// remote motor on | off=coast | brake (variable)
// pre-alpha: remote motor PID control feat. INT24 enc data type
// double validity check for safe i2c data transmission
//************************************************************************************
//
// freie Verwendung für private Zwecke
// für kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// programming language: NXC (John Hansen's enhanced firmware), BCC 3.3.8.11
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/


//------------------------------------------------------------------------------

/*
remote motor commands:
//======================

void remoteOn(byte MOTnr, int pwm)

void remoteCoast(byte MOTnr)

void remoteBrake(byte MOTnr, int pwm)

void remoteRotatePIDdegrees(byte MOTnr, long degrees, int pwm)    // "long"=INT_24

void remoteRotatePIDtoTarget(byte MOTnr, long degrees, int pwm)   // "long"=INT_24

void remoteRotatePIDholdTarget(byte MOTnr, long degrees, int pwm) // "long"=INT_24

void remoteStopPIDcontrol(byte MOTnr)


*/



// NXC - C/Sketch compatibility

#define cls           ClearScreen
#define memset(a,v,c) ArrayInit(a,v,c)
#define delay         Wait
#define random        Random
#define timeMS        CurrentTick // elapsed time since NXT boot
#define rtimeMS       FirstTick   // elapsed time since program start


//------------------------------------------------------------------------------
// stdio.h faking

#define printf1( _x, _y, _format1, _value1) {  \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); }

#define printf1i( _x, _y, _format1, _value1) {  \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1,DRAW_OPT_INVERT); }

#define printf2( _x, _y, _format1, _format2, _value1, _value2) { \
  string sval1 = FormatVal(_format1, _value1); \
  string sval2 = FormatVal(_format2, _value2); \
  string s =sval1+sval2; \
  TextOut(_x, _y, s); \
}

#define printf3( _x, _y, _format1, _format2, _format3, _value1, _value2, _value3) { \
  string sval1 = FormatVal(_format1, _value1); \
  string sval2 = FormatVal(_format2, _value2); \
  string sval3 = FormatVal(_format3, _value3); \
  string s =sval1+sval2+sval3; \
  TextOut(_x, _y, s); \
}

#define printf4( _x, _y, _format1,_format2,_format3,_format4,_value1,_value2,_value3,_value4) { \
  string sval1 = FormatVal(_format1, _value1); \
  string sval2 = FormatVal(_format2, _value2); \
  string sval3 = FormatVal(_format3, _value3); \
  string sval4 = FormatVal(_format4, _value4); \
  string s =sval1+sval2+sval3+sval4; \
  TextOut(_x, _y, s); \
}


//------------------------------------------------------------------------------
// Arduino slave I/Os

#define MAXMOTORS           6 // maximum number of encoder motors supported by Arduino Uno == 2 // Mega == 8
#define MAXANALOG           8 // maximum number of analog sensors supported by Arduino Uno == 4  // Mega == 8
#define MAXDIGPINS         16 // maximum number of digital pins supported by Mega (for Uno just 1 pin (12) left)

#define VOID              127 // void pin address or number


int   Aanalog[MAXANALOG];     // Arduino remote A0-A7 ADC integer  (Uno: analog 4+5 => i2c)
int   Amotenc[MAXMOTORS];     // Arduino remote motor encoders
int   Amotstate[MAXMOTORS];   // Arduino remote motor runstates

byte  Adigital[MAXDIGPINS];   // Arduino digital pins (touch+pwm); (Uno: 0=TX 1=RX 13=LED)

mutex i2cmutex;

//************************************************************************************
// global variables for I²C master-slave communication
//************************************************************************************

#define I2CMSGSIZE   14  // size of I2C messages (byte arrays to send + receive)

#define ARDSLVADDR_0        4  // don't forget to shift << 1     (7-bit Arduino I2C address to 8 bit number)
#define ARDSLVPORT_0       S4

#define MASTER_ID         100  // master ID
#define ARDS_0_ID           4  // Arduino slave_0 ID
#define ARDS_1_ID         201  // Arduino slave_1 ID
#define ARDS_2_ID         202  // Arduino slave_2 ID


#define OUT_REGSTATE_NUL            0
#define OUT_REGSTATE_COAST          2
#define OUT_REGSTATE_BRAKE          3
#define OUT_REGSTATE_EMERG_STOP     5
#define OUT_REGSTATE_ON             8

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_PIDACTIVE     16
#define OUT_REGSTATE_PIDRELATIVE   17
#define OUT_REGSTATE_PIDABSOLUTE   18
#define OUT_REGSTATE_PIDHOLD       19
#define OUT_REGSTATE_PIDHOLDCLOSE  20


// i2c message arrays
byte i2cmastermsg[I2CMSGSIZE],  // i2c master message (command)
                           // 0+1:   CHKSUM this Msg (INT_16)
                           // 2:     Sender ID byte
                           // 3:     MOTORSLOT: MotNr
                           // 4:     mot_runstate
                           // 5:     pwm  (-100...+100)
                           // 6+7:   mot enc_int16
                           // 8:     +mot enc_int24 byte
                           // 9:     (ANASLOT, n/u)
                           // 10+11: buffer (n/u)
                           // 12+13: DIGSLOT: write digital pins 12(0-7) + 13(8-15)

     i2cslavemsg[I2CMSGSIZE];  //i2c  slave message (reply)
                           // 0+1:   chksum this Msg (INT_16)
                           // 2:     Sender ID byte
                           // 3:     MOTORSLOT: MotNr
                           // 4:     mot_runstate
                           // 5:     pwm
                           // 6+7:   mot enc_int16
                           // 8:     buffer (encoder INT_24 / keyboard char)
                           // 9:     ANASLOT: nr
                           // 10+11: analog value INT_16
                           // 12+13: DIGSLOT: read digital pins 12(0-7) + 13(8-15)
                           
                           


#define CHKSSLOT      0  // chksum this Msg (INT_16)
#define TESTSLOT      2  // ID byte
#define MOTORSLOT     3  // motors: 3:motor_nr, 4:reg_state 5+6:encoder_INT_16 7+8:2*buffer
#define KEYBSLOT      8  // 8: optional keyboard key pressed
#define ANASLOT       9  // 9,10+11: analog Data:  pin_nr + sensor_INT_16
#define DIGSLOT      12  // 12,13: digital Data: bitpattern


// check sums for i2c messages
int  __chksum_M__, __chksum_S__, __chksum_R__ ;


// buffer for motor commands
byte _motorarray_[6];      // 0: MOTORSLOT: MotNr
                           // 1: mot_runstate
                           // 2: pwm
                           // 3+4: mot enc_int16
                           // 5: buffer (encoder INT_24?)

byte __keybchar__;           // Arduino keyboard key i2cslavemsg[KEYBSLOT]







//------------------------------------------------------------------------------

void synthikeyb_gr(byte k) {
  int tone;


  if(k== '<' ) tone =   TONE_B3  ;
  else
  if(k== 'y' ) tone =   TONE_C4  ;
  else
  if(k== 's' ) tone =   TONE_CS4 ;
  else
  if(k== 'x' ) tone =   TONE_D4  ;
  else
  if(k== 'd' ) tone =   TONE_DS4 ;
  else
  if(k== 'c' ) tone =   TONE_E4  ;
  else
  if(k== 'v' ) tone =   TONE_F4  ;
  else
  if(k== 'g' ) tone =   TONE_FS4 ;
  else
  if(k== 'b' ) tone =   TONE_G4  ;
  else
  if(k== 'h' ) tone =   TONE_GS4 ;
  else
  if(k== 'n' ) tone =   TONE_A4  ;
  else
  if(k== 'j' ) tone =   TONE_AS4 ;
  else
  if(k== 'm' ) tone =   TONE_B4  ;
  else
  if(k== ',' ) tone =   TONE_C5  ;
  else
  if(k== 'l' ) tone =   TONE_CS5 ;
  else
  if(k== '.' ) tone =   TONE_D5  ;
  else
  if(k== 182 ) tone =   TONE_DS5 ; // ö
  else
  if(k== '-' ) tone =   TONE_E5  ;


  else

  if(k== '^' ) tone =   TONE_AS4 ;
  else
  if(k==   9 ) tone =   TONE_B4  ;  // TAB
  else
  if(k== 'q' ) tone =   TONE_C5  ;
  else
  if(k== '2' ) tone =   TONE_CS5 ;
  else
  if(k== 'w' ) tone =   TONE_D5  ;
  else
  if(k== '3' ) tone =   TONE_DS5 ;
  else
  if(k== 'e' ) tone =   TONE_E5  ;
  else
  if(k== 'r' ) tone =   TONE_F5  ;
  else
  if(k== '5' ) tone =   TONE_FS5 ;
  else
  if(k== 't' ) tone =   TONE_G5  ;
  else
  if(k== '6' ) tone =   TONE_GS5 ;
  else
  if(k== 'z' ) tone =   TONE_A5  ;
  else
  if(k== '7' ) tone =   TONE_AS5 ;
  else
  if(k== 'u' ) tone =   TONE_B5  ;
  else
  if(k== 'i' ) tone =   TONE_C6  ;
  else
  if(k== '9' ) tone =   TONE_CS6 ;
  else
  if(k== 'o' ) tone =   TONE_D6  ;
  else
  if(k== '0' ) tone =   TONE_DS6 ;
  else
  if(k== 'p' ) tone =   TONE_E6  ;
  else
  if(k== 188 ) tone =   TONE_F6  ;   // ü
  else
  if(k==  39 ) tone =   TONE_FS6 ;   // '
  else
  if(k== '+' ) tone =   TONE_G6  ;
  else                               // <-
  if(k==   8 ) tone =   TONE_GS6 ;
  else
  if(k==  13 ) tone =   TONE_A6  ;   // ENTER

  else tone = 55;  // TONE_A1

  /*
  else
  if(k== '' ) tone =   TONE_AS6  ;
  else
  if(k== '' ) tone =   TONE_B6   ;
  else
  if(k== '' ) tone =   TONE_C7   ;
  else
  if(k== '' ) tone =   TONE_CS7  ;
  else
  if(k== '' ) tone =   TONE_D7   ;
  else
  if(k== '' ) tone =   TONE_DS7  ;
  else
  if(k== '' ) tone =   TONE_E7   ;
  else
  if(k== '' ) tone =   TONE_F7   ;
  else
  if(k== '' ) tone =   TONE_FS7  ;
  else
  if(k== '' ) tone =   TONE_G7   ;
  else
  if(k== '' ) tone =   TONE_GS7  ;
  else
  if(k== '' ) tone =   TONE_A7   ;
  else
  if(k== '' ) tone =   TONE_AS7  ;
  else
  if(k== '' ) tone =   TONE_B7   ;

  */

  if (tone==55) PlayTone(tone, 10);
  else          PlayTone(tone,125);

}




//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

inline byte lowbyte(int INT24)  {return (INT24 & 0xFF); }
inline byte highbyte(int INT24) {return ((INT24 >> 8) & 0xFF); }
inline byte high24byte(int INT24) {return ((INT24 >> 16) & 0xFF); }



//------------------------------------------------------------------------------

inline int ByteArrayToInt16(byte array[], byte slot) {
   return ((array[slot+1]<<8) + (array[slot]));
}

//------------------------------------------------------------------------------

inline void Int16ToByteArray(int intval, byte &array[], byte slot) {

   array[slot]   =(intval & 0xFF);  // loByte
   array[slot+1] =(intval >>8);     // hiByte
}


//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
inline byte bitRead(byte source, byte bitnr) {
  return (source & (1 << bitnr) ) != 0;          // read bitval at pos bitnr
}

#define bitSet (source, bit) ( (source) |= (1UL << (bit)) )

#define bitClear(source, bit) ( (source) &= ~(1UL << (bit)) )

#define bitWrite(source, bit, bitvalue) ( bitvalue ? bitSet(source, bit) : bitClear(source, bit) )



//------------------------------------------------------------------------------

byte GetdigpinArduino(byte digpin){
    if( (digpin>=0)&&(digpin<8) )
       return (bitRead (i2cslavemsg[DIGSLOT], digpin) !=0);
    if( (digpin>=8)&&(digpin<14) )
       return (bitRead (i2cslavemsg[DIGSLOT+1], digpin-8)!=0);
    else return 0;

}




//------------------------------------------------------------------------------

inline void writeMotCmdToI2Cmsg() {
                           // motorarray 0 <-> i2cmsg 3: MOTORSLOT: MotNr
                           // motorarray 1 <-> i2cmsg 4: mot_runstate
                           // motorarray 2 <-> i2cmsg 5: pwm
                           // motorarray 3+4 <-> i2cmsg 6+7: mot enc_int16
                           // motorarray 5 <-> i2cmsg 8: buffer (encoder INT_24?)

   for (char i=0; i<6; ++i) i2cmastermsg[MOTORSLOT+i]= _motorarray_[i];

}



//------------------------------------------------------------------------------


inline void writeMasterChkSumToI2Cmsg() {
  char i;

  __chksum_M__=0;
  for(i=2;i<I2CMSGSIZE; ++i) __chksum_M__ += i2cmastermsg[i];
  Int16ToByteArray( __chksum_M__, i2cmastermsg, CHKSSLOT);

}


//------------------------------------------------------------------------------










//------------------------------------------------------------------------------
// remote motor commands
//------------------------------------------------------------------------------
                           // 0 <= 3: MOTORSLOT: MotNr
                           // 1 <= 4: mot_runstate
                           // 2 <= 5: pwm
                           // 3+4 <= 6+7: mot enc_int16
                           // 5 <= 8: buffer (encoder INT_24?)


void remoteOn(byte MOTnr, int pwm) {
  memset(_motorarray_, 0, 6);
  pwm+=100;              // -100...+100 => 0...+200
  if(pwm>200) pwm=200;
  if(pwm< 0)  pwm= 0;
  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_ON;
  _motorarray_[2]=pwm;
  _motorarray_[3]=VOID;
  _motorarray_[4]=VOID;
  _motorarray_[5]=VOID;

}

//------------------------------------------------------------------------------


void remoteCoast(byte MOTnr) {
  memset(_motorarray_, 0, 6);
  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_COAST;
  _motorarray_[2]=100;   // -100...+100 => 0...+200
  _motorarray_[3]=VOID;
  _motorarray_[4]=VOID;
  _motorarray_[5]=VOID;

}


//------------------------------------------------------------------------------


void remoteBrake(byte MOTnr, int pwm) {
  memset(_motorarray_, 0, 6 );
  pwm+=100;             // -100...+100 => 0...+200
  if(pwm>200) pwm=200;
  if(pwm< 0)  pwm= 0;
  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_BRAKE;
  _motorarray_[2]=pwm;   // -100...+100 => 0...+200
  _motorarray_[3]=VOID;
  _motorarray_[4]=VOID;
  _motorarray_[5]=VOID;

}


//------------------------------------------------------------------------------

// PID control:

// void remoteRotatePIDtotarget   (byte MOTnr, long target, int pwm);  // approach absolute target once
// void remoteRotatePIDdegrees    (byte MOTnr, long degrees, int pwm); // turn relative degrees
// void remoteRotatePIDholdtarget (byte MOTnr, long target, int pwm);  // approach target continuously
// void remoteStopPIDcontrol      (byte MOTnr);

//------------------------------------------------------------------------------


void remoteRotatePIDdegrees(byte MOTnr, long degrees, int pwm) {
  memset(_motorarray_, 0, 6);
  pwm+=100;              // -100...+100 => 0...+200
  if(pwm>200) pwm=200;
  if(pwm< 0)  pwm= 0;
  byte b8, b16, b24;
  b8=lowbyte(degrees);
  b16=highbyte(degrees);
  b24=high24byte(degrees);

  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_PIDRELATIVE;
  _motorarray_[2]=pwm;
  _motorarray_[3]=b8;
  _motorarray_[4]=b16;
  _motorarray_[5]=b24;

}

//------------------------------------------------------------------------------


void remoteRotatePIDtoTarget(byte MOTnr, long degrees, int pwm) {
  memset(_motorarray_, 0, 6);
  pwm+=100;              // -100...+100 => 0...+200
  if(pwm>200) pwm=200;
  if(pwm< 0)  pwm= 0;
  byte b8, b16, b24;
  b8=lowbyte(degrees);
  b16=highbyte(degrees);
  b24=high24byte(degrees);

  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_PIDABSOLUTE;
  _motorarray_[2]=pwm;
  _motorarray_[3]=b8;
  _motorarray_[4]=b16;
  _motorarray_[5]=b24;

}

//------------------------------------------------------------------------------


void remoteRotatePIDholdTarget(byte MOTnr, long degrees, int pwm) {
  memset(_motorarray_, 0, 6);
  pwm+=100;              // -100...+100 => 0...+200
  if(pwm>200) pwm=200;
  if(pwm< 0)  pwm= 0;
  byte b8, b16, b24;
  b8=lowbyte(degrees);
  b16=highbyte(degrees);
  b24=high24byte(degrees);

  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_PIDHOLD;
  _motorarray_[2]=pwm;
  _motorarray_[3]=b8;
  _motorarray_[4]=b16;
  _motorarray_[5]=b24;

}


//------------------------------------------------------------------------------


void remoteStopPIDcontrol(byte MOTnr) {
  memset(_motorarray_, 0, 6);
  _motorarray_[0]=MOTnr;
  _motorarray_[1]=OUT_REGSTATE_PIDIDLE;
  _motorarray_[2]=100; // -100...+100 => 0...+200
  _motorarray_[3]=0;
  _motorarray_[4]=0;
  _motorarray_[5]=0;



}





//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





int Arduino_I2C_RW(byte port, byte devaddr){

  byte nByteReady=0, cnt;
  int  i2cresult = 0;
  byte sendbuf[I2CMSGSIZE+1];

  devaddr=devaddr<<1; //  upper 7 of 8  bits  !!!

  cnt=I2CMSGSIZE;

  while (I2CStatus(port, nByteReady) == STAT_COMM_PENDING) { Yield(); }

  ArrayBuild(sendbuf, devaddr, i2cmastermsg);

  i2cresult=I2CBytes(port, sendbuf, cnt, i2cslavemsg);

  while (!i2cresult){  // false = error
     cls();
     printf1(0,LCD_LINE1,"i2c=%d", 0);
     PlayTone(110,500);
     delay(1000);

     i2cresult=I2CBytes(port, sendbuf, cnt, i2cslavemsg); // try again
  }

  delay(15);
  return i2cresult; // true: succeeded
                    // false:  failed
}







//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
task i2ctask() {

  int i2cresult;  // ok = TRUE !!!

  byte buf, i;
  int  value;


  while(true) {

  Acquire(i2cmutex);
  //AAAAAAAAAAAAAAAAAAAAAAAAAAAA

    writeMotCmdToI2Cmsg();

    i2cmastermsg[TESTSLOT] = MASTER_ID;   // Master testbyte
    writeMasterChkSumToI2Cmsg();           // chksum Master Msg

    i2cresult = Arduino_I2C_RW(ARDSLVPORT_0, ARDSLVADDR_0);    // 14 bytes r+w

    if (i2cresult)  {  // true: succeeded   // false:  failed
                                                              // chksum in msg [0]+[1]
      __chksum_S__ = ByteArrayToInt16(i2cslavemsg, CHKSSLOT); // read slave chksum

      __chksum_R__ = 0;                                       // recalc slave chksum

      for(i=2;i<I2CMSGSIZE; ++i) __chksum_R__ += i2cslavemsg[i];

                                                              // validity check
      if( (i2cslavemsg[TESTSLOT]==ARDS_0_ID) && ( __chksum_S__==__chksum_R__ ) )
      {
         buf = i2cslavemsg[DIGSLOT];               // decypher digital bitpattern
         for (i=2; i<8;++i)  {Adigital[i]= bitRead(buf, i);}

         buf = i2cslavemsg[DIGSLOT+1];
         for (i=8; i<14;++i) {Adigital[i]= bitRead(buf, i-8); }

         i = i2cslavemsg[ANASLOT];   // pin nr
         Aanalog[i] = ByteArrayToInt16(i2cslavemsg,ANASLOT+1);

         i=i2cslavemsg[MOTORSLOT]; // motor nr
         Amotstate[i] = i2cslavemsg[MOTORSLOT+1];
         Amotenc[i] = ByteArrayToInt16(i2cslavemsg,MOTORSLOT+3);

         __keybchar__ = i2cslavemsg[KEYBSLOT];

      }

    }

  //RRRRRRRRRRRRRRRRRRRRRRRRRRRR
  Release(i2cmutex);

     Yield();

  }

}



//------------------------------------------------------------------------------

task displayvalues() {


    cls();



    while (true) {


       printf1(48,56, "%s ",">");

       printf1i(0,40, "%s", "0123456789ABCDEF");  // digital bits

       printf1i(0,24, "%s", " A00 A01 A02 A03");  // analog  INT_16

       printf1( 0,56, "i2c=%-3d", ARDSLVADDR_0<<1);  // print 7 bit i2c addr

       if(__keybchar__) {
         printf1(48,56, ">%c",  __keybchar__ );
         synthikeyb_gr(TRUE);
         __keybchar__ = 0;
       }

       //printf3(0,48,"M%3d ","S%3d ","R%3d",__chksum_M__,__chksum_S__,__chksum_R__);


       printf4( 0,32, "%d","%d","%d","%d", Adigital[ 0],Adigital[ 1],Adigital[ 2],Adigital[ 3]);
       printf4(24,32, "%d","%d","%d","%d", Adigital[ 4],Adigital[ 5],Adigital[ 6],Adigital[ 7]);
       printf4(48,32, "%d","%d","%d","%d", Adigital[ 8],Adigital[ 9],Adigital[10],Adigital[11]);
       printf4(72,32, "%d","%d","%d","%d", Adigital[12],Adigital[13],Adigital[14],Adigital[15]);

       printf4( 0,16, "%4d","%4d","%4d","%4d", Aanalog[0],Aanalog[1],Aanalog[2],Aanalog[3]);

       printf4( 0, 8, "%-6d","%-2d"," %-6d","%2-d", Amotenc[0],Amotstate[0],
                                                    Amotenc[1],Amotstate[1]);
       printf4( 0, 0, "%-6d","%-2d"," %-6d","%-2d", Amotenc[2],Amotstate[2],
                                                    Amotenc[3],Amotstate[3]);

       delay(10);

    }


}




//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

void init() {

  int i;

  SetSleepTime(0);
  cls();

  SetSensorLowspeed(ARDSLVPORT_0);
  delay(20);

  for (i=0; i<MAXMOTORS; ++i); {remoteCoast(i); delay(20); }

}



//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

task main() {

   int  pwm, deg;

   init();

   start displayvalues;
   start i2ctask;




   memset(i2cmastermsg, 0, I2CMSGSIZE); // all entries in i2coutput to 0

   while (true) {


      // demo test values

      remoteRotatePIDdegrees(0, 360, 40);       delay(2000);
      remoteRotatePIDdegrees(0,-360, 40);       delay(2000);

      /*
      remoteOn(0,40);     delay(500);
      remoteOn(1,40);     delay(500);
      remoteOn(2,40);     delay(500);
      remoteOn(3,40);     delay(500);
      remoteCoast(0);     delay(500);
      remoteCoast(1);     delay(500);
      remoteCoast(2);     delay(500);
      remoteCoast(3);     delay(500);
      remoteOn(0,-40);    delay(500);
      remoteOn(1,-40);    delay(500);
      remoteOn(2,-40);    delay(500);
      remoteOn(3,-40);    delay(500);
      remoteCoast(0);     delay(500);
      remoteCoast(1);     delay(500);
      remoteCoast(2);     delay(500);
      remoteCoast(3);     delay(500);
      */
     
      while(true);


 //-------------------------------------------------------------------------



   }

}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



Code für Arduino Mega 2560 als Sensor- und Motor-Multiplexer für
- 6 Motore (max. bis zu 8)
- 16 Touch Sensoren (switch I/O)
- 8 analoge Sensoren



Code: Alles auswählen

//***********************************************************************************
// I²C Muxer feat. PID controller
// I²C Arduino Slave
// ver 1.127 Arduino Mega 2560
// returns medians_of_3 of all sensor data;
// + keyboard interface  ASCII 1...127
// remote motor on/off
// pre-alpha: remote PID control feat. int24 encoder values
// double validity check for safe i2c data transmission
// time-out + Emergency Stop at i2c connx failure
//************************************************************************************
//
// freie Verwendung für private Zwecke
// für kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// programming language: Sketch C/C++ 1.5.6-r2
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http: //creativecommons.org/licenses/by-nc-sa/3.0/ //

#define DEBUG        // debug mode
#define I2CCHECK     // connx check

#include <PID_v1.h>
#include <Wire.h>
#include <PS2Keyboard.h>

#define IARDUINO_I2CADDR    4  // I_ARDUINO I2C dev addr (to shift to upper 7 bit << 1 == 8)

#define MASTER_ID         100  // master ID
#define ARDS_0_ID           4  // Arduino slave_0 ID
#define ARDS_1_ID         201  // Arduino slave_1 ID
#define ARDS_2_ID         202  // Arduino slave_2 ID

#define MAXMOTORS           6 // maximum number of encoder motors supported by Arduino Uno == 2 // Mega == 8
#define MAXANALOG           8 // maximum number of analog sensors supported by Arduino Uno == 4  // Mega == 8
#define MAXDIGPINS         16 // maximum number of digital pins supported by Mega (for Uno just 1 pin (12) left)

// invalid pin setting or token
#define VOID              127

#define MAXMAINCOUNTER     12 // main loop counter 1-12 for 1/12 thread priority prescaler

#define    timeMS         millis // elapsed time since Arduino start
#define    TIMEOUTMS      500
uint32_t  _timer_stop_ ;


//************************************************************************************
// global pin and data setup
//************************************************************************************
/* system API pin constants
#define  INPUT   0
#define  OUTPUT  1
#define  LOW     0
#define  HIGH    1
*/

// I2C setup:
// SDA: dig pin 20 <-> NXT blue   (+pullup resitor 47k-83k -> NXT Vc 4,3V)
// SCL: dig pin 21 <-> NXT yellow (+pullup resitor 47k-83k -> NXT Vc 4,3V)
// >>> !!! connect Arduino-GND      <-> NXT-GND red wire !!! <<<
// >>> !!! DON'T connect Arduino-Vc <-> NXT-Vc  green wire !!! <<<  DON'T DO IT !!!  <<<

// set of pin numbers to read digital pins         // <<<<< !!! customize !!! <<<<<<
uint8_t digpinsread[] = {12,11};                      // <<<<< !!! customize !!! <<<<<<


// setup for motor/data pins
// notice: for Uno/Duemilanove, use pwm only for pins 3,5,6,9,10,11 !

// for Arduino Mega pwm pins are all from 2-13, and 44,45,46
// for Arduino Mega interfering timer interrupt pins are:
// timer 0 (pin 13, 4)  1 ( pin 12, 11)  2 ( pin 10, 9)
// timer 3 ( pin 5, 3, 2) 4 ( pin 8, 7, 6) 5 (pin 44-46)
// timer interrupt   1 for 250µs rotation encounter readings
// timer interrupts  0 + 4 + 5 for motor pwm control
// timer interrupt   3 for keyboard interface
// pins 50-53 reserved for SPI: 50 (MISO), 51 (MOSI), 52 (SCK), 53 (SS)
//
// der Mega ist in der Standardkonfiguration nicht i2c-Kompatibel zum NXT wegen eingebauter pullups.
// Man kann die aber softwaremäßig soweit disablen, dass es trotzdem wieder funktioniert wie beim Uno.
// Dazu die Datei twi.c im Arduino-Programmordner suchen und die beiden unteren Zeilen auskommentieren
// wie es hier steht, falls nicht schon geschehen:
// Code:
// file: twi.c
//           //activate internal pullups for twi.
//           //digitalWrite(SDA, 1);
//           //digitalWrite(SCL, 1);

//keyboard
const int16_t pinKbddata = 4; // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<  customize !!
const int16_t pinKbdIRQ =  3; // keyb IRQ  <<  %timer 3     <<<<<<<  customize !!

// motor 0
#define pinenc0A   22  // enc0A yellow
#define pinenc0B   23  // enc0B blue
#define pinmot0d1  24  // dir0-1   <<
#define pinmot0d2  25  // dir0-2
#define pinmot0pwm 10  // pwm enable0  <<  %timer 2

// motor 1
#define pinenc1A   26  // enc1A yellow
#define pinenc1B   27  // enc1B blue
#define pinmot1d1  28  // dir1-1   <<
#define pinmot1d2  29  // dir1-2
#define pinmot1pwm  9  // pwm enable1  <<  %timer 2


// motor 2
#define pinenc2A   30  // enc2A yellow
#define pinenc2B   31  // enc2B blue
#define pinmot2d1  32  // dir2-1   <<
#define pinmot2d2  33  // dir2-2
#define pinmot2pwm  8  // pwm enable2  <<  %timer 4

// motor 3
#define pinenc3A   34  // enc3A yellow
#define pinenc3B   35  // enc3B blue
#define pinmot3d1  36  // dir3-1   <<
#define pinmot3d2  37  // dir3-2
#define pinmot3pwm  7  // pwm enable3  <<  %timer 4

// motor 4
#define pinenc4A   38  // enc4A yellow
#define pinenc4B   39  // enc4B blue
#define pinmot4d1  40  // dir4-1   <<
#define pinmot4d2  41  // dir4-2
#define pinmot4pwm  6  // pwm enable4  <<  %timer 4

// motor 5
#define pinenc5A   42  // enc5A yellow
#define pinenc5B   43  // enc5B blue
#define pinmot5d1  47  // dir5-1   <<
#define pinmot5d2  48  // dir5-2
#define pinmot5pwm  5  // pwm enable5  <<  %timer 3


// optional: 8 motors
/*
// motor 6
#define pinenc6A   49  // enc6A yellow
#define pinenc6B   13  // enc6B blue
#define pinmot6d1  12  // dir6-1   <<
#define pinmot6d2  11  // dir6-2
#define pinmot6pwm 44  // only for Mega! pwm enable6  <<  %timer 5

// motor 7
#define pinenc7A    4  // enc7A yellow
#define pinenc7B    3  // enc7B blue
#define pinmot7d1   2  // dir7-1   <<
#define pinmot7d2  45  // dir7-2
#define pinmot7pwm 46  // only for Mega! pwm enable7  <<  %timer 5
*/


// I2C pins
#define SDA      20 (NXT blau)
#define SCL      21 (NXT gelb)
 
// SPI header                      |--|
// SPI pins Due         -----------|  |------------
//#define MISO     74   |  RES   76_SCK   74_MISO |
//#define MOSI     75   | -GND   75_MOSI   Vc+5V  |
//#define SCK      76   --------------------------

// SPI pins Mega
//#define MISO     50
//#define MOSI     51
//#define SCK      52
//#define SS       53

// intern LED pin
#define LEDPIN   13


int16_t   afifo[MAXANALOG][ 3];   // fifo buffer for analog values
int16_t   dfifo[MAXDIGPINS][ 3];  // fifo buffer for boolean (I/O) values


uint8_t pinmotdir[MAXMOTORS][ 2] =  // motor direction pin array
{
  {pinmot0d1, pinmot0d2},       
  {pinmot1d1, pinmot1d2},
  {pinmot2d1, pinmot2d2},
  {pinmot3d1, pinmot3d2},
  {pinmot4d1, pinmot4d2},
  {pinmot5d1, pinmot5d2},
};

int16_t  pinmotpwm[MAXMOTORS] =      {pinmot0pwm, pinmot1pwm, pinmot2pwm,  // motor pwm pin array
                                  pinmot3pwm, pinmot4pwm, pinmot5pwm,
                                 };


//************************************************************************************
// global variables for I²C master-slave communication
//************************************************************************************

#define I2CMSGSIZE   14 // size of I2C messages (byte arrays to send + receive)

#define ARDUINOADDR0  4
#define ARDUINOPORT0 S4

#define OUT_REGSTATE_NUL            0
#define OUT_REGSTATE_COAST          2
#define OUT_REGSTATE_BRAKE          3
#define OUT_REGSTATE_EMERG_STOP     5
#define OUT_REGSTATE_ON             8

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_PIDACTIVE     16
#define OUT_REGSTATE_PIDRELATIVE   17
#define OUT_REGSTATE_PIDABSOLUTE   18
#define OUT_REGSTATE_PIDHOLD       19
#define OUT_REGSTATE_PIDHOLDCLOSE  20


uint8_t i2cmastermsg[I2CMSGSIZE],  // i2c master message (command)
                           // 0+1:   chksum this Msg (INT_16)
                           // 2:     sender ID byte
                           // 3:     MOTORSLOT: MotNr
                           // 4:     mot_runstate
                           // 5:     pwm  (-100...+100)
                           // 6+7:   mot enc_int16
                           // 8:     +mot enc_int24 byte
                           // 9:     (anaslot, n/u)
                           // 10+11: buffer (n/u)
                           // 12+13: DIGSLOT: write digital pins 12(0-7) + 13(8-15)

     i2cslavemsg[I2CMSGSIZE];  //i2c  slave message (reply)
                           // 0+1:   chksum this Msg (INT_16)
                           // 2:     sender ID byte
                           // 3:     MOTORSLOT: MotNr
                           // 4:     mot_runstate
                           // 5:     pwm
                           // 6+7:   mot enc_int16
                           // 8:     buffer (encoder INT_24 / keyboard char)
                           // 9:     ANASLOT: nr
                           // 10+11: analog value INT_16
                           // 12+13: DIGSLOT: write digital pins 12(0-7) + 13(8-15)

#define CHKSSLOT      0  // chksum this Msg (INT_16)
#define TESTSLOT      2  // ID byte
#define MOTORSLOT     3  // motors: 3:motor_nr, 4:reg_state 5+6:encoder_INT_16 7+8:2*buffer
#define KEYBSLOT      8  // 8: optional keyboard key pressed
#define ANASLOT       9  // 9,10+11: analog Data:  pin_nr + sensor_INT_16
#define DIGSLOT      12  // 12,13: digital Data: bitpattern


// check sums for i2c messages
volatile int16_t __chksum_M__,
                 __chksum_S__,
                 __chksum_R__ ;

// buffer for motor commands
uint8_t   _motorarray_[6];      // 0: MOTORSLOT: MotNr
// 1: mot_runstate
// 2: pwm
// 3+4: mot enc_int16
// 5: buffer (encoder INT_24?)





//************************************************************************************
// global variables for Interrupt-Service-Routine (ISR)
//************************************************************************************
volatile int8_t ISRab[MAXMOTORS]     = {0, 0, 0, 0, 0, 0};


//************************************************************************************
// global motor & PID variables
//************************************************************************************

volatile int32_t    motenc[MAXMOTORS]    = {0, 0, 0, 0, 0, 0};
                //oldenc[MAXMOTORS]    = {0, 0, 0, 0, 0, 0};

double    PIDsetpoint[MAXMOTORS],
          PIDinput[MAXMOTORS],
          PIDoutput[MAXMOTORS];
uint8_t   OUTregstate[MAXMOTORS];
float     PIDencgap[MAXMOTORS];

//------------------------------------------------------------------------------------



// PID tuning parameters
PID PIDs[] =
{ PID (&PIDinput[ 0], &PIDoutput[ 0], &PIDsetpoint[ 0],  20, 50, 1, DIRECT) ,
  PID (&PIDinput[ 1], &PIDoutput[ 1], &PIDsetpoint[ 1],  20, 50, 1, DIRECT) ,
  PID (&PIDinput[ 2], &PIDoutput[ 2], &PIDsetpoint[ 2],  20, 50, 1, DIRECT) ,
  PID (&PIDinput[ 3], &PIDoutput[ 3], &PIDsetpoint[ 3],  20, 50, 1, DIRECT) ,
  PID (&PIDinput[ 4], &PIDoutput[ 4], &PIDsetpoint[ 4],  20, 50, 1, DIRECT) ,
  PID (&PIDinput[ 5], &PIDoutput[ 5], &PIDsetpoint[ 5],  20, 50, 1, DIRECT)
};

#define  PID_REGTIME_MS      5
#define  PID_REG_PREC        2

#define  PID_REGMAX        255
#define  PID_REGMIN       -255

const float  PWM_SCALE_F = 2.55;  // pwm +/- 100 ==> +/-255


//************************************************************************************
// PS2Keyboard library
//************************************************************************************
/*  PS2Keyboard library example
  PS2Keyboard now requries both pins specified for begin()
  keyboard.begin(data_pin, irq_pin);

  Valid irq pins:
     Arduino Uno:  2, 3
     Arduino Due:  All pins, except 13 (LED)
     Arduino Mega: 2, 3, 18, 19, 20, 21
     Teensy 2.0:   All pins, except 13 (LED)
     Teensy 2.0:   5, 6, 7, 8
     Teensy 1.0:   0, 1, 2, 3, 4, 6, 7, 16
     Teensy++ 2.0: 0, 1, 2, 3, 18, 19, 36, 37
     Teensy++ 1.0: 0, 1, 2, 3, 18, 19, 36, 37
     Sanguino:     2, 10, 11

  for more information you can read the original wiki in arduino.cc
  at http: //www.arduino.cc/playground/Main/PS2Keyboard    //
  or http: //www.pjrc.com/teensy/td_libs_PS2Keyboard.html  //
  Like the Original library and example this is under LGPL license.
  Modified by Cuninganreset@gmail.com on 2010-03-22
  Modified by Paul Stoffregen <paul@pjrc.com> June 2010
*/


PS2Keyboard keyboard;

volatile uint8_t __keybchar__;

void GetKeybInput(); // forward







//************************************************************************************
//************************************************************************************
// setup
//************************************************************************************
//************************************************************************************

inline void Timer1_AVR_Interrupt() {
  //------------------------------------------------------------------------------------
  // timer interrupt for encoder readings
  noInterrupts(); // Jetzt keine Interrupts / disable

  TIMSK1 |= (1 << OCIE1A); // Timer 1 PIDOutput Compare A Match Interrupt Enable
  TCCR1A = 0;             // "Normaler" Modus
  // WGM12: CTC-Modus einschalten (Clear Timer on Compare match)
  //        Stimmen OCR1A und Timer überein, wird der Interrupt ausgelöst
  // Bit CS12 und CS10 setzen

  TCCR1B = (1 << WGM12) | (1 << CS11); // Frequenz = 16000000 / 1024 / 15 = rd. 1042Hz = 1kHz


  // Prescaler OCR1A  einstellen:
  // Frequenz = 16,000,000 / 8 / 64 = rd. 31 kHz (64-1=63)
  // Frequenz = 16,000,000 / 8 / 128 = rd. 16 kHz (128-1=127)
  // Frequenz = 16,000,000 / 8 / 512 = rd. 4 kHz (512-1=511)
  // Frequenz = 16,000,000 / 8 /1024 = rd. 2 kHz (1024-1=1023)
  OCR1A = 511; // also 4kHz = alle 250µs

  interrupts(); // Interrupts wieder erlauben / enable
  //------------------------------------------------------------------------------------
 
}


int16_t   __mainloopcnt__ = 0;           // counter for priority prescaler in main loop

void setup()
{
  //------------------------------------------------------------------------------------

  Wire.begin(IARDUINO_I2CADDR);   // join i2c bus with MY (I_ARDUINO) dev address
  Wire.onReceive(receiveEvent);     // register event
  Wire.onRequest(requestEvent);     // register event

  memset (i2cslavemsg, 0, sizeof(i2cslavemsg)); // reset i2c msg array

  //------------------------------------------------------------------------------------
  // motor pin settings
  // setup for L293D motor driver

  // motor 0
     pinMode(pinenc0A, INPUT_PULLUP);  // enc0A    yellow
     pinMode(pinenc0B, INPUT_PULLUP);  // enc0B    blue
     pinMode(pinmot0d1, OUTPUT);        // dir0-1   
     pinMode(pinmot0d2, OUTPUT);        // dir0-2   
     pinMode(pinmot0pwm ,OUTPUT);       // enable0 
       
     // motor 1
     pinMode(pinenc1A, INPUT_PULLUP);  // enc1A    yellow
     pinMode(pinenc1B, INPUT_PULLUP);  // enc1B    blue
     pinMode(pinmot1d1, OUTPUT);        // dir1-1   
     pinMode(pinmot1d2, OUTPUT);        // dir1-2 
     pinMode(pinmot1pwm, OUTPUT);       // enable1
       
     // motor 2
     pinMode(pinenc2A, INPUT_PULLUP);  // enc2A    yellow
     pinMode(pinenc2B, INPUT_PULLUP);  // enc2B    blue
     pinMode(pinmot2d1, OUTPUT);        // dir2-1 
     pinMode(pinmot2d2, OUTPUT);        // dir2-2   
     pinMode(pinmot2pwm, OUTPUT);       // enable2 
       
     // motor 3
     pinMode(pinenc3A, INPUT_PULLUP);  // enc3A     yellow
     pinMode(pinenc3B, INPUT_PULLUP);  // enc3B     blue
     pinMode(pinmot3d1, OUTPUT);        // dir3-1   
     pinMode(pinmot3d2, OUTPUT);        // dir3-2 
     pinMode(pinmot3pwm, OUTPUT);       // enable3 
       
     // motor 4
     pinMode(pinenc4A, INPUT_PULLUP);  // enc4A     yellow
     pinMode(pinenc4B, INPUT_PULLUP);  // enc4B     blue
     pinMode(pinmot4d1, OUTPUT);        // dir4-1   
     pinMode(pinmot4d2, OUTPUT);        // dir4-2 
     pinMode(pinmot4pwm, OUTPUT);       // enable4 
       
     // motor 5
     pinMode(pinenc5A, INPUT_PULLUP);  // encA5     yellow
     pinMode(pinenc5B, INPUT_PULLUP);  // encB5     blue
     pinMode(pinmot5d1, OUTPUT);        // dir5-1   
     pinMode(pinmot5d2, OUTPUT);        // dir5-2   
     pinMode(pinmot5pwm, OUTPUT);       // enable5
       


  //------------------------------------------------------------------------------------
  // touchpins in list "digipinsread" =>  INPUT_PULLUP (=> no external resistors needed! )
  for (int16_t i = 0; i < sizeof(digpinsread); ++i) {
    pinMode(digpinsread[i], INPUT_PULLUP);
  }

  //------------------------------------------------------------------------------------
  // AVR timer interrupt for encoder readings
 
  Timer1_AVR_Interrupt();
 
  //------------------------------------------------------------------------------------

  // reset PID parameters

  for (int16_t i = 0; i < MAXMOTORS; ++i) {
    PIDs[ i].SetMode(AUTOMATIC);
    PIDs[ i].SetOutputLimits(PID_REGMIN, PID_REGMAX);
    PIDs[ i].SetSampleTime(PID_REGTIME_MS);
    PIDsetpoint[ i] = 0;                      // set target,
    OUTregstate[ i] = OUT_REGSTATE_NUL;       // switch the PID on to motor[i]
  }

  //------------------------------------------------------------------------------------
  // init PS/2 keyboard

  //keyboard.begin(pinKbddata, pinKbdIRQ, PS2Keymap_German);

  Serial.begin(115200);
 
  Serial.println( "safety delay before start");
  delay(1000);  // safety delay before start
  Serial.println();
 
 
 

  //------------------------------------------------------------------------------------

}










//************************************************************************************
// bit and byte and pin operations
//************************************************************************************

//------------------------------------------------------------------------------

inline int16_t  ByteArrayToInt16(uint8_t  array[], uint8_t  slot) {
  return ((array[slot + 1] << 8) + (array[slot]));
}

inline int32_t  ByteArrayToInt24(uint8_t  array[], uint8_t  slot) {
  return ((array[slot + 2] << 16) + (array[slot + 1] << 8) + (array[slot]));
}

//------------------------------------------------------------------------------

inline void Int16ToByteArray(int16_t  integer, uint8_t  *array, uint8_t  slot) {
  array[slot]   = (integer & 0xFF); // loByte
  array[slot + 1] = (integer >> 8); // hiByte
}

inline void Int24ToByteArray(int32_t  int24, uint8_t  *array, uint8_t  slot) {
  array[slot]   = (int24 & 0xFF);         // loByte 8
  array[slot + 1] = ((int24 >> 8) & 0xFF); // hiByte 16
  array[slot + 2] = ((int24 >> 16) & 0xFF); // hiByte 24
}

//------------------------------------------------------------------------------

/*
#define bitRead(source, bit) ( ((source) >> (bit)) & 0x01 )
#define bitSet (source, bit) ( (source) |= (1UL << (bit)) )
#define bitClear(source, bit) ( (source) &= ~(1UL << (bit)) )
#define bitWrite(source, bit, bitvalue) ( bitvalue ? bitSet(source, bit) : bitClear(source, bit) )
*/


//------------------------------------------------------------------------------------

uint8_t  digpinMode(uint8_t  pinnr) {             // returns pinmode setting: OUTPUT (1) or INPUT (0) ?
  if (pinnr < 8)
    return ((DDRD & (1 << pinnr)) != 0);
  else    if (pinnr < 14)                 // for Arduino Uno: 14 digital pins 0..13
    return ((DDRB & (1 << (pinnr - 8))) != 0);
}


//------------------------------------------------------------------------------------

int16_t  toggleval(int16_t  nr,  int16_t  max) {
  if ( nr < (max - 1) ) ++nr;
  else nr = 0;
  return nr;
}


//------------------------------------------------------------------------------------

#define sensortouch(pinHIGH) !digitalRead(pinHIGH)

//------------------------------------------------------------------------------------






//************************************************************************************
// array  &  fifo operations
//************************************************************************************



inline void bubblesort(int16_t  *array,  int16_t  length)
{
  int16_t  i, j;
  int16_t  tmp;

  for (i = 0; i < length - 1; ++i)     {
    for (j = 0; j < length - i - 1; ++j) {
      if (array[j] > array[j + 1])         {
        tmp = array[j];
        array[j] = array[j + 1];
        array[j + 1] = tmp;
      }
    }
  }
}



// next version: int32_t  medianIL(int16_t nr, int16_t len)
//--------------------------------------------


inline int16_t  mediana3(int16_t  nr)
{
  int16_t temp[3], i;

  memset ( temp, 0, 3 );
  for ( i = 0; i < 3; ++i)  temp[i] = afifo[nr][i];
  bubblesort( temp, 3 );
  return temp[1];
}



inline int16_t  mediand3(int16_t  nr)
{
  int16_t  temp[3], i;

  memset ( temp, 0, 3 );
  for ( i = 0; i < 3; ++i) temp[i] = dfifo[nr][i];
  bubblesort( temp, 3 );
  return temp[1];
}



// next version: void * memmove ( void * destination, const void * source, size_t num );
//--------------------------------------------

inline void afifopush(int16_t  nr,  int16_t  _new)
{
  afifo[nr][2] = afifo[nr][1];
  afifo[nr][1] = afifo[nr][0];
  afifo[nr][0] = _new;
}

//--------------------------------------------

inline void dfifopush(int16_t  nr,  uint16_t  _new)
{
  dfifo[nr][2] = dfifo[nr][1];
  dfifo[nr][1] = dfifo[nr][0];
  dfifo[nr][0] = _new;
}


//--------------------------------------------






//************************************************************************************
// Encoder functions courtesy of / entnommen aus: http: //www.meinDUINO.de //
//************************************************************************************

// Schritt-Tabellen für 1/1, 1/2 oder 1/4-Auflösung/resolution

// 1/1 Auflösung/resolution
//int8_t schrittTab[16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};

// 1/2 Auflösung/resolution
int8_t  schrittTab[16] = {0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 1, 0, 0, -1, 0};

// 1/4 Auflösung/resolution
//int8_t schrittTab[16] = {0,0,0,0,0,0,0,-1,0,0,0,0,0,1,0,0};



//------------------------------------------------------------------------------------
// Interrupt Service Routine: encoder readings at Interrupt
//------------------------------------------------------------------------------------


ISR(TIMER1_COMPA_vect) {  // read encoder values

  ISRab [ 0] <<= 2;
  ISRab [ 0] &= B00001100;
  ISRab [ 0] |= (digitalRead(pinenc0A) << 1) | digitalRead(pinenc0B);
  motenc[ 0] += schrittTab[ISRab[0]];           //

  ISRab [ 1] <<= 2;
  ISRab [ 1] &= B00001100;
  ISRab [ 1] |= (digitalRead(pinenc1A) << 1) | digitalRead(pinenc1B);
  motenc[ 1] += schrittTab[ISRab[1]];           //

  ISRab [ 2] <<= 2;
  ISRab [ 2] &= B00001100;
  ISRab [ 2] |= (digitalRead(pinenc2A) << 1) | digitalRead(pinenc2B);
  motenc[ 2] += schrittTab[ISRab[2]];           //

  ISRab [ 3] <<= 2;
  ISRab [ 3] &= B00001100;
  ISRab [ 3] |= (digitalRead(pinenc3A) << 1) | digitalRead(pinenc3B);
  motenc[ 3] += schrittTab[ISRab[3]];           //

  ISRab [ 4] <<= 2;
  ISRab [ 4] &= B00001100;
  ISRab [ 4] |= (digitalRead(pinenc4A) << 1) | digitalRead(pinenc4B);
  motenc[ 4] += schrittTab[ISRab[4]];           //

  ISRab [ 5] <<= 2;
  ISRab [ 5] &= B00001100;
  ISRab [ 5] |= (digitalRead(pinenc5A) << 1) | digitalRead(pinenc5B);
  motenc[ 5] += schrittTab[ISRab[5]];           //


}












//************************************************************************************
// motor control
//************************************************************************************
// no PID

inline void motoron(int16_t  motnr,  int16_t  power) {
  if (power > 0) {
    digitalWrite( pinmotdir[motnr][0], HIGH);
    digitalWrite( pinmotdir[motnr][1], LOW);
  }  else  if (power < 0) {
    digitalWrite( pinmotdir[motnr][0], LOW);
    digitalWrite( pinmotdir[motnr][1], HIGH);
  }  else  if (power == 0) {
    digitalWrite( pinmotdir[motnr][0], LOW);
    digitalWrite( pinmotdir[motnr][1], LOW);
  }
  power = abs(power);
  if (power > PID_REGMAX) power = PID_REGMAX;
  analogWrite( pinmotpwm[motnr], power);
}


inline void motorbrake(int16_t  motnr, int16_t  brakepwr) {
  digitalWrite( pinmotdir[motnr][0], LOW);
  digitalWrite( pinmotdir[motnr][1], LOW);
  analogWrite( pinmotpwm[motnr], brakepwr);         
}


inline void motorcoast(int16_t  motnr) {
  digitalWrite( pinmotdir[motnr][0], LOW);
  digitalWrite( pinmotdir[motnr][1], LOW);
  analogWrite( pinmotpwm[motnr], 0); 
}


#define     motoroff    motorcoast


void EmergencyStop() {
  for (int16_t motnr = 0; motnr < MAXMOTORS; ++motnr) {
    motorcoast(motnr);
    OUTregstate[motnr] = OUT_REGSTATE_COAST;
  }
}


//------------------------------------------------------------------------------------
// PID control

inline void rotatePIDdegrees (int16_t  motnr,  int32_t  angle) {

      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
           PIDsetpoint[motnr] = angle + motenc[motnr];
           OUTregstate[motnr] = OUT_REGSTATE_PIDRELATIVE;
      }

}

inline void rotatePIDtoTarget (int16_t  motnr,  int32_t  angle) {
      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE  ) {
           PIDsetpoint[motnr] = angle;
           OUTregstate[motnr] = OUT_REGSTATE_PIDABSOLUTE;
      }
}

inline void rotatePIDholdTarget (int16_t  motnr,  int32_t  angle) {
      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE  ) {
           PIDsetpoint[motnr] = angle;
           OUTregstate[motnr] = OUT_REGSTATE_PIDHOLD;
      }
}


inline void stopPIDcontrol (int16_t  motnr) {
      OUTregstate[motnr] = OUT_REGSTATE_PIDIDLE;
      motorcoast(motnr);

}

//------------------------------------------------------------------------------------



inline void writeMotCmdToI2CSmsg() {
  for (int8_t i = 0; i < 6; ++i) i2cslavemsg[MOTORSLOT + i] = _motorarray_[i];
}

//------------------------------------------------------------------------------------

inline void writeI2CMmsgToMotCmd() {
  for (int8_t i = 0; i < 6; ++i) _motorarray_[i] = i2cmastermsg[MOTORSLOT + i];
}

//------------------------------------------------------------------------------------
// 0 <= 3: MOTORSLOT: MotNr
// 1 <= 4: mot_runstate
// 2 <= 5: pwm
// 3+4 <= 6+7: mot enc_int16
// 5 <= 8: buffer (encoder INT_24?)

//------------------------------------------------------------------------------------


inline void executeRemoteMotCmd() {
  int16_t  motnr, pwm, regst;
  int32_t  enc, angle;


  motnr = _motorarray_[0];
  regst = _motorarray_[1];
  pwm  = ( (int)_motorarray_[2] - 100) * PWM_SCALE_F; // 0...200 => -100...+100 => -255...+255
  angle = ByteArrayToInt24( _motorarray_, 3);

  if (regst <= OUT_REGSTATE_PIDIDLE)
  {                                               
    if (regst == OUT_REGSTATE_COAST) {            // coast
       OUTregstate[motnr] = regst;
       motorcoast(motnr);
    }

    else

    if (regst == OUT_REGSTATE_BRAKE) {          // brake + braking force pwm   
         OUTregstate[motnr] = regst;
         motorbrake(motnr, pwm);
    }

    else

    if (regst == OUT_REGSTATE_ON)   {           // on forward / reverse (+/- pwm)
         OUTregstate[motnr] = regst;
         motoron(motnr, pwm);
    }
  }

  else                                             // PID commmands only if PID inactive
  {                                                //-----------------------------------
     if (regst == OUT_REGSTATE_PIDRELATIVE) {      // rotate relative degrees, then release
        if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
           rotatePIDdegrees(motnr, angle);
        }
     }

     else

     if (regst == OUT_REGSTATE_PIDABSOLUTE) {      // rotate to absolute enc target, then release
        if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
           rotatePIDtoTarget(motnr, angle);
        }
     }

     else

     if (regst >= OUT_REGSTATE_PIDHOLD) {          // rotate to absolute enc target + hold position
        if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
           rotatePIDholdTarget(motnr, angle);
        }
     }

     else

     if (regst == OUT_REGSTATE_PIDIDLE) {             // shut off PID control intermediately
          OUTregstate[motnr] = OUT_REGSTATE_PIDIDLE;
          motorcoast(motnr);
     }
  }


}




//************************************************************************************
// I²C events
//************************************************************************************

void receiveEvent(int16_t byteCount) {

  uint8_t i = 0, data;
  __chksum_R__ = 0;                                      // recalc master cksum
  while ( Wire.available() ) {
    data = Wire.read();
    i2cmastermsg[i] = data;
    // chksum in msg [0]+[1]
    if (i >= 2) __chksum_R__ += i2cmastermsg[i];         // recalc master cksum
    ++i;                                                 // number of bytes read
  }

  // verify chksum
  __chksum_M__ = ByteArrayToInt16(i2cmastermsg, CHKSSLOT); // read master chksum
 
  // validity check
  if ( (i2cmastermsg[TESTSLOT] == MASTER_ID) && ( __chksum_M__ == __chksum_R__ ) )
  {
    writeI2CMmsgToMotCmd();   // copy motor command to array:
    executeRemoteMotCmd();   // decypher motor command array and execute
  }




}

//------------------------------------------------------------------------------------

void requestEvent() {
  _timer_stop_ = timeMS(); // time-out reset

  Wire.write(i2cslavemsg, sizeof(i2cslavemsg));  // respond with message (sizeof) bytes

  __keybchar__ = 0;
  _timer_stop_ = timeMS(); // time-out reset
}









//************************************************************************************
// I²C read / write arrays
//************************************************************************************

inline void writeDIGtoi2creply(uint8_t  digpin,  uint8_t  bitval) {

  if ((digpin >= 2) && (digpin < 8))  {
    bitWrite(i2cslavemsg[DIGSLOT], digpin, bitval);
  }
  else if ((digpin >= 8) && (digpin < 14)) {
    bitWrite(i2cslavemsg[DIGSLOT + 1], digpin - 8, bitval);
  }
}

//------------------------------------------------------------------------------------

inline void writeANAtoi2creply(uint8_t  slot,  uint8_t pin,  int16_t  intval) {
  i2cslavemsg[slot] = pin;
  i2cslavemsg[slot + 1] = lowByte(intval);
  i2cslavemsg[slot + 2] = highByte(intval);
}

//------------------------------------------------------------------------------------

inline void writeMOTtoi2creply(uint8_t  nr) {
  i2cslavemsg[MOTORSLOT]   = nr;
  i2cslavemsg[MOTORSLOT + 1] = OUTregstate[nr];
  i2cslavemsg[MOTORSLOT + 2] = pinmotpwm[nr];
  i2cslavemsg[MOTORSLOT + 3] = lowByte(motenc[nr]);
  i2cslavemsg[MOTORSLOT + 4] = highByte(motenc[nr]);
  i2cslavemsg[MOTORSLOT + 5] = 0;  //  to do !!
}

//------------------------------------------------------------------------------

inline void writeSlaveChkSumToI2Cmsg() {
  int8_t i;

  __chksum_S__ = 0;
  for (i = 2; i < I2CMSGSIZE; ++i) __chksum_S__ += i2cslavemsg[i];
  Int16ToByteArray( __chksum_S__, i2cslavemsg, CHKSSLOT);

}



//************************************************************************************
// get input values
//************************************************************************************


inline void GetSensorInputs() {

  // read all analog pins
  for (int16_t i = 0; i < MAXANALOG; ++i) {
    afifopush(i, analogRead(i));                               // write median of 3 analog readings
  }

  // read list of digital INPUT  pins
  for (int16_t i = 0; i < sizeof(digpinsread); ++i) {
    dfifopush(digpinsread[i], digitalRead(digpinsread[i]) );  // write median of 3 digital readings
  }
}


//------------------------------------------------------------------------------------

inline void GetKeybInput() {
  uint8_t c;
  if (keyboard.available() ) {

    c = keyboard.read(); // read the first key

    // debug mode: only Standard ASCII
    if ( c <= 127 ) __keybchar__ = c;
    else {
      //if (keyboard.available() ) {c = keyboard.read(); } // read the next key
      __keybchar__ = 0; // only
    }

    if (__keybchar__ > 0) {        //debug
      if ( (__keybchar__ <= 32) || (__keybchar__ > 126) ) {
        Serial.println();
        Serial.println((c));
      }
      else Serial.print( char(__keybchar__) );
    }

  }
}









//************************************************************************************
// build I²C message to the master out of digital and analog values
//************************************************************************************


void BuildI2Cmessage() {
  volatile static uint8_t anapin = 0, motpin = 0; // toggle

  memset (i2cslavemsg, 0, sizeof(i2cslavemsg));

  //----------------------------------------------------------
  // poll + send all digital pin values
  //Serial.println();
  for (int16_t i = 0; i < sizeof(digpinsread); ++i) { // write median of last 3 digital readings
    writeDIGtoi2creply(digpinsread[i] , (uint8_t)mediand3(digpinsread[i]));   
  }

  //----------------------------------------------------------
  // poll + send 1 analog pin+data sequentially; auto toggle
  writeANAtoi2creply(ANASLOT, anapin, mediana3(anapin));    // write median to i2creply array
  anapin = toggleval(anapin, MAXANALOG);

  //----------------------------------------------------------
  // poll + send motor pin+data sequentially; auto toggle
  writeMOTtoi2creply(motpin); // motor data: regstate, pwm, encoder
  motpin = toggleval(motpin, MAXMOTORS);


  //----------------------------------------------------------
  // send keyboard
  i2cslavemsg[KEYBSLOT] = __keybchar__;

  //----------------------------------------------------------
  // calculate and send test + chksum => i2cmsg [0]+[1]
  i2cslavemsg[TESTSLOT] = ARDS_0_ID;
  writeSlaveChkSumToI2Cmsg();

}





//************************************************************************************
// PID regulation
//************************************************************************************


int16_t rotatePID(int16_t  motnr,  int32_t  angle, int16_t  regmode) {
   if (OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
       PIDsetpoint[motnr] = angle;   
       OUTregstate[motnr] = regmode;
   }
   
}

//------------------------------------------------------------------------------------

void  PIDupdate( uint8_t  nr)
{
  PIDinput[nr]  = motenc[nr];
 
  PIDencgap[nr] = abs( PIDsetpoint[nr] - PIDinput[nr] ); //distance away from setpoint

  if ( OUTregstate[nr] >= OUT_REGSTATE_PIDACTIVE )   // PID_ACTIVE
  {
     if( PIDencgap[nr] <= PID_REG_PREC ) {
        PIDs[nr].SetTunings( 0, 0, 0);   
     }

     else     
     if( PIDencgap[nr] <= 5 ) {     
       if ( OUTregstate[nr]  < OUT_REGSTATE_PIDHOLD )
          PIDs[nr].SetTunings( 20, 30, 1); 
     }
     else    {
        PIDs[nr].SetTunings( 20, 40, 1.2);   
     }
     
     PIDs[nr].Compute();
 
     if ( PIDencgap[nr] > PID_REG_PREC                     // target gap failed
          &&   OUTregstate[nr] >= OUT_REGSTATE_PIDACTIVE ) // PID_ACTIVE
     {
        motoron(nr, PIDoutput[nr]);
     }
  }
 

   if ( PIDencgap[nr] <= PID_REG_PREC                   // target  reached
   && OUTregstate[nr]  >= OUT_REGSTATE_PIDACTIVE        // PID_ACTIVE !
   && abs( PIDoutput[nr] ) < PID_REGMAX / 4
   && OUTregstate[nr]   < OUT_REGSTATE_PIDHOLD)           // small motor regulation
   {
      PIDs[nr].SetTunings( 0,  0,  0);
      PIDs[nr].SetMode(MANUAL);
      OUTregstate[nr] = OUT_REGSTATE_PIDIDLE;
   }

   if ( OUTregstate[nr]  >= OUT_REGSTATE_PIDHOLD ) {
      if ( PIDencgap[nr] <= 3*PID_REG_PREC ) {
         OUTregstate[nr] = OUT_REGSTATE_PIDHOLDCLOSE;
      }
      else
         OUTregstate[nr] = OUT_REGSTATE_PIDHOLD;
   }
}




//************************************************************************************
// main loop()
//************************************************************************************

void loop()
{
  char strbuf[50];
  int16_t  regmode;

  // read analog and digital inputs, store to fifo buf
  GetSensorInputs();

  //read keyboard inputs
  GetKeybInput();   // read keyboard input: to do list ! <<<<<<<<<<<<<<

  // build I²C message to the master out of digital and analog values repeatedly
  if (__mainloopcnt__ % 2 == 0) {    // priority
    BuildI2Cmessage();
  }


#ifdef I2CCHECK
  if (timeMS() - _timer_stop_ > TIMEOUTMS ) { EmergencyStop(); delay(500); }
#endif
 
  // test, debug
  //
  // 
   
  // motor and PID handling
  for (int16_t i = 0; i < MAXMOTORS ; ++i) PIDupdate( i); // regulate motors
 
  if ((__mainloopcnt__ %2) )
  {
     sprintf(strbuf,
     "tgt=%5d  enc=%5d  pwr=%6d  rstate=%d",
     (int)PIDsetpoint[ 0], (int)motenc[ 0], (int)PIDoutput[0], OUTregstate[0] );
     Serial.print(strbuf);
     Serial.println();
  }

  __mainloopcnt__ ++;
  if (__mainloopcnt__ >= MAXMAINCOUNTER) {          // priority = 1/12
    __mainloopcnt__ = 0;                          // reset loop counter
  }
 
 
  delay(1);
}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 25. Mai 2015 17:32

Bilder aus einem -Backup

NXT plus Arduino als Motor- / Sensor-Multiplexer

NXT_ArduinoUno_02.jpg
1. Testaufbau mit Uno: 2 Motoren plus 1 Taster plus 3 Potis (analog-Sensor)

NXT_ArduinoMega_03.jpg
1. Testaufbau mit Mega: 4 Motoren plus 1 Taster plus 4 Potis (analog-Sensor)

NXT_ArduinoMega_04.jpg
zusätzlich ein PC-Keyboard zur Tastatureingabe am Mega (kann auch Töne spielen ;) )

NXT_ArduinoMega_05.jpg
4 Motoren, 2x digital Taster, 5x analog Potis

Arduino_6xPowerMotor_Board01a.jpg
6x Hochleistungs-Motortreiber (bis 24V, bis 40A jeweils)

Arduino_6xPowerMotor_Board01b.jpg
6x Hochleistungs-Motortreiber (bis 24V, bis 40A jeweils)

Arduino_6xPowerMotor_Board05.jpg
6x Hochleistungs-Motortreiber am Arduino

10881722_597705007026646_6790633453854269263_n.jpg
multiple Erweiterungen lokal am NXT -master und "remote" am Arduino-slave, inkl. Pixy-Cam und Original-Lego-Sensoren am Arduino

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 25. Mai 2015 17:34

Zusatz-libs:

PID_v1.h
PID_v1.zip
(7.02 KiB) 170-mal heruntergeladen


keyboard.h
PS2Keyboard.zip
(10.64 KiB) 153-mal heruntergeladen


DueTimer.h:
DueTimer.zip
(8.57 KiB) 155-mal heruntergeladen


lib.c float patch:
libc_all.zip
lib.c float patch, neu hochgeladen;
copy to:
C:\Programme\Arduino\hardware\tools\avr\avr\lib
(1 MiB) 230-mal heruntergeladen

Achtung! Der Patch äuft mit aktuelleren Arduino-IDE-Versionen evtl. nicht mehr!

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 26. Mai 2015 11:20

(Platzhalter)

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 1. Jul 2015 09:24

hier einige Links, wie Lego NXT Sensoren mit Arduinos verbunden und abgefragt werden können:

NXTUSS_demo.jpg
http://www.thecompblog.com/2012/07/hacking-lego-nxt-part-1.html
http://www.thecompblog.com/2012/07/hacking-lego-nxt-part-2.html
http://www.thecompblog.com/2012/08/hacking-lego-nxt-part-3.html


NXT Taster (Touch-Sensor) :
http://www.thecompblog.com/2012/07/hacking-lego-nxt-part-2.html
Verkabelung:
pins 2+3 verbinden, dann --> 1kOhm --> GND
pin 4 --> +3,3V
pin 1 --> digitaler Arduino-PIN

Code: Alles auswählen

*
This is a program to test the Lego NXT Touch Sensor.
A schematic of the circuit required can be found on TheCompBlog.com.
This code was writen (badly) by Nicolas Weninger, author of TheCompBlog.com.

THIS SOFTWARE COMES WITHOUT ANY WARRANTY, IMPLIED OR EXPLICIT, TO THE MAXIMUM EXTENT PERMITTABLE BY LAW. THIS INCLUDES
WARRANTY AGAINST DAMAGE TO COMPUTER SYSTEMS OR DATA, LOSS OF PROFIT, PERSONAL INJURY OR DEATH.
 
This code is in the public domain.
*/

const int button = 2; //connects to pin3 of the sensor

void setup()
{
  Serial.begin(9600);
  pinMode(13, OUTPUT);
}

void loop()
{
  if(digitalRead(button) == 1)
  {
    digitalWrite(13, HIGH);
  }
  else
  {
    digitalWrite(13, LOW);
  }
  Serial.println(digitalRead(button));
}



NXT-Lichtsensor (Light-Sensor):
http://www.thecompblog.com/2012/07/hacking-lego-nxt-part-2.html
NXTLightSensor_Fritzing.jpg
Light Sensor (Fritzing)

Code: Alles auswählen

/*
This is a program to test the Lego NXT Light Sensor.
A schematic of the circuit required can be found on TheCompBlog.com.
This code was writen (badly) by Nicolas Weninger, author of TheCompBlog.com.

THIS SOFTWARE COMES WITHOUT ANY WARRANTY, IMPLIED OR EXPLICIT, TO THE MAXIMUM EXTENT PERMITTABLE BY LAW. THIS INCLUDES
WARRANTY AGAINST DAMAGE TO COMPUTER SYSTEMS OR DATA, LOSS OF PROFIT, PERSONAL INJURY OR DEATH.
 
This code is in the public domain.
*/

const int reader = A5; //connects to pin1 of sensor
const int button = 2; //momentary switch
const int light = 3; //connects to pin5 of sensor

int state = LOW;
int previous = LOW;

void setup()
{
  Serial.begin(9600);
  pinMode(light, OUTPUT);
}

void loop()
{
  int reading = digitalRead(button);
  if(reading == HIGH && previous == LOW)
  {
    if(state == HIGH)
    {
      state = LOW;
    }
    else
    {
      state = HIGH;
    }
  }
 
  digitalWrite(light, state);
  previous = reading;
  Serial.println(analogRead(reader)); //print the light level reading
  delay(10); //good practice after an analogRead. Don't know why though…
}





NXT Geräuschsensor (Sound Sensor):
http://www.thecompblog.com/2012/08/hacking-lego-nxt-part-3.html
NXTSoundSensor_Fritzing.jpg
Sound Sensor (Fritzing)


Code: Alles auswählen

/*
  This code reads the analog value of the Lego NXT sound sensor.
  The schematic can be found on thecompblog.com
 
  This code was written (badly) by Nicolas Weninger, auther of TheCompBlog.
  THIS SOFTWARE COMES WITHOUT ANY WARRANTY, IMPLIED OR EXPLICIT, TO THE MAXIMUM EXTENT PERMISSIBLE BY LAW.

  THIS INCLUDES WARRANTY AGAINST DAMAGE TO COMPUTER SYSTEMS, LOSS OF PROFIT, PERSONAL INJURY OR DEATH.
 
  This code is in the public domain.
*/

const int mic = A5;

void setup()
{
  Serial.begin(9600);
}

void loop()
{
  Serial.println(analogRead(mic));
  delay(10);
}



NXT Ultraschallsensor (Ultrasonic Sensor)
http://www.thecompblog.com/2012/08/hacking-lego-nxt-part-3.html
http://blog.tkjelectronics.dk/2011/10/nxt-shield-ver2/
http://blog.tkjelectronics.dk/wp-content/uploads/Appendix-7-LEGO-MINDSTORMS-NXT-Ultrasonic-Sensor-I2C-communication-protocol.pdf
NXTUSS_Fritzing.jpg
US Sensor (Fritzing)

notwendige i2cmaster.h library für NXT USS:
i2cmaster.zip
i2cmaster.h library (für NXT-USS driver)
(6.25 KiB) 153-mal heruntergeladen



Code: Alles auswählen

#include <i2cmaster.h>

byte clockPin = 4;
byte buf[9];//Buffer to store the received valeus
byte addr = 0x02;//address 0x02 in a 8-bit context - 0x01 in a 7-bit context
byte distance;

void setup()

  i2c_init();//I2C frequency = 11494,253Hz
  Serial.begin(115200);
  printUltrasonicCommand(0x00);//Read Version
  printUltrasonicCommand(0x08);//Read Product ID
  printUltrasonicCommand(0x10);//Read Sensor Type
  printUltrasonicCommand(0x14);//Read Measurement Units 
}

void loop()
{   
//  printUltrasonicCommand(0x42);//Read Measurement Byte 0
  distance = readDistance();
  if(distance == 0xFF)
    Serial.println("Error Reading Distance");
  else
    Serial.println(distance, DEC);   
}
byte readDistance()

  delay(100);//There has to be a delay between commands
  byte cmd = 0x42;//Read Measurement Byte 0
   
  pinMode(clockPin, INPUT);//Needed for writing to work
  digitalWrite(clockPin, HIGH); 
 
  if(i2c_start(addr+I2C_WRITE))//Check if there is an error
  {
    Serial.println("ERROR i2c_start");
    i2c_stop();
    return 0xFF;
  }   
  if(i2c_write(cmd))//Check if there is an error
  {
    Serial.println("ERROR i2c_write");
    i2c_stop();
    return 0xFF;
  } 
  i2c_stop();
   
  delayMicroseconds(60);//Needed for receiving to work
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW);
  delayMicroseconds(34);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  delayMicroseconds(60); 
   
  if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
  {
    Serial.println("ERROR i2c_rep_start");
    i2c_stop();   
    return 0xFF;
  } 
  for(int i = 0; i < 8; i++)
    buf[i] = i2c_readAck();
  buf[8] = i2c_readNak(); 
  i2c_stop();
 
  return buf[0]; 
}

void printUltrasonicCommand(byte cmd)
{
  delay(100);//There has to be a delay between commands
   
  pinMode(clockPin, INPUT);//Needed for writing to work
  digitalWrite(clockPin, HIGH);
 
  if(i2c_start(addr+I2C_WRITE))//Check if there is an error
  {
    Serial.println("ERROR i2c_start");
    i2c_stop();
    return;
  }
  if(i2c_write(cmd))//Check if there is an error
  {
    Serial.println("ERROR i2c_write");
    i2c_stop();
    return;
  }
  i2c_stop();
   
  delayMicroseconds(60);//Needed for receiving to work
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW);
  delayMicroseconds(34);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  delayMicroseconds(60); 

  if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
  {
    Serial.println("ERROR i2c_rep_start");
    i2c_stop();
    return;
  }
  for(int i = 0; i < 8; i++)
    buf[i] = i2c_readAck();
  buf[8] = i2c_readNak();
  i2c_stop(); 
 
  if(cmd == 0x00 || cmd == 0x08 || cmd == 0x10 || cmd == 0x14)
  {
      for(int i = 0; i < 9; i++)
      {
        if(buf[i] != 0xFF && buf[i] != 0x00)
          Serial.print(buf[i]);
        else
          break;
      }             
  }
  else
    Serial.print(buf[0], DEC); 

  Serial.println("");     
}
/*
' Wires on NXT jack plug.
' Wire colours may vary. Pin 1 is always end nearest latch.
' 1 White +9V
' 2 Black GND
' 3 Red GND
' 4 Green +5V
' 5 Yellow SCL - also connect clockpin to give a extra low impuls
' 6 Blue SDA
' Do not use i2c pullup resistor - already provided within sensor.
*/
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5402
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT plus Arduino als Motor- / Sensor-Multiplexer

Beitragvon HaWe » 22. Jul 2015 21:01

Driver und Source Code, um EV3-Sensoren an Arduinos zu betreiben:

https://lejosnews.wordpress.com/2014/05 ... t-sensors/

https://github.com/lawrie/EV3_Dexter_In ... V3_arduino

https://github.com/lawrie/EV3_Dexter_In ... UARTSensor

Kabel-Verbindung :

Code: Alles auswählen

IR-sensor 2 (black)......Arduino GND
IR-sensor 3 ( red )......Arduino GND
IR-sensor 4 (green)......Arduino +5V
IR-sensor 5 (yellow).....Arduino RX
IR-sensor 6 ( blue ).....Arduino TX


(danke an Andy Shaw (leJOS) und Lawrie Griffiths (Dexter Industries) ! :)


Zurück zu „Projekte, Showcase, Bauanleitungen“

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 8 Gäste

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss