Projekt: LegoPi - Raspberry Pi für Lego Roboter (statt NXT, EV3)

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

Moderator: Moderatoren


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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 21. Feb 2016 10:55

verwendete Hardware:

Raspberry Pi 2 Model B:
(ARM Cortex-A7 CPU, 900 MHz, quad-core, 1GB RAM, 4x USB, 1x LAN, 1x HDMI)
https://www.raspberrypi.org/products/raspberry-pi-2-model-b/
Bild
Preis je nach Bezugsquelle ca. 40 EUR inkl. Versand
http://www.idealo.de/preisvergleich/Off ... ation.html

Raspi-Zubehör:
Erweiterungs- und Starter-Sets, Maus, Keyboard, HDMI Display:

Erweiterungsset aus Netzteil, T-Cobbler, Kühlkörper, WiFi-Stick, Micro-SD-Card (mind. 8GB, am besten >= 16GB), HDMI-Kabel, z.B. über Ebay:
http://www.ebay.de/itm/121792804226?_trksid=p2060353.m2749.l2649&ssPageName=STRK%3AMEBIDX%3AIT

alle kabelgebundenen USB-Maus/Tastaturen direkt über USB, ansonsten kompatible Wireless Geräte (z.B. von LogiLink oder Rii u.a.);
handelsübliche HDMI-Monitore oder DVI-Monitore über HDMI-Adapter. Es gibt auch spezielle kleinere 3 - 7" HDMI Touchscreen-Monitore, z.B.
http://www.ebay.de/itm/281920007508?_tr ... EBIDX%3AIT
http://www.amazon.com/dp/B00Z9NN7OO
http://www.raspberrypiwiki.com/index.ph ... SKU:384212
http://www.ebay.de/sch/i.html?_odkw=102 ... i&_sacat=0

Raspberry Pi Accessory Kit.jpg
Zubehör-Set
Riii8.JPG
Riii8.JPG (9.32 KiB) 1407 mal betrachtet
7inch_1024x600_TFT_Pi.jpg
HDMI-TFT

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 21. Feb 2016 10:55

Verkabelung von Lego Encoder-Motoren per L293D H-Brücke:

L293D und/oder L298N Doppel-H-Bridge chip (alternativ, oder auch ähnliche) :
Bild Bild Bild




Schematische Verkabelung:
Raspi B+/2B Pin out diagram + Fritzing
RPi_GPIO_pinout.png
Rpi_L293D_LegoEncmotor2.jpg



ich verwende das folgende Pin-Setup für meine 2 Motoren (vgl. Foto):

Code: Alles auswählen

// Raspi motor pins, wiringPi numbering (in parenthesis: BCM numbering)
     motor[0].pinQa = 5;   // (BCM 24) change for rotation direction
     motor[0].pinQb = 4;   // (BCM 23) 
     motor[0].pind1 =24;   // (BCM 19)
     motor[0].pind2 =25;   // (BCM 26)
     motor[0].pinpwm=26;   // (BCM 12) pwm
     
     motor[1].pinQa = 0;   // (BCM 17) change for rotation direction
     motor[1].pinQb = 2;   // (BCM 27) 
     motor[1].pind1 =21;   // (BCM 5)
     motor[1].pind2 =22;   // (BCM 6)
     motor[1].pinpwm=23;   // (BCM 13) pwm   

driver code: viewtopic.php?f=78&t=8689&p=67780#p67780
Theoretisch könnten insgesamt 4 Motoren direkt am Raspi per 2x L293D/L298N Chips angeschlossen werden, dann aber unter Verzicht auf etliche besondere GPIO Pins mit speziellen Eigenschaften; da diese aber später noch gebraucht werden, werde ich weitere Motoren über Huckepack-Boards betreiben (z.B. über UART, USB oder I2C verbundene Arduinos).

Breadboards sind sehr gut im Experimentalstadium. Wenn man H-Brücken etc. aber auf Raspi-compatible Prototype-Shields mit 40-pol. Stacking Header montiert und verlötet/verkabelt, dann kann man nach meiner Erfahrung deutlich zuverlässige Steuerungen bauen als mit Breadboards, und außerdem kann es nicht zum ungewollten Herausrutschen von Verbindungskabeln kommen:

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

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 21. Feb 2016 10:55

Liste der GPIO Pin Settings: physikalisch (BCM):

Code: Alles auswählen

 1 (  )   +3.3                              2 (  )   +5
 3 ( 2)   i2c-1 SDA-1                       4 (  )   +5
 5 ( 3)   i2c-1 SCL-1                       6 (  )   GND
 7 ( 4)   <<< 1-Wire  <<<                   8 (14)   UART TX ArdMEGA
 9 (  )   GND                              10 (15)   UART RX ArdMEGA
11 (17)   motQa-m1                         12 (18)   < Bumper FLeft  >
13 (27)   motQb-m1                         14 (  )   GND
15 (22)   <           >                    16 (23)   motQa-m0
17 (  )   +3.3                             18 (24)   motQb-m0
19 (10)   <<< SPI MOSI  <<<                20 (  )   GND
21 ( 9)   <<< SPI MISO  <<<                22 (25)   <           >   
23 (11)   <<< SPI SCK   <<<                24 ( 8)   <<< SPI CE0 <<<
25 (  )   GND                              26 ( 7)   <<< SPI CE1 <<<
27 ( 0)   i2c-0 SDA-0                      28 ( 1)   i2c-0 SCL-0 
29 ( 5)   motd1-m1                         30 (  )   GND
31 ( 6)   motd2-m1                         32 (12)   motpwm-m0
33 (13)   motpwm-1                         34 (  )   GND
35 (19)   motd1-m0                         36 (16)   < Bumper FRight >
37 (26)   motd2-m0                         38 (20)   < Bumper Rear   >
39 (  )   GND                              40 (21)   < EMERGENCY STOP >


die mit "<<<" gekennzeichneten Pins stehen als optionale dPins zur Verfügung (digitalRead, digitalWrite, softPwmWrite), u.a. auch 1-Wire und SPI

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 21. Feb 2016 11:00

Demo-Code zum Ansteuern per L293-H-Bridge plus Encoder-Funktionen:

verwendet wird ein mobiler Roboter mit Differentialantrieb ( also vom Typ "Tribot" oder "Panzer" (engl.: "tank"),
aber natürlich hier jetzt mit Raspi statt NXT:
Bild
Dinobot.jpg

mit diesen API-Kommandos können die Motoren dann auf vorwärts, rückwärts, coast (rollen) und brake (bremsen) geschaltet werden:

Code: Alles auswählen

motorCoast( nr ) 
motorOn( nr,  motor_pwm )    // pwm==signed, also mit Richtungs-Signal, 0=coast
motorBrake( nr,  motor_pwm ) // brakes motor by adjustable pwm power 
 


Code: Alles auswählen

// Raspberry Pi Encoder Motor Control
//
// High Priority phread task for Encoder Timer
// H-Bridge control: direction + pwm (L293 type)
//
// ver 0012b

// (C) Helmut Wunder (HaWe) 2014, 2015, 2016
// freie Verwendung fuer private Zwecke
// fuer kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// Programming language: gcc  C/C++, Geany IDE
// protected under the friendly Creative Commons
// Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/


#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <stdint.h>
#include <errno.h>
#include <pthread.h>
#include <termios.h>

#include "VG/openvg.h"
#include "VG/vgu.h"
#include "fontinfo.h"
#include "shapes.h"

#include <wiringPi.h>
#include <wiringSerial.h>

#include <stdbool.h>
#include <termio.h>



#define  byte  uint8_t;

#define  MAXMOTORS    2   // max number of encoder motors

typedef struct  {   
               // electrical motor pins
      uint8_t  pind1, pind2, pinpwm;    // dir + pwm L293 H-Bridge type
      uint8_t  pinQa, pinQb;            // rotary enc pins Qa,Qb
      
               // pwm and encoder values
      int32_t  dirpwm;     
      int32_t  motenc, oldenc;          // rotary encoder values
} tEncMotor;


tEncMotor motor[MAXMOTORS];



/*************************************************************
* motor functions
*************************************************************/


#define motorCoast(nr) motorOn(nr, 0)          // alias for motor coast


inline void motorBrake(uint8_t nr, int dirpwm) {  // brake by pwm power
   int pwm;
   
   pwm = abs(dirpwm);
   
   digitalWrite(motor[nr].pind1, HIGH);
   digitalWrite(motor[nr].pind2, HIGH);      
   
   motor[nr].dirpwm = pwm;
   pwmWrite(motor[nr].pinpwm, pwm);  // brake power always > 0   
   
}



inline void motorOn(uint8_t nr, int dirpwm) { // motor On (nr, dir_pwm)
   int dir, pwm;                             // signed pwm:
                                             // pwm > 0: forward
   if(dirpwm>= 0) dir= +1;                   // pwm < 0: reverse
   else dir= -1;                             // pwm = 0: coast
   pwm = abs(dirpwm);
   
   if(dir> 0) {
      digitalWrite(motor[nr].pind1, HIGH);
      digitalWrite(motor[nr].pind2, LOW);      
   }
   else
   if(dir==0) {
      digitalWrite(motor[nr].pind1, LOW);
      digitalWrite(motor[nr].pind2, LOW);
   }
   else {
      digitalWrite(motor[nr].pind1, LOW);
      digitalWrite(motor[nr].pind2, HIGH);
   }
   motor[nr].dirpwm = dirpwm;
   pwmWrite(motor[nr].pinpwm, pwm);
   
   
}




/*************************************************************
* rpi_conio
*************************************************************/

bool kbhit(void)
{
    struct termios original;
    tcgetattr(STDIN_FILENO, &original);

    struct termios term;
    memcpy(&term, &original, sizeof(term));

    term.c_lflag &= ~ICANON;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);

    int characters_buffered = 0;
    ioctl(STDIN_FILENO, FIONREAD, &characters_buffered);

    tcsetattr(STDIN_FILENO, TCSANOW, &original);

    bool pressed = (characters_buffered != 0);

    return pressed;
}

//*************************************************************

void echoOff(void)
{
    struct termios term;
    tcgetattr(STDIN_FILENO, &term);

    term.c_lflag &= ~ECHO;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);
}

//*************************************************************

void echoOn(void)
{
    struct termios term;
    tcgetattr(STDIN_FILENO, &term);

    term.c_lflag |= ECHO;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);
}

                             
/*************************************************************
* Encoder Handler Routine
*************************************************************/

volatile int8_t ISRab[MAXMOTORS];

// 1/2 resolution
int8_t enctab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};

void updateEncoders() {   
  int i;    
  for( i=0; i<MAXMOTORS; ++i ) {   
     ISRab [ i] <<= 2;
     ISRab [ i] &=  0b00001100;
     ISRab [ i] |= (digitalRead( motor[ i].pinQa ) << 1) | digitalRead( motor[ i].pinQb );
     motor[ i].motenc += enctab[ISRab[ i] ];   
  }     

}

/*************************************************************
* pthread tasks
*************************************************************/

volatile static int8_t threadrun = 1;               
             

void* thread3Go(void *)   //  high priority encoder task
{
   while(threadrun) {
      updateEncoders();
      usleep(100);
   }         
   return NULL;
}

//*************************************************************

void* thread2Go(void *)  // higher priority motor control task
{
   while(threadrun) {
      
     for(int pwm = -1023; pwm < 1023; ++pwm ) {
      motorOn(0, pwm); 
      if (!threadrun) return NULL;
        delay(10);
     }
     for(int pwm = 1023; pwm > -1023; --pwm ) {
      motorOn(0, pwm); 
      if (!threadrun) return NULL;
        delay(10);
     }
   }         
   return NULL;
}

//*************************************************************

void* thread1Go(void *)   // medium priority task for keyboard monitoring
{
   char  sbuf[128];
   int   c;
   
   while(threadrun) {         
      c=0;
      if (kbhit())    {
          c = getchar();          // ESC to quit program 
          if( c==27 ) {
             threadrun=0;  // semaphore to stop all tasks
             printf("\n\n ESC pressed - program terminated by user \n\n");
             return NULL;
          }
      }
      delay(50);
   }         
   return NULL;
}

//*************************************************************

void* thread0Go(void *)  // low priority display task
{
   char  sbuf[128];
   
   while(threadrun) {
      sprintf(sbuf, " m0=%8ld  pwm=%6d     m1=%8ld  pwm=%6d \n ", 
         motor[0].motenc, motor[0].dirpwm,   motor[1].motenc, motor[1].dirpwm );
      printf(sbuf);
      delay(100);
   }         
   return NULL;
}




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

void setup() {
  int i;   
   
  // motor pin settings
  // encoder pin settings
  // setup for L293D motor driver
 
  // motor pins, wiringPi numbering (in parenthesis: BCM numbering)
 
     motor[0].pinQa = 5;   // (BCM 24) change for rotation direction
     motor[0].pinQb = 4;   // (BCM 23) change for rotation direction
     motor[0].pind1 =24;   // (BCM 19)
     motor[0].pind2 =25;   // (BCM 26)
     motor[0].pinpwm= 1;   // (BCM 18) hardware pwm
     
     motor[1].pinQa = 0;   // (BCM 17) change for rotation direction
     motor[1].pinQb = 2;   // (BCM 27) change for rotation direction
     motor[1].pind1 =21;   // (BCM 5)
     motor[1].pind2 =22;   // (BCM 6)
     motor[1].pinpwm=23;   // (BCM 13) hardware pwm   
     
     
     for( i=0; i< MAXMOTORS; ++i)  {       
        pinMode(motor[i].pinQa, INPUT);        // encA   
        pinMode(motor[i].pinQb, INPUT);        // encB   
        pinMode(motor[i].pind1, OUTPUT);       // dir-1   
        pinMode(motor[i].pind2, OUTPUT);       // dir-2   
        pinMode(motor[i].pinpwm ,PWM_OUTPUT);  // pwm
       
        motor[i].motenc = 0;
        motor[i].oldenc = 0;
        ISRab[i] = 0;
   }
}



/*************************************************************
* main
*************************************************************/


int main() {
    char  sbuf[128];
    int ioerr;
    pthread_t thread0, thread1, thread2, thread3;
    struct  sched_param  param;
   
    ioerr = wiringPiSetup();   
    if( ioerr == -1 ) return 1;   
   
    setup();
   
    pthread_create(&thread0, NULL, thread0Go, NULL);     // lowest priority task: screen output
    param.sched_priority = 10;
    pthread_setschedparam(thread0, SCHED_RR, &param);
   
    pthread_create(&thread1, NULL, thread1Go, NULL);     // medium priority task: keyboard monitoring (stop program)
    param.sched_priority = 25;
    pthread_setschedparam(thread1, SCHED_RR, &param);
   
    pthread_create(&thread2, NULL, thread2Go, NULL);     // higher priority task: !!! motor control program !!!
    param.sched_priority = 50;
    pthread_setschedparam(thread2, SCHED_RR, &param);
   
    pthread_create(&thread3, NULL, thread3Go, NULL);     // highest priority task: encoder reading     
    param.sched_priority = 90;
    pthread_setschedparam(thread3, SCHED_RR, &param);
   
   
    pthread_join(thread0, NULL);
    pthread_join(thread1, NULL);
    pthread_join(thread2, NULL);
    pthread_join(thread3, NULL);
   
    for(int i=0; i< MAXMOTORS; ++i)  {
       motorOn(i, 0);
    }
   
    delay(1000);
   
    exit(0);
   
}


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

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 22. Feb 2016 17:22

optionales Motortreiber HAT:
Pololu Dual MC33926 Motortreiber for Raspberry Pi


Bild

ein alternatives, sehr leistungsstarkes und kompaktes Motortreiber HAT mit einer etwas anderen Pin-Belegung und Steuer-Logik:

http://www.ebay.de/itm/272182305548?_tr ... EBIDX%3AIT



Code: Alles auswählen

MC33926
http://www.ebay.de/itm/272182305548?_trksid=p2060353.m2749.l2649&ssPageName=STRK%3AMEBIDX%3AIT

Default pin mappings

This table shows how the Raspberry Pi’s GPIO pins are used to interface with the motor drivers:
---------------------------------------------------------------------------------------------------------
RPi
GPIO     Motor driver pin    Description
---------------------------------------------------------------------------------------------------------
5    Motor 1 SF              Status flag output: When the driver is functioning normally,
                                this pin should be pulled high by the Raspberry Pi.
                                In the event of a driver fault , the driver IC drives SF low.
                                If either of the disable pins (D1 or D2) is disabling the outputs,
                                SF will also be low.
6    Motor 2 SF              dto.
---------------------------------------------------------------------------------------------------------
12    Motor 1 PWM             Motor speed input: A PWM (pulse-width modulation) signal on this pin
                                corresponds to a PWM output on the corresponding driver’s motor outputs.
                                When this pin is low, the motor brakes low.
                                When it is high, the motor is on.
                                The maximum allowed PWM frequency is 20 kHz.
13    Motor 2 PWM             dto.
---------------------------------------------------------------------------------------------------------
22    Motor 1 EN              Enable input: This pin is internally pulled low,
                                putting the motor driver IC into a low-current sleep mode
                                and disabling the motor outputs (setting them to high impedance).
                                EN must be driven high to enable the motor driver.
23    Motor 2 EN              dto.
---------------------------------------------------------------------------------------------------------
24    Motor 1 DIR             Motor direction input: When DIR is low, motor current flows
                                from output A to output B;
                                when DIR is high, current flows from B to A.
25    Motor 2 DIR             dto.
---------------------------------------------------------------------------------------------------------
 
Simplified motor control truth table

This table shows how the drivers’ control inputs affect the motor outputs:
---------------------------------------------------------------------------------------
Inputs                             Outputs
---------------------------------------------------------------------------------------
EN      DIR        PWM        MxA       MxB       operating mode
---------------------------------------------------------------------------------------
1        0         PWM      PWM(H/L)     L        forward/brake at speed PWM %
1        1         PWM         L      PWM(H/L)    reverse/brake at speed PWM %
1        X          0          L         L        brake low (outputs shorted to ground)
0        X          X          Z         Z        coast (outputs off)
---------------------------------------------------------------------------------------


hierfür neue Pinbelegung:

Code: Alles auswählen

/**************************************************************
 
 * NEW pin setup (>< mögliche künft. Konflikte mit BP3, ggf. ändern)
 *
 1 (  )   +3.3                              2 (  )   +5
 3 ( 2)   i2c-1 SDA-1                       4 (  )   +5
 5 ( 3)   i2c-1 SCL-1                       6 (  )   GND
 7 ( 4)   <<< 1-Wire  <<<                   8 (14)   UART TX ArdMEGA
 9 (  )   GND                              10 (15)   UART RX ArdMEGA
11 (17)   motQa-1                          12 (18)   Bumper FLeft   >< BP3 SAMDRES
13 (27)   motQb-1                          14 (  )   GND
15 (22)   motd1-m0 (en)                    16 (23)   motd1-m1 (en)
17 (  )   +3.3                             18 (24)   motd2-m0 (dir) >< BP3 FWUPD   
19 (10)   SPI MOSI  BP3                    20 (  )   GND
21 ( 9)   SPI MISO  BP3                    22 (25)   motd2-m1 (dir) >< BP3 FWUPD
23 (11)   SPI SCK   BP3                    24 ( 8)   SPI CE0
25 (  )   GND                              26 ( 7)   SPI CE1 BP3 
27 ( 0)   i2c-0 SDA-0                      28 ( 1)   i2c-0 SCL-0
29 ( 5)   motSF-m0   <<<                   30 (  )   GND
31 ( 6)   motSF-m1   <<<                   32 (12)   motpwm-m0
33 (13)   motpwm-m1                        34 (  )   GND
35 (19)   motQa-m0                         36 (16)   < Bumper FRight >
37 (26)   motQb-m0                         38 (20)   < Bumper rear >
39 (  )   GND                              40 (21)   < EMERGENCY STOP >
 
*/


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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 22. Feb 2016 17:23

Fernbedienung per UART mit einem Arduino-DUE Board einrichten:


Arduino DUE Board:
http://www.arduino.org/products/boards/4-arduino-boards/arduino-due
Bild


UART vorbereiten: viewtopic.php?f=78&t=8689&start=15#p67813


Arduino per UART mit Raspi verbinden: viewtopic.php?f=78&t=8689&start=15#p67815

Testweise mit Kabelverbindung:
für DUE: einfach
Arduino GND <-> Raspi GND
Arduino RX0 <-> Raspi TX
Arduino TX0 <-> Raspi RX
- fertig!


Drahtlosverbindung über zwei HC-05 Module:

HC05-1.jpeg

HC05-2.jpeg

HC05-3.jpeg


Quelle: http://www.ebay.de/itm/Bluetooth-Modul-HC-05-Master-Slave-UART-fur-z-B-Arduino-inkl-Adapterboard-/291672244026?hash=item43e9053f3a:g:DlwAAOSwabhUVUBi
(Herr Niesen ist sehr hilfsbereit bezüglich Rückfragen und Konfigurationsproblemen!)


Achtung: verträgt nur 3,3V UART Signal-Level, nicht 5V-kompatibel!
Spannungsversorgung dagegen ist immer 5V!
D.h.: mit Raspi und Arduino DUE läuft es direkt, für Arduino MEGA werden 5V -> 3,3V Levelshifter benötigt!

Durchführung:
1 HC-05 Modul als Master konfigurieren, das andere als Slave.
BAUD Rate richtig konfigurieren (ich verwende durchweg 115200 baud).
Dann diese beiden Module miteinander pairen und die UART-RX/TX Pins über Kreuz mit Raspi bzw. Arduino verbinden:
HC-05 <-> 5V Vc und GND
Arduino-Seite:
Arduino GND <-> HC-05 GND
Arduino RX0 <-> HC-05 TX
Arduino TX0 <-> HC-05 RX
Raspi-Seite:
HC-05 GND <-> Raspi GND
HC-05 RX <-> Raspi TX
HC-05 TX <-> Raspi RX
Keine weiteren Einstellungen nötig - läuft automatisch als wäre ein langes Kabel dazwischen.

Links z.B.:
http://www.ebay.de/itm/Bluetooth-Modul-HC-05-Master-Slave-UART-fur-z-B-Arduino-inkl-Adapterboard-/291672244026?hash=item43e9053f3a:g:DlwAAOSwabhUVUBi
http://www.brunwinkel.de/elektronik/module/btm400_6b/
http://www.exp-tech.de/serial-port-bluetooth-module-master-slave-hc-05

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 22. Feb 2016 17:45

Definition eines Kommunikations-Arrays für Fernsteuerung und Empfangen von zurückgesendeten Telemetrie-Daten :


Code: Alles auswählen

const    uint8_t  MSGSIZE=64;  // array size for messages to send and receive
uint8_t  bsync= 0xff,  // transmission control bytes: begin of msg signal : bsync=0xff (255)
         esync= 0xfe;  // end of msg signal: esync=0xfe (254)

uint8_t  sendbuf[MSGSIZE];  // send array
uint8_t  recvbuf[MSGSIZE];  // receive array
uint8_t  currbuf[MSGSIZE];  // array buffer
uint8_t  dispbufs[MSGSIZE], dispbufr[MSGSIZE]; // intermediate s+r buffers for display update

// message array setup:

#define SYNCSLOT      0  // start sync signal of this Msg: bsync=0xff (255)
#define CKSSLOT       1  // chksum this Msg
#define BYTE0         2  // byte 0     // byte: 8-bit => 8x digital bits 0-7
#define BYTE1         3  // byte 1     // byte: 8-bit => 8x digital bits 8-15
#define ENC0          4  // motorenc 0 // 10x encoders: 32-bit
#define ENC1          8  // motorenc 1
#define ENC2         12  // motorenc 2
#define ENC3         16  // motorenc 3
#define ENC4         20  // motorenc 4
#define ENC5         24  // motorenc 5
#define ENC6         28  // motorenc 6 
#define ENC7         32  // motorenc 7
#define ENC8         36  // motorenc 8
#define ENC9         40  // motorenc 9
#define ANA0         44  // analog 0   // 9x analog: 16-bit
#define ANA1         46  // analog 1   // analog 0+1 = joystick for drive
#define ANA2         48  // analog 2
#define ANA3         50  // analog 3
#define ANA4         52  // analog 4
#define ANA5         54  // analog 5
#define ANA6         56  // analog 6
#define ANA7         58  // analog 7
#define ANA8         60  // analog 8
#define BYTE2        62  // byte 2     // byte: 8-bit => 8x digital bits 16-23
#define TERM         63  // terminating byte

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

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 26. Feb 2016 09:30

Pinsetup für die Arduino Fernsteuerungs-Konsole:
analoge Eingänge (Joystick-Potis) digitale Eingänge (Buttons, Schalter und Keypad) und TFT Display:


angeschlossen werden an den Arduino als Remote-Konsole zunächst:

1 Joystick aus 2 analogen Potis plus 1 Druck-Button:
Bild
viewtopic.php?f=78&t=8491&start=30#p68976
beide Potis (X+Y) an Arduino A0 +A1,
codiert in message array ANA0 + ANA1
1 Joystick-Button: an Arduino dPin: 30
codiert in message array BYTE0, Bit0
Zweck:
die vom Arduino ermittelten Steuerwerte sollen simultan die Raspi-Encodermotoren steuern.
Im Gegensatz zur Lego-Fernsteuerung ist es ein analoger Joystick, der Werte "sanft" und propotional zur Auslenkung übermittelt, nicht nur an/aus.


2 weitere normale Button switches:
Bild
dPins: an Arduino dPin 35+dPin 36
codiert in message array BYTE0, Bits 5+6
Zweck: zunächst nur zu Testzwecken.


4x5 Keypad mit insg. 9 dPins
keypad4x5.jpeg

viewtopic.php?f=78&t=8491&start=30#p68975
Arduino rowPins[ROWS] = {37, 38, 39, 40}
Arduino colPins[COLS] = {41, 42, 43, 44, 45}
codiert in message array BYTE0_BYTE1, Bits 7-15
Zweck: zunächst nur zu Testzwecken.


1 Display ILI9341
Bild Bild
viewtopic.php?f=78&t=8491#p66189
Pins: Arduino SPI (74,75,76), Arduino tft_cs=48, tft_dc== 49 (nur lokal, nicht für message array)
Driver: ILI9341_due: https://github.com/marekburiak/ILI9341_due
Zweck:
Anzeige der Telemetriewerte, die vom Raspi zurückgesendet werden.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 26. Feb 2016 19:46

Pinsetup für den Raspi :
Motor H-Brücken für 2 Motoren, IMU CMPS11, LEDs mit Vorwiderständen:



Motor-H-Brücken:
verwendet werden H-Brücken vom Typ L293D oder L298N oder andere mit identischer Pin-Ansteuerung (2x dir, 1x enable(pwm) )
siehe oben: viewtopic.php?f=78&t=8851&p=68945#p68945
driver code: http://www.mindstormsforum.de/viewtopic.php?f=78&t=8689&p=67780#p67780 (ggf scrollen!)
Funktion: die Motoren werden über die Poti-Steuerwerte des Arduinos mit der entsprechenden Richtung und Stärke ferngesteuert.
Über die Encoderwerte soll zusammen mit den IMU-Gyro-Werten die gefahrene Route und die aktuelle Position auf dem Raspi ermittelt und auf dem Display angezeigt werden. Die Position soll an die Fernsteuerung zurückgesendet und auch dort auf dem Display angezeigt werden.




IMU
CMPS11 (hier per I2C)

Bild
driver code: viewtopic.php?f=78&t=8689&p=67780#p68268
Raspi I2C-1: GPIO pins
SDA-1 wiringPi 8 (BCM 2)
SCL-1 wiringPi 9 (BCM 3)
Funktion: zusammen mit den Encoderwerten soll die gefahrene Route und die aktuelle Position auf dem Raspi ermittelt und auf dem Display angezeigt werden. Die Position soll an die Fernsteuerung zurückgesendet und auch dort auf dem Display angezeigt werden.



3 LEDs
mit 470 Ohm Vorwiderständen

gpio-led-pi2.png

driver code: viewtopic.php?f=78&t=8689&p=67785#p67785
an Pins wiringPi 27,28,29 (BCM 16,20,21)
Raspi Pin(+) -> LED -> R_470Ω -> GND
Funktion: für Testzwecke.

auch andere künftige Sensorwerte, die vom Raspi kommen (z.B. Distanz- oder Stoß-Sensoren), sollen auch als Telemetriewerte an die Fernbedienung zurück übermittelwerden.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 27. Feb 2016 23:52

Beispiele, um noch Lego Sensoren anzuschließen:

Pin-Belegung für die Verwendung von Lego Mindstorms RJ11-Steckern
Schema speziell für die NXT-Sensoren; Motor-Antrieb auf pins 1+2 (weiß+schwarz), Encoder auf pins 5+6 (gelb + blau):
Bild


Lego Berührungs-/ Touch Sensor (ADC):
Bild
s.a.: http://www.dexterindustries.com/howto/l ... pberry-pi/

Verkabelung:

pins 2+3 verbinden, dann --> 1kOhm --> GND
pin 4 --> +3,3V
pin 1 --> GPIO (z.B. GPIO pin BCM_18 )
Testcode etc. s.a. hier: viewtopic.php?f=78&t=8689&p=69023#p69178


Lego Licht / Light Sensor (ADC):
Der Raspi hat keinen Analog-Eingang (ADC), daher Anschluss über das Arduino Muxer board oder einen PCF8591!
Bild Bild


Lego Ultraschall / Ultrasonic Sensor (I2C):
http://2.bp.blogspot.com/-ab3H52TkIEI/V ... dboard.jpg

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

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 28. Feb 2016 19:29

Fernsteuerungskonsole (Baustelle):
Bild
rpiremote_1.jpg
Fernsteuerungskonsole mit Arduino
und mobiler Spannungsversorgung


Testaufbau der Raspi-Robotersteuerungs-Platine
(Backpane Mitte/oben rechts im Bild neben Monitor):
rpiremote_test2_20160312.jpg
Raspi-Backpane rechts oben neben Bildschirm mit debug-Screens:
auf dem Backpane links unten Raspi,
auf dem Backpane links oben T-Cobbler,
auf dem Backpane rechts Motor-Treiber Platinen
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

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

Re: Projekt: Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 2. Mär 2016 20:14

Fernsteuerungs-Sender: Code für den Arduino (Remote Control Console):

Code: Alles auswählen

/*   Arduino Remote Console     
 *   UART / Bluetooth HC-05 (master)
 *   Arduino Due
 *   IDE 1.6.5
 *   
 *   version RCsender0013
 * 
 * change log:
 *   "heartbeat"
 *   
 */
 
// (C) Helmut Wunder (HaWe) 2015
// freie Verwendung für private Zwecke
// für kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// Programming language: Arduino Sketch C/C++ (IDE 1.6.1 - 1.6.5)
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/

 
#include <SPI.h>
#include <Scheduler.h>


// SPI header            //             |--|
// SPI pins Due          //  -----------|  |------------
  #define DUEMISO     74 //  |  RES   76_SCK   74_MISO |
  #define DUEMOSI     75 //  | -GND   75_MOSI   Vc+5V  |
  #define DUESCK      76 //  --------------------------

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



//=====================================================================================
// TFT LCD
//=====================================================================================

#define   UTFT_SmallFont     8 // UTFT 8x10
#define   UTFT_MediumFont   12 // UTFT ++
#define   UTFT_BigFont      18 // UTFT +++
#define   _SmallFont_        1 // 9341 6x9
#define   _MediumFont_       2 // 9341 12x16
#define   _BigFont_          3 // 9341 18x23

int16_t  LCDmaxX , LCDmaxY ;                // display size
int16_t  _curx_, _cury_,                    // last x,y cursor pos on TFT screen
         _maxx_, _maxy_;                    // max. x,y cursor pos on TFT screen       



// set LCD TFT type
int16_t  LCDTYPE   =   -1;

#define  _LCD1602_    1  // LCD1602  Hitachi HD44780 driver <LiquidCrystal.h>
                           // http://www.arduino.cc/en/Tutorial/LiquidCrystal   //
#define  _UTFT_       4  // Henning Karlsen UTFT 2.2-2.4" 220x176 - 320x240 lib
                           // http://henningkarlsen.com/electronics/library.php?id=51   //
#define _ILI9341_     8  // https://github.com/adafruit/Adafruit_ILI9340
                           // https://github.com/adafruit/Adafruit-GFX-Library //
#define _ILI9341due_  9  // ILI9341_due NEW lib by Marek Buriak
                           // http://marekburiak.github.io/ILI9341_due/ //
                           
//--------------------------------------------------------------------------------------------------

#define   tft_rst      0
#define   tft_cs      48
#define   tft_dc      49
#define   _MEGAMISO_  50   
#define   _MEGAMOSI_  51
#define   _MEGASCK_   52

               
                         
//=====================================================================================
// UTFT Henning Karlsen
//=====================================================================================
#include <UTFTQD.h>  // patch for QD220A

// UTFT  qdUTFT(Model,  SDA=MOSI,   SCL,      CS,     RESET,   RS)    // Due: 3 exposed SS pins: 4,10,52
// UTFT  qdUTFT(QD220A,   A2,       A1,       A5,      A4,     A3);   // adjust model parameter and pins!
   UTFT  qdUTFT(QD220A,   23,       22,       26,      25,     24);   //
// UTFT  qdUTFT(QD220A, _MEGAMOSI_,_MEGASCK_, tft_cs, tft_rst, 47);   // A0->Vc (LED), A4->BoardReset
 extern  uint8_t SmallFont[];
 
//=====================================================================================
// TFT Adafruit LIL9340/ILI9341
//=====================================================================================
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

Adafruit_ILI9341  tft = Adafruit_ILI9341(tft_cs, tft_dc, tft_rst);


//=====================================================================================
// TFT ILI9341_due // http://marekburiak.github.io/ILI9341_due/ //
//=====================================================================================
#include <ILI9341_due_config.h>
#include <ILI9341_due.h>
#include <SystemFont5x7.h>

ILI9341_due      dtft = ILI9341_due(tft_cs, tft_dc);

// Color set
#define  BLACK          0x0000
#define RED             0xF800
#define GREEN           0x07E0
//#define BLUE            0x001F
#define BLUE            0x102E
#define CYAN            0x07FF
#define MAGENTA         0xF81F
#define YELLOW          0xFFE0
#define ORANGE          0xFD20
#define GREENYELLOW     0xAFE5
#define DARKGREEN       0x03E0
#define WHITE           0xFFFF

uint16_t  color;

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

#define  lcdWhiteBlack()  {                                                                 \
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(255,255,255); qdUTFT.setBackColor(  0,  0,  0);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, BLACK) ;} \
}

#define  lcdNormal()      {                                                                 \
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(255,255,255); qdUTFT.setBackColor(  0,  0,  0);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, BLACK) ;} \
}

#define  lcdInvers()      {                                                                 \
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(  0,  0,  0); qdUTFT.setBackColor(255,255,255);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(BLACK, WHITE) ;} \
}

#define  lcdWhiteRed()    {                                                                 \
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(255,255,255); qdUTFT.setBackColor(255,  0,  0);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_WHITE, ILI9341_RED) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, RED) ;} \
}

#define  lcdRedBlack()    {                                                                 \   
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(255,  0,  0); qdUTFT.setBackColor(  0,  0,  0);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_RED, ILI9341_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(RED, BLACK) ;} \
}

#define  lcdYellowBlue()  {                                                                 \     
   if(LCDTYPE==_UTFT_)      { qdUTFT.setColor(255,255,  0); qdUTFT.setBackColor( 64, 64, 64);} \
   else if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9341_YELLOW, ILI9341_BLUE);} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(YELLOW, BLUE);} \
}



int16_t  fontwi= 8;  // default
int16_t  fonthi=10;  // default


void putfonttype(uint8_t fsize) {
  if(LCDTYPE==_UTFT_)  {
    fontwi= qdUTFT.getFontXsize();
    fonthi= qdUTFT.getFontYsize();
  }
  else
  if(LCDTYPE==_ILI9341_) {
     if(fsize==_SmallFont_)     { fontwi= 6; fonthi=9; }  // 5x7 + overhead
     else
     if(fsize==_MediumFont_)    { fontwi=12; fonthi=16; } // ?
     else
     if(fsize==_BigFont_)       { fontwi=18; fonthi=23; } // ?
  }
  else
  if(LCDTYPE==_ILI9341due_) {
     if(fsize==_SmallFont_)     { fontwi= 6; fonthi=9; }  // 5x7 + overhead
  }
  _maxx_ = LCDmaxX / fontwi;    // max number of letters x>>
  _maxy_ = LCDmaxY / fonthi;    // max number of letters y^^
}


void setlcdorient(int16_t orient) {
 
  if(LCDTYPE==_ILI9341_) {
      tft.setRotation(orient);
      LCDmaxX=tft.width();
      LCDmaxY=tft.height();       
   }
   else
   if(LCDTYPE==_ILI9341due_) {
      dtft.setRotation( (iliRotation)orient);
      LCDmaxX=dtft.width();
      LCDmaxY=dtft.height();   
   }
     
}

void lcdcls()  {                                                         
   if(LCDTYPE==_UTFT_)       { qdUTFT.clrScr();                }
   else
   if(LCDTYPE==_ILI9341_)    { tft.fillScreen(ILI9341_BLACK);  }
   else
   if(LCDTYPE==_ILI9341due_) { dtft.fillScreen(BLACK);  }
   _curx_ =0;  _cury_ =0;
}

void curlf()   {                                                       
   _curx_=0;
   if( _cury_ <=(LCDmaxY-10) ) _cury_+=fonthi;
   else _cury_=0;
     
   if(LCDTYPE==_ILI9341_)    { tft.setCursor(0, _cury_); }
   else
   if(LCDTYPE==_ILI9341due_) { dtft.cursorToXY(0, _cury_); }
}



void curxy(int16_t  x,  int16_t  y) {
   _curx_ = x;
   _cury_ = y;
   if(LCDTYPE==_ILI9341_)      {tft.setCursor(x, y); }
   else
   if(LCDTYPE==_ILI9341due_)   {dtft.cursorToXY(x, y); }
}



void lcdprintxy(int16_t x, int16_t y, char * str) {
   if(LCDTYPE==_UTFT_)          {
     qdUTFT.print(str,x,y);
     _curx_=x+strlen(str)*fontwi;
     _cury_=y;
   }
   else if(LCDTYPE==_ILI9341_)  {
      tft.setCursor(x,y);     
      tft.print(str);
      _curx_=tft.getCursorX();
      _cury_=tft.getCursorY();
   }
   else if(LCDTYPE==_ILI9341due_)  {
      dtft.cursorToXY(x,y);     
      dtft.printAt(str, x, y);
      _curx_= x + strlen(str)*fontwi;
      _cury_= y;
   }
}


void lcdprint(char * str) {
    if(LCDTYPE==_UTFT_)     {
      qdUTFT.print(str, _curx_, _cury_);
      _curx_=_curx_+strlen(str)*fontwi;
    }
    else if(LCDTYPE==_ILI9341_)  {
       tft.setCursor(_curx_, _cury_);
       tft.print(str);
       _curx_=tft.getCursorX();
       _cury_=tft.getCursorY();
    }
    else if(LCDTYPE==_ILI9341due_)  {
       dtft.cursorToXY(_curx_, _cury_);
       dtft.printAt(str, _curx_, _cury_ );
       _curx_ = _curx_ + (strlen(str)+3)*fontwi;
    }
}



void initLCD(uint8_t orientation) { // 0,2==Portrait  1,3==Landscape
   if(LCDTYPE==_UTFT_) {
      qdUTFT.InitLCD(orientation%2);
      LCDmaxX=qdUTFT.getDisplayXSize();
      LCDmaxY=qdUTFT.getDisplayYSize();
      qdUTFT.setFont(SmallFont);
      putfonttype(UTFT_SmallFont);
   }
   else
   if(LCDTYPE==_ILI9341_) {
      tft.begin();
      setlcdorient(orientation);       
      tft.setTextSize(_SmallFont_);
      putfonttype(_SmallFont_);
   }
   else
   if(LCDTYPE==_ILI9341due_) {
      dtft.begin();
      setlcdorient(orientation);       
      dtft.setFont(SystemFont5x7);
      putfonttype(_SmallFont_);
   }
}







//=====================================================================================
// UART DATA PACKAGES
//=====================================================================================
const    uint8_t  MSGSIZE   = 64;
const    uint32_t UARTclock = 115200;
uint8_t  bsync= 0xff;  // transmission control bytes: begin of msg signal : bsync=0xff (255)


volatile static uint8_t _heartbeat_ = 0;

#define  MAXDIGPINS   16
#define  MAXANALOG     9
#define  MAXMOTORS    10

uint8_t  sendbuf[MSGSIZE];
uint8_t  recvbuf[MSGSIZE];
uint8_t  sdispbuf[MSGSIZE];
uint8_t  rdispbuf[MSGSIZE];

// message array setup:

#define SYNCSLOT      0  // start sync signal of this Msg: bsync=0xff (255)
#define CKSSLOT       1  // chksum this Msg
#define BYTE0         2  // byte 0     // byte: 8-bit => 8 digital bits 0-7
#define BYTE1         3  // byte 1     // byte: 8-bit => 8 digital bits 8-15
#define ENC0          4  // motorenc 0 // 10 encoders: 32-bit
#define ENC1          8  // motorenc 1
#define ENC2         12  // motorenc 2
#define ENC3         16  // motorenc 3
#define ENC4         20  // motorenc 4
#define ENC5         24  // motorenc 5
#define ENC6         28  // motorenc 6
#define ENC7         32  // motorenc 7
#define ENC8         36  // motorenc 8
#define ENC9         40  // motorenc 9
#define ANA0         44  // analog 0   // 9 analog: 16-bit
#define ANA1         46  // analog 1   // analog 0+1 = joystick for drive
#define ANA2         48  // analog 2
#define ANA3         50  // analog 3
#define ANA4         52  // analog 4
#define ANA5         54  // analog 5
#define ANA6         56  // analog 6
#define ANA7         58  // analog 7
#define ANA8         60  // analog 8
#define BYTE2        62  // byte 2     // byte: 8-bit => 8 digital bits 16-23
#define TERM         63  // terminating: heart beat signal


// motor runstates:

#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



int8_t   digval[MAXDIGPINS];
uint16_t digvalraw=0;
int16_t  anaval[MAXANALOG];
int32_t  motenc[MAXMOTORS],  oldenc[MAXMOTORS] ;


// buffer for motor commands
uint8_t   _motorstate_[7];      // 0: MOTORSLOT: MotNr
                                // 1: mot_runstate
                                // 2: pwm
                                // 3+4: mot enc_int16
                                // 5+6: mot enc_int32
                               



//*************************************************************************************
//*************************************************************************************
// ARDUINO GPIO PIN SETUP
//*************************************************************************************
//*************************************************************************************


//=====================================================================================
// MOTORS
//=====================================================================================
// NA

//=====================================================================================
//  analog Pins
//=====================================================================================
// A0, A1: Jstick_0 (remote tank propulsion drive)

//=====================================================================================
//  digital  Pins
//=====================================================================================
// 12 dPins R/W (free) :  D02-D13
// 8 dPins R/W 16-23 :    D22-D29
// 16 dPins R/W 0-15 :    D30-D45
//    7 button pins: D30-D36
//    9 keypad pins: D37-D45
// 2 dPins (free) :       D46-D47
// 2 TFT cs+dc pins:      D48+D49
// SPI (Mega):            D50-D52
// SPI (Due):             D74-D76
// SD cs:                 D53

#define  joystBtn_0       30
   
void setupdPins() {
   int i;   
   for (i=22; i<29; ++i) {pinMode(i, INPUT_PULLUP);}
   for (i=30; i<45; ++i) {pinMode(i, INPUT_PULLUP);}
 
}




//************************************************************************************
// bit and byte and pin operations
//************************************************************************************
// convert byte arrays to int

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

inline long  ByteArrayToInt32(uint8_t  array[], uint8_t  slot) {
  return ( (array[slot+3]<<24) + (array[slot+2]<<16) + (array[slot+1]<<8) + array[slot] );
}

//------------------------------------------------------------------------------
// convert int to byte arrays

inline void Int16ToByteArray(int16_t vint16,  uint8_t *array,  uint8_t slot) {
  memcpy(array+slot*sizeof(char), &vint16, sizeof(int16_t));    // copy int16 to array
}

inline void Int32ToByteArray(int32_t vint32,  uint8_t *array,  uint8_t slot) {
  memcpy(array+slot*sizeof(char), &vint32, sizeof(int32_t));    // copy int32 to array
}

inline void FloatToByteArray(float vfloat32,  uint8_t *array,  uint8_t slot) {   
  memcpy(array+slot*sizeof(char), &vfloat32, sizeof(float));    // copy float to array
}

//------------------------------------------------------------------------------
// read+write bits in numbers
/*
#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) )
*/


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

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


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

#define sensortouch(pinHIGH) !digitalRead(pinHIGH)

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







//*************************************************************************************
//*************************************************************************************
// UART TRANSMISSION CONTROL
//*************************************************************************************
//*************************************************************************************


//=====================================================================================
// CHECKSUM
//=====================================================================================

uint8_t calcchecksum(uint8_t array[]) {
  int32_t  sum=0;
  for(int i=2; i<MSGSIZE; ++i) sum+=(array[i]);
  return (sum & 0x00ff);
}

bool checksumOK(uint8_t array[]){
return (calcchecksum(array)==array[1]);
}


//=====================================================================================
// UART SEND/RECEIVE BUFFER
//=====================================================================================
// addToBuffer and receive function courtesy of chucktodd

bool addToBuffer( uint8_t buf[], uint8_t *cnt, uint16_t timeout){
bool inSync = *cnt>0;
unsigned long start=millis();
while((*cnt<MSGSIZE)&&(millis()-start<timeout)){
  if(Serial1.available()){ // grab new char, test for sync char, if so start adding to buffer
    buf[*cnt] = (uint8_t)Serial1.read();
    if(inSync) *cnt += 1;  // my origional *cnt++ was updating the pointer address, not
                           // the pointed to sendbuffer
    else{
     if(buf[*cnt]==0xFF){
       inSync = true;
       *cnt +=1;
       }
     }
    }
  }
return (*cnt==MSGSIZE);
}


//=====================================================================================

bool receive(uint8_t * buf, uint16_t timeout, uint8_t *cnt){ // by passing cnt in and out,
// i can timeout and still save a partial buffer, so a resync costs less (less data lost)

bool inSync=false;
unsigned long start=millis();
uint8_t * p;  // pointer into buf for reSync operation
bool done=false;

  do
  {
    done = addToBuffer(buf,cnt,timeout); // if this return false, a timeout has occured, and the while will exit.
    if(done){ // do checksumOK test of buffer;
      done=checksumOK(buf);
      if (!done){// checksumOK failed, scan buffer for next sync char
         p = (uint8_t*)memchr((buf+1),0xff,(MSGSIZE-1)); //forgot to skip the current sync at 0
       
       
         if(p){ // found next sync char, shift buffer content, refill buffer
           *cnt = MSGSIZE -(p-buf); // count of characters to salvage from this failure
           memcpy(buf,p,*cnt); //cnt is now where the next character from Serial is stored!
           }
         else *cnt=0; // whole buffer is garbage
         }
      }
   
  } while(!done&&(millis()-start<timeout));

  return done; // if done then buf[] contains a sendbufid buffer, else a timeout occurred
}




//=====================================================================================
// acquire remote control values for send buffer
//=====================================================================================
int16_t  analogTmpx=0, analogTmpy=0;
int16_t  throttle, direction = 0; //throttle (Y axis) and direction (X axis)

int16_t  leftMotor,leftMotorScaled = 0;

float    leftMotorScale = 0;

int16_t  rightMotor,rightMotorScaled = 0; //right Motor helper variables
float    rightMotorScale = 0;

float    maxMotorScale = 0; //holds the mixed output scaling factor

const byte joyst_0XA = A0; //Analog Jostick X axis  // Joyst Anschluesse links
const byte joyst_0YA = A1; //Analog Jostick Y axis

//=====================================================================================

void getremotevalues()
{   
   char sbuf [128];
   int16_t anabuf;
   int8_t  digbuf;
 
  //aquire the analog input for Y  and rescale the 0..1023 range to -255..255 range
  analogTmpx = 1024 - analogRead(joyst_0YA);   
  throttle =  (512-analogTmpx)/2;   
  //...and  the same for X axis
  analogTmpy = 1024- analogRead(joyst_0XA);
  direction  =  (512-analogTmpy)/2;
  sprintf(sbuf, "x:%6d, y:%6d ", analogTmpx, analogTmpy);  Serial.print(sbuf);

  //mix throttle and direction
  leftMotor  =  -throttle + direction;   
  rightMotor =  -throttle - direction;
  //print the initial mix results
  sprintf(sbuf, "LIN:%6d, RIN:%6d ", leftMotor, rightMotor);  Serial.print(sbuf);

  //calculate the scale of the results in comparision base 8 bit PWM resolution
  leftMotorScale =  leftMotor/255.0;
  leftMotorScale = abs(leftMotorScale);
  rightMotorScale =  rightMotor/255.0;
  rightMotorScale = abs(rightMotorScale);
  sprintf(sbuf, "| LSCALE:%4.1f, RSCALE:%4.1f ", leftMotorScale, rightMotorScale);  Serial.print(sbuf);

  //choose the max scale value if it is above 1
  maxMotorScale = max(leftMotorScale,rightMotorScale);
  maxMotorScale = max(1,maxMotorScale);

  //and apply it to the mixed values
  leftMotorScaled  = constrain(leftMotor/maxMotorScale,-255,255);
  rightMotorScaled = constrain(rightMotor/maxMotorScale,-255,255);
  sprintf(sbuf, "| LOUT:%6d,  ROUT:%6d ", leftMotorScaled, rightMotorScaled);  Serial.print(sbuf);
  Serial.println("");
 
  Int16ToByteArray(leftMotorScaled, sendbuf, ANA0);
  Int16ToByteArray(rightMotorScaled, sendbuf, ANA1);

  digval[0] = digbuf = sensortouch(joystBtn_0);
  bitWrite(anabuf, 0, digbuf);
  Int16ToByteArray(anabuf, sendbuf, BYTE0);

  delay(1);


}





//=====================================================================================
// L O O P
//=====================================================================================

void loop()
{
  char     sbuf[128],  resOK=1;   
  static   uint8_t cnt=0;
  uint8_t  cbuf[MSGSIZE], chk;
  uint32_t xtime;
 
 
     //   send to Tx master
     memset(sendbuf, 0, sizeof(sendbuf));
     getremotevalues();
 
     //TCP sync bytes
     sendbuf[0]=bsync;

     // heart beat
     sendbuf[TERM] = _heartbeat_; 
   
     // checksum
     sendbuf[1]=calcchecksum(sendbuf);
 
     // Send value to the Rx Master
     for(uint8_t i=0; i<MSGSIZE; i++) {       
        Serial1.write(sendbuf[i]);                             
     }         
     memcpy(sdispbuf, sendbuf, sizeof(sendbuf));
 




  //     Receive fromTx master

  memset(cbuf, 0, sizeof(cbuf));   
  // debug
  resOK = receive ( cbuf, 10000,&cnt);
 
  if( resOK ) {                                      // byte 0 == syncbyte
    cnt=0;
    //displayvalues(60, "Received...:", cbuf);
     chk=(byte)calcchecksum(cbuf);     
     memcpy(recvbuf, cbuf, sizeof(cbuf));
     memcpy(rdispbuf, cbuf, sizeof(cbuf));
  }
  else _heartbeat_ = 0;

}

//=====================================================================================
// L O O P 2:  Display
//=====================================================================================

void loop2() {
   char     sbuf[128]; 
   sprintf(sbuf, "EKG: %3d", _heartbeat_ );
   lcdprintxy(0, 20, sbuf);
   sprintf(sbuf, "JSt_0 | Le=%4d | Ri=%4d | Btn=%1d |", leftMotorScaled, rightMotorScaled, digval[0] );
   lcdprintxy(0, 40, sbuf);

   yield();
   delay(50);
   yield();
}


//=====================================================================================
// L O O P 3:  Heart Beat
//=====================================================================================
uint32_t stoptimer=0;

void loop3() {
   if (millis()-stoptimer >= 100) {
      ++_heartbeat_;
      stoptimer=millis();   
      if(_heartbeat_ >= 100) _heartbeat_ = 1;  // always > 0
   }   
   yield();
   delay(10);
   yield();
}


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


void setup() {
   char sbuf[128];   
   int32_t  i=0;
         
   // Serial
   Serial.begin(115200);   // USB terminal
 
   Serial1.begin(UARTclock);                    // RX-TX UART
   while(Serial1.available())  Serial1.read();  // clear output buffer

   
   // TFT LCD
   Serial.println();
   LCDTYPE = _ILI9341due_;
   Serial.print("init LCD...");
   initLCD(3);   
   Serial.println(" done.");   lcdcls();
   sprintf(sbuf, "LCD=%d wi%dxhi%d Font %dx%d",LCDTYPE,LCDmaxX,LCDmaxY,fontwi,fonthi);
   Serial.println(sbuf);
   Serial.println();
   lcdcls(); lcdprint(sbuf);

   setupdPins();   
   sprintf(sbuf, "dpins(): done.");
   Serial.println(); Serial.println(sbuf);   
   curlf(); lcdprint(sbuf);   
   
   sprintf(sbuf, "Rx slave, BAUD= %ld", UARTclock );;
   curlf(); lcdprint(sbuf);

   // Add "loopX"  to scheduling.
   Scheduler.startLoop(loop2);
   sprintf(sbuf, "MT display task started " ); 
   curlf();
   lcdprint(sbuf);
   Scheduler.startLoop(loop3);
   sprintf(sbuf, "MT heartbeat task started ");
   curlf();
   lcdprint(sbuf);
   curlf();

   sprintf(sbuf, "setup(): done.");
   Serial.println(); Serial.println(sbuf);   
   curlf(); curlf(); lcdprint(sbuf);

   sprintf(sbuf, "press JoystickButton 0 to start!");
   Serial.println(); Serial.println(sbuf);   
   curlf(); curlf(); lcdprint(sbuf);

   while( !sensortouch( joystBtn_0 ) );   while(  sensortouch( joystBtn_0 ) );
   curlf();   sprintf(sbuf, " ...", _curx_); lcdprint(sbuf);   delay(333);   
     
   lcdcls();

}




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

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

Re: Projekt: LegoPi - Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 2. Mär 2016 20:18

Code für den Raspberry Pi (mobiler Robot):

Hilfsdatei "rpiconio.h" für Keyboard-Funktionen:

Code: Alles auswählen

#ifndef _RPICONIO_H
#define   _RPICONIO_H

// courtesy of AndyD, raspberrypi.org forums, and peterfido, roboternetz.de forum
// version 003

// freie Verwendung fuer private Zwecke
// fuer kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// Programming language: gcc  C/C++, Geany IDE
// protected under the friendly Creative Commons
// Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/

#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <termio.h>
#include <unistd.h>

#include <linux/input.h>
#include <termios.h>
#include <signal.h>
#include <sys/types.h>
#include <dirent.h>
#include <sys/stat.h>
#include <sys/select.h>


// Terminal: move cursor
#define cursup    "\033[A"
#define curshome  "\033[0;0H"
#define cls_lxt   "\033[2J"

// keyboard dev
int    fkbd;
char * kbdin = "/dev/input/event0";
struct input_event ev[64];

int shiftl=0;
int shiftr=0;
int _keyshift_=0; // mod keypress state
int ctrll=0;
int ctrlr=0;
int _keyctrl_=0; // mod keypress state
int capsl=0;
int altl=0;
int altgr=0;
int _keyalt_=0;  // mod keypress state
int windows=0;
int kontext=0;
int _keypress_=0;  // keypress state
int modscode=0;
volatile int _kbscode_ ;

#define _ESC_   1
#define _F1_   59
#define _F2_   60
#define _F3_   61
#define _F4_   62
#define _F5_   63
#define _F6_   64
#define _F7_   65
#define _F8_   66
#define _F9_   67
#define _F10_  68
#define _F11_  69
#define _F12_  70



//*************************************************************
// conio.h - mimics
//*************************************************************

bool kbhit(void)
{
    struct termios original;
    tcgetattr(STDIN_FILENO, &original);

    struct termios term;
    memcpy(&term, &original, sizeof(term));

    term.c_lflag &= ~ICANON;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);

    int characters_buffered = 0;
    ioctl(STDIN_FILENO, FIONREAD, &characters_buffered);

    tcsetattr(STDIN_FILENO, TCSANOW, &original);

    bool pressed = (characters_buffered != 0);

    return pressed;
}

void echoOff(void)
{
    struct termios term;
    tcgetattr(STDIN_FILENO, &term);

    term.c_lflag &= ~ECHO;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);
}

void echoOn(void)
{
    struct termios term;
    tcgetattr(STDIN_FILENO, &term);

    term.c_lflag |= ECHO;
    tcsetattr(STDIN_FILENO, TCSANOW, &term);
}




//*************************************************************
// keyboard scancode
//*************************************************************

int getkbscancode() {
   int  rd,  size = sizeof (struct input_event);
   int keybscan=0;  // scan code "normal key"
   
   
   if ((rd = read (fkbd, ev, size * 64)) < size)
          printf ("Fehler mit Tastatur");
       
        if (ev[1].type != EV_KEY) return 0;
       
        if (ev[1].value==0){         //Taste losgelassen
                switch (ev[1].code) {
                    case 42: shiftl=0;   break;
                    case 54: shiftr=0;   break;
                    case 29: ctrll=0;    break;
                    case 97: ctrlr=0;    break;
                    case 56: altl=0;     break;
                    case 125: windows=0; break;
                    case 100: altgr=0;   break;
                    case 127: kontext=0; break;
                }
        }
        else
        {
            if (ev[1].value==1){           
             //==1 fuer nur einen Druck ohne Wiederholung. >=1 fuer Erkennung von gedrueckt gehaltener Taste
                 modscode = 0;
                 switch (ev[1].code) {
                    case 42: shiftl=1;   break;
                    case 54: shiftr=1;   break;
                    case 29: ctrll=1;    break;
                    case 97: ctrlr=1;    break;
                    case 56: altl=1;     break;
                    case 125: windows=1; break;
                    case 100: altgr=1;   break;
                    case 127: kontext=1; break;                   
                   
                    // Ab hier 'normale Tasten'                   
                    default: keybscan=ev[1].code;// Scancode ablegen
                   
                    _keypress_ = keybscan;  // standard keypress state
                    _keyshift_ = 0;         // reset modifier key pressed
                    _keyalt_   = 0;
                    _keyctrl_  = 0;
                                       
                    if(shiftl || shiftr ) { modscode+=1024; _keyshift_=1; } 
                    if(ctrll  || ctrlr  ) { modscode+=2048; _keyctrl_=1; }
                    if(altl)              { modscode+=4096; _keyalt_=1; }
                    if(altgr)             { modscode+=(2048+4096);  _keyalt_=1; _keyctrl_=1; }
                    if(windows)           modscode+=8192;   
                    if(kontext)           modscode+=16384; 
                                   
                    if(keybscan>0) {
                       _kbscode_= keybscan + modscode;
                       return keybscan;
                    }
                    else  return 0 ;
                //break;
                }
            }
        }
        return 0 ;
}
 

int setupKbdin() {
  if ((fkbd = open (kbdin, O_RDONLY)) == -1){
        printf ("Keyboard error");
        return -1;
  }
  else return 0;
}


#endif

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

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

Re: Projekt: LegoPi - Raspberry Pi für Lego Roboter (statt NXT, EV3)

Beitragvon HaWe » 23. Mär 2016 23:13

Code für den Raspberry Pi (mobiler Robot):
update: Änderung des Raspi- UART-Ports auf USB (per ch341 converter)
anstelle von UART GPIO (BCM14+15) :


Herstellen der UART-BT-Verbindung über einen USB-zu UART ch340/ch341 converter:
viewtopic.php?f=78&t=8689&p=67907#p67830
ch341_57.JPG


Im Source Code wird je nach USB-Port-initialisierung der serielle Port für die Fernsteuerung angepasst:

Code: Alles auswählen

char   * usb0  =  "/dev/ttyUSB0"; // USB-UART-BT(HC05) Arduino remote control


letztes Update: PID control API Funktionen, basierend auf meinem früheren NXC Code:
viewtopic.php?f=25&t=7521&p=61935#p61930
https://mindboards.org/viewtopic.php?f= ... d8c#p16362

Es hat sich als sinnvoller erwiesen, den Raspi an das HC05-slave Modul anzuschließen;
übrigens, auch die Logik der UART-Fernsteuerung steuert den Raspi als Slave und den Arduino als Master.

Code: Alles auswählen

/*   Raspi  Robot
 *   USB - > UART ->  Bluetooth HC-05 (slave)
 *   
 *   Raspberry Pi 2 model B
 *   Raspbian Jessie
 *   version RCrobot0034
 *
 * change log:
 *   extended RC dashboard monitoring/control
 *   new: first PID pre-alpha test implementation (detached pthreads)
 *   RC heartbeat control ! <=> safety stop motors
 *   odometry and IMU navigation basics
 *   CMPS11 IMU (gyro/compass) on i2c-0
 *   extra wmctrl - Terminal Window
 *   conio.h-like keybd reading (termio.h, termios.h)
 *   multiple encoder readings by quick realtime threads
 *   pthread priorities:  close to realtime scheduling
 *   sensor interfaces: digital, i2c, UART, ADC  (std. and part. Lego-compat.)
 *   HC-05 BT remote control via ch341 USB-to-UART converter     
 *   UART remote control by Arduino
 *   simple NXC-like motor API commands
 *   openVG graphics for 5" to  24" HDMI screens (600x480 to 1920x1080)
 *   basic POSIX pthread multithreading
 *   wiringPi /wiringSerial UART control
 *   wiringPi /wiringPiI2C i2c control (i2c-1, i2c-0)
 *   wiringPi GPIO + softPWM control
 */
 
// (C) Helmut Wunder (HaWe) 2015, 2016
// freie Verwendung fuer private Zwecke
// fuer kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// Programming language: gcc  C/C++, Geany IDE
// protected under the friendly Creative Commons
// Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http://creativecommons.org/licenses/by-nc-sa/3.0/
 


#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>

#include <math.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <errno.h>
#include <pthread.h>

#include "VG/openvg.h"   
#include "VG/vgu.h"
#include "fontinfo.h"
#include "shapes.h"

#include <wiringPi.h>
#include <wiringSerial.h>
#include <wiringPiI2C.h>
#include <softPwm.h>


#include "rpikbdio.h"

#define  byte  uint8_t



//  semaphores
volatile uint8_t   _TASKS_ACTIVE_= 1;
volatile uint8_t   _REMOTE_OK_   = 1;


//*************************************************************//*************************************************************
//    global settings: array size, dev tree, dev addr, ...     //    global settings: array size, dev tree, dev addr, ...
//*************************************************************//*************************************************************

// IO array size setting

#define  MAXANALOG           9      // maximum number of analog sensors
#define  MAXDIGITAL         16      // maximum number of digital sensors

#define  serMSGSIZE         64      // size of serial comm arrays
#define  i2cMSGSIZE         32      // size of i2c comm arrays

uint8_t  sercurrbuf[serMSGSIZE];    // serial temp comm buffer

uint8_t  rcrtlsendbuf[serMSGSIZE];  // serial remote control comm buffer
uint8_t  rcrtlrecvbuf[serMSGSIZE];
uint8_t  drctrlrecvbuf[serMSGSIZE]; // serial remote control display copy
uint8_t  drctrlsendbuf[serMSGSIZE];

uint8_t  digitalIn[MAXDIGITAL], recvdigital[MAXDIGITAL], senddigital[MAXDIGITAL];
int16_t  analogIn[MAXANALOG],  recvanalog[MAXANALOG],  sendanalog[MAXANALOG] ;


// UART + USB
char   * usb0  =  "/dev/ttyUSB0";  // USB-UART-BT(HC05) Arduino remote control
char   * uart0 =  "/dev/ttyAMA0";  // reserved for UART-Arduino I/O extension board
int    fserial0, fserialUSB0;
const  uint32_t  UARTclock = 115200;


// I2C
char * i2c0  =  "/dev/i2c-0";
char * i2c1  =  "/dev/i2c-1";

// I2C device: CMPS11 IMU
#define CMPS11_ADDR 0x60
int     fcmps11;

// GPIO WiringPi to BCM vpin numbering
#define WP0  17
#define WP1  18
#define WP2  27
#define WP3  22
#define WP4  23
#define WP5  24
#define WP6  25
#define WP7   4
#define WP8   2
#define WP9   3
#define WP10  8
#define WP11  7
#define WP12 10
#define WP13  9
#define WP14 11
#define WP15 14
#define WP16 15
/*
#define WP17
#define WP18
#define WP19 
#define WP20
*/
#define WP21  5
#define WP22  6
#define WP23 13
#define WP24 19
#define WP25 26
#define WP26 12
#define WP27 16
#define WP28 20
#define WP29 21


//*************************************************************
// maths
//*************************************************************   

#define sign(t) ( (t < 0)? -1 : +1 )


//*************************************************************//*************************************************************
//                         openVG                              //                         openVG
//*************************************************************//*************************************************************

// color name constants
#define BLACK     0,   0,   0
#define WHITE   255, 255, 255
#define MAGENTA 255,   0, 255
#define RED     255,   0,   0
#define YELLOW  255, 255,   0
#define LIME      0, 255,   0
#define GREEN     0, 128,   0
#define CYAN      0, 255, 255
#define AQUA    102, 204, 204
#define BLUE      0,   0, 255

int   _scrwidth_,  _scrheight_;
int   _fontsize_ = 10;

#define cls()   WindowClear()


//*************************************************************

inline void lcdprintxy(VGfloat x, VGfloat y, char * buf) {
   Text( (x+_fontsize_), (y+_fontsize_*1.2), buf, MonoTypeface , _fontsize_);   
}

//*************************************************************

void setupGraphics() {
     InitWindowSize(1,1, 520,400);
     InitShapes(&_scrwidth_, &_scrheight_);

     Start(_scrwidth_, _scrheight_);  // Start the picture
     Background(0, 0, 0);
     WindowOpacity(225);              // 90% opacity
     Stroke(WHITE, 1);                // Set these at the start, no need to   
     Fill(WHITE, 1);                  // keep calling them if colour hasn't changed
     cls();
     
     End();
}


//*************************************************************//*************************************************************
//                    NAVIGATOR                                //                       NAVIGATOR
//*************************************************************//*************************************************************



// vehicle properties


#define Wheeldiameter   7.7    // Raddurchmesser
#define Wheelgauge     28.8    // Achsbreite (Mittellinie Antriebsraeder)
#define Wheelgearratio  1.0    // Getriebe 1:1.

                               // Radumfang
double  Wheelcircumference  = M_PI * Wheeldiameter;   // 2*PI*r = PI*d

                               // Abrollstrecke pro 1° MotorEncoderTick
double  Wheelrollperencdeg  = (M_PI * Wheeldiameter * Wheelgearratio)/360.0;



//*************************************************************

//               navigation and localization


double fxpos=0, fypos=0;     // robot position coordinates
double IMUHeading=0;         // robot IMU heading (Gyro,compass)
double fHeading=0;           // robot heading (fusioned)

#define  deg2rad   (M_PI/180.0)


//*************************************************************//*************************************************************
//                      motor API                              //                      motor API
//*************************************************************//*************************************************************

#define  MAXMOTORS          10    // maximum number of motors
#define  MAXMOTORSLOCAL      2    // maximum number of local motors
#define  MAXPWMRANGE       255    // maximum software-pwm range (0-255)

// motor control structure

typedef struct  {   
                // electrical motor pins
      uint8_t   pind1, pind2, pinpwm;    // dir + pwm L293 H-Bridge type
      uint8_t   pinQa, pinQb;            // rotary enc pins Qa,Qb
     
                // pwm and encoder values
      int16_t   dirpwm;     
      int32_t   motenc, oldenc;          // rotary encoder values
     
                // PID   
      pthread_t tid;
   
                // PID custom target values
      int32_t   target;                  // set target
      int16_t   tarpwm;                  // motor target speed
                // PID custom regulation parameters
      double    P;                       // P: basic propotional to error
      double    I;                       // I: integral: avoid perish
      double    D;                       // D: derivative: avoid oscillating
      double    precis;                  // error precision to target
      int32_t   regtime;                 // PID loop time
      double    damp;                    // damp the integral memory
      int8_t    cont;                    // target: continue or hit once
                // internal control variables
      int16_t   runstate;                // monitors runstate
      int16_t   outp;                    // PID control output value
      int16_t   maxout;                  // max output (max motor pwr)
      int32_t   read;                    // current sensor reading
      double    err;                     // current error
      double    integr;                  // integral of errors
      double    speed;                   // current speed
      int8_t    stopPIDcontrol;          // flag for external termination
     
} tEncMotor;


tEncMotor motor[MAXMOTORS];


#define motorLeft  motor[0]
#define motorRight motor[1]



// motor runstates:

#define OUT_REGSTATE_NULL           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_RAMPUP         9
#define OUT_REGSTATE_RAMPDOWN      10

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_ACTIVE        16 
#define OUT_REGSTATE_PIDSTART      17
#define OUT_REGSTATE_PIDEND        18
#define OUT_REGSTATE_PIDHOLD       19
#define OUT_REGSTATE_PIDHOLDCLOSE  20





//*************************************************************

#define motorCoast(nr) motorOn(nr, 0)  // alias for motor coast

//*************************************************************



inline void motorBrake(uint nr, int dirpwm) {      // brake by pwm power
   int pwm;
   
   pwm = abs(dirpwm);
   
   digitalWrite(motor[nr].pind1, HIGH);
   digitalWrite(motor[nr].pind2, HIGH);     
   
   motor[nr].dirpwm = pwm;
   softPwmWrite(motor[nr].pinpwm, pwm);    // brake power always > 0   
   
}

//*************************************************************

inline void motorOn(uint nr, int dirpwm) { // motor On (nr, dir_pwm)
   int dir, pwm;                             // signed pwm:
   
   if(dirpwm > 0) dir = +1;                   // pwm > 0: forward
   else if(dirpwm < 0) dir = -1;              // pwm < 0: reverse
   else dir = 0;                              // pwm = 0: coast
   
   if(! _REMOTE_OK_) dirpwm=0;
   pwm = abs(dirpwm);
   
     
   if(dir> 0) {
      digitalWrite( motor[nr].pind1, HIGH);
      digitalWrite( motor[nr].pind2, LOW);     
   }
   else
   if(dir==0) {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, LOW);
   }
   else {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, HIGH);
   }
   motor[nr].dirpwm = dirpwm;
   softPwmWrite( motor[nr].pinpwm, pwm);
   
   
}

                       
                       
//*************************************************************//*************************************************************
//                       PID control                           //                       PID control
//*************************************************************//*************************************************************   

// forward: motor API functions
inline void RotatePIDtoTarget (uint nr, int32_t Target, double RotatSpeed); // approach absolute target once
inline void RotatePIDdegrees  (uint nr, int32_t rTarget, double RotatSpeed); // turn relative degrees
inline void RotatePIDcontinue (uint nr, int32_t Target, double RotatSpeed); // approach target continuously
inline void StopPIDcontrol    (uint nr);               
inline void PIDinit(); // P=0.4, I=0.4, D=10.0
// simple customized PID setting:
inline void SetPIDparam(uint nr, double P,double I,double D);
// extended customized parameter setting:
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp);   


//*************************************************************                   
                       
inline void PIDcleanup(uint nr) {   
        motorCoast(nr);
        motor[nr].runstate = OUT_REGSTATE_NULL;
        motor[nr].speed    = 0;
        motor[nr].outp     = 0;
        motor[nr].cont     = 0;
}                       
 

//*************************************************************                   

void * PID_calc(void *arg) {
  double  aspeed, damp, PWMpwr, readold, errorold, tprop;
  int32_t    readstart, cmax, cmin;                    // for monitoring
  int32_t    starttime, runtime, clock, dtime;         // timer
  int     regloop;

 
  // arg nach index casten
  unsigned nr = (unsigned)arg;
 
 
  motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;           // reg state: RAMPUP
  motor[nr].read     = motor[nr].motenc;             // get current encoder reading
  motor[nr].err      = motor[nr].target - motor[nr].read; // error to target

  readstart      = motor[nr].read;
  regloop        = 1;


  damp=0;                                   // damp the integral memory

  starttime= millis();


  // appoach target
  _Astart:
  motor[nr].runstate = OUT_REGSTATE_ACTIVE;  // run state: RUNNING

  do {
     
    pthread_testcancel();
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    } 

    dtime    = millis() - clock;
    clock    = millis();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;
   
   
    if ((motor[nr].err==errorold)&& (abs(motor[nr].err)>motor[nr].precis)) damp=1;    // stalling
    else
    damp=motor[nr].damp;

    motor[nr].integr = (damp * motor[nr].integr) + motor[nr].err;

    if((motor[nr].integr) > 3*motor[nr].maxout) motor[nr].integr = 3*motor[nr].maxout; // cut away
    else
    if((motor[nr].integr) <-3*motor[nr].maxout) motor[nr].integr =-3*motor[nr].maxout;

    PWMpwr= (motor[nr].P*motor[nr].err) + (motor[nr].I*motor[nr].integr)*tprop + (motor[nr].D*(motor[nr].err-errorold))/tprop;


    if(PWMpwr >  motor[nr].maxout) PWMpwr=  motor[nr].maxout;   // forward maxout
    else
    if(PWMpwr < -motor[nr].maxout) PWMpwr= -motor[nr].maxout;   // reverse maxout


    motor[nr].speed= (motor[nr].read-readold)*100/dtime;  // rotat speed [degrees/100ms]
    aspeed = abs(motor[nr].speed) ;
       
    if (abs(PWMpwr) > motor[nr].tarpwm )  {
        PWMpwr = sign(PWMpwr) * motor[nr].tarpwm ;
    }

    motor[nr].outp = round(PWMpwr);


    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                      // PID regulation !
    motorOn(nr, motor[nr].outp);                  // action !
    delay(motor[nr].regtime);                       // wait regulation time
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    readold     = motor[nr].read;                           // save old sensor
    errorold    = motor[nr].err;                            // save old error

    motor[nr].read  = motor[nr].motenc;                   // get new encoder value
    motor[nr].err   = motor[nr].target-motor[nr].read;  // new error to target

    if (motor[nr].read>cmax) cmax=motor[nr].read;         // monitor overshooting
    else
    if (motor[nr].read<cmin) cmin=motor[nr].read;         // monitor overshooting

    if ((motor[nr].cont)&& (abs(motor[nr].err)<=motor[nr].precis))
         motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;
    else motor[nr].runstate = OUT_REGSTATE_ACTIVE;

    if (motor[nr].cont) continue;
    if (abs(motor[nr].err)<=motor[nr].precis) {
        regloop +=1 ;
        motor[nr].runstate = OUT_REGSTATE_PIDEND  ;
    }
   
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    }


  } while ((abs(motor[nr].err)>=motor[nr].precis) && (regloop<=5));  // target reached

  motorCoast(nr);                                 // finished - stop motor
  motor[nr].runstate = OUT_REGSTATE_PIDEND;       // run state: RAMPDOWN
  motor[nr].outp=0;

  delay(50);
  motor[nr].read = motor[nr].motenc;
  regloop=1;

  if (motor[nr].read>cmax) cmax=motor[nr].read;            // detect overshooting
  if (motor[nr].read<cmin) cmin=motor[nr].read;            // detect overshooting
  motor[nr].err = motor[nr].target-motor[nr].read;


  if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
  }

  if ((abs(motor[nr].err)>motor[nr].precis))  {goto _Astart;}

  motor[nr].runstate=0;
  delay(1);                                //runstate = IDLE

  return (NULL);
}



//*************************************************************

inline void RotatePID(uint nr, int32_t Target, double RotatSpeed, int8_t cont) {
   
    if( !motor[nr].tid && motor[nr].runstate) {
        motor[nr].runstate=0; // repair
        delay(1);
    }
    if( ( motor[nr].runstate ) || ( motor[nr].tid ) ) {
       motor[nr].stopPIDcontrol = 1;
       delay(1);
       while( motor[nr].runstate || motor[nr].tid );  // wait for PID task to terminate
       delay(1);
       PIDcleanup(nr);
    }
    // init new PID structure 
    motor[nr].runstate = 1;               // set runstate: PID active
    // custom init PID [nr]   
    motor[nr].target = Target;                   // assign target
    motor[nr].tarpwm = abs(RotatSpeed);          // assign max rotation speed
    motor[nr].cont=cont;                         // cont vs. hit once
    // Reset PID control defaults
    motor[nr].outp    = 0;                // PID control output value
    motor[nr].maxout  = MAXPWMRANGE;      // absolute max possible output (max pwr)
    motor[nr].read    = 0;                // current reading
    motor[nr].err     = 0;                // current error
    motor[nr].integr  = 0;                // integral of errors
    motor[nr].speed   = 0;                // current speed
    motor[nr].stopPIDcontrol = 0;         // flag for external termination
   
    // start PID control task
    pthread_create( & motor[nr].tid,      // id speichern in dem dazugehörigen Array-Element
                    0,                    // Default Attribute (threads cancelbar)
                    PID_calc,             // Name der PID-Kalkulationsroutinge
                    (void *) nr);         // der Index des Array-Elements für eine PID Struktur,
                                          // die mit dem Motorindex gleich ist.   
   
    // run pthread task in detached mode, auto-cleanup                                   
    pthread_detach(motor[nr].tid);

}

//*************************************************************                   

inline void StopPIDcontrol (uint nr) {

  if (motor[nr].tid) { // stop PID task if already running
     motor[nr].stopPIDcontrol = 1;   
  }

}
               
//*************************************************************                         
                     
// custom PID parameters Extended
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp) {
    motor[nr].P       = P;             // P: propotional to error
    motor[nr].I       = I;             // I: avoid perish
    motor[nr].D       = D;             // D: derivative: avoid oscillating
    motor[nr].precis  = prec;          // error precision to target
    motor[nr].regtime = regtime;       // PID regulation time
    motor[nr].damp    = damp;          // damp error integral
}


//************************************************************* 
                 
// custom PID parameters
inline void SetPIDparam(uint nr, double P, double I, double D) { 
    motor[nr].P     = P;             // P: propotional to error
    motor[nr].I     = I;             // I: integral: avoid perish
    motor[nr].D     = D;             // D: derivative: avoid oscillating
}                       
   
                   
//*************************************************************                                       
                     
inline void PIDinit() {
  for (uint nr=0; nr < MAXMOTORS; ++nr) {
    SetPIDparamEx(nr, 0.40, 0.40, 10.0, 1.0, 5, 0.75); // p,i,d, precis, reg_time, damp
    delay(1);
  }
}

//*************************************************************                                       


inline void RotatePIDtoTarget(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, false);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDcont(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, true);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDdegrees(uint nr, int32_t  rTarget, double RotatSpeed)  {
   RotatePID(nr, Target+motor[nr].motenc, RotatSpeed, false);
   delay(1);
}


//*************************************************************
//  ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
//*************************************************************                     
                       
                       
                           
/************************************************************************************************************************** 
*          updateEncoders:  Encoder Handler                                 updateEncoders:  Encoder Handler Routine           
**************************************************************************************************************************/

volatile int8_t ISRab[MAXMOTORS];

// 1/2 resolution for Lego motors: 1 tick == 1°
int8_t enctab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};

void updateEncoders() {   
  int i;   
  for( i=0; i<MAXMOTORS; i++ ) {   
     ISRab[ i] <<= 2;
     ISRab[ i] &=  0b00001100;
     ISRab[ i] |= (digitalRead( motor[ i].pinQa ) << 1) | digitalRead( motor[ i].pinQb );
     motor[ i].motenc += enctab[ ISRab[ i] ];   
  }     

}

//*************************************************************//*************************************************************
//             bits, bytes, pins & arrays                      //               bits, bytes, pins & arrays
//*************************************************************//*************************************************************


//*************************************************************
//           bit and byte and pin operations
//*************************************************************
// convert byte arrays to int

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

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

#define ifinrange(val,lo,hi)  if((val>=lo)&&(val<=hi))


//-------------------------------------------------------------
// convert int to byte arrays

inline void Int16ToByteArray(int16_t vint16,  uint8_t *array,  uint8_t slot) {
  memcpy(array+slot*sizeof(char), &vint16, sizeof(int16_t));    // copy int16 to array
}

inline void Int32ToByteArray(int32_t vint32,  uint8_t *array,  uint8_t slot) {
  memcpy(array+slot*sizeof(char), &vint32, sizeof(int32_t));    // copy int32 to array
}

inline void FloatToByteArray(float vfloat32,  uint8_t *array,  uint8_t slot) {   
  memcpy(array+slot*sizeof(char), &vfloat32, sizeof(float));    // copy float to array
}

//-------------------------------------------------------------
// read+write bits in numbers
#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) )



//*************************************************************
//                  msg array structure
//*************************************************************

uint8_t  bsync= 0xFF;  // transmission control bytes: begin of msg signal : bsync=0xFF (255)

// message array setup:

#define SYNCSLOT      0  // start sync signal of this Msg: bsync=0xFF (255)
#define CKSSLOT       1  // chksum this Msg
#define BYTE0         2  // byte 0     // byte: 8-bit => 8 digital bits 0-7
#define BYTE1         3  // byte 1     // byte: 8-bit => 8 digital bits 8-15
#define ENC0          4  // motorenc 0 // 10 encoders: 32-bit
#define ENC1          8  // motorenc 1
#define ENC2         12  // motorenc 2
#define ENC3         16  // motorenc 3
#define ENC4         20  // motorenc 4
#define ENC5         24  // motorenc 5
#define ENC6         28  // motorenc 6
#define ENC7         32  // motorenc 7
#define ENC8         36  // motorenc 8
#define ENC9         40  // motorenc 9
#define ANA0         44  // analog 0   // 9 analog: 16-bit
#define ANA1         46  // analog 1   // rcv analog 0+1 = RC joystick 0
#define ANA2         48  // analog 2
#define ANA3         50  // analog 3
#define ANA4         52  // analog 4
#define ANA5         54  // analog 5
#define ANA6         56  // analog 6
#define ANA7         58  // analog 7
#define ANA8         60  // analog 8
#define BYTE2        62  // byte 2     // byte: 8-bit => 8 digital bits 16-23
#define TERM         63  // terminating byte (heart beat)




//*************************************************************
// msg array read / write
//*************************************************************



void getdigvals( uint8_t array[], uint8_t digital[] ) {
  uint16_t digvalraw;
  memcpy( &digvalraw, array+BYTE0*sizeof(char), sizeof(uint16_t) );   
  for (int i=0; i<MAXDIGITAL; ++i) {
     digital[i] = bitRead(digvalraw, i);
  }   
}


void getanavals( uint8_t array[], int16_t analog[] ) {
   int16_t anatemp, j;
   
   for (j=0; j<MAXANALOG; ++j) {
       memcpy( &anatemp, array + (ANA0 + j*sizeof(int16_t)) * sizeof(char) , sizeof(int16_t) );   
       analog[j] = anatemp;   
    }
   
}

//*************************************************************//*************************************************************
//                         TCP                                 //                         TCP
//*************************************************************//*************************************************************

volatile int  _heartbeat_= 0 , _lastbeat_= 0, _missedbeats_= 999;

uint8_t calcchecksum(uint8_t array[]) {
  int32_t  sum=0;
  for(int i=2; i<serMSGSIZE; ++i) sum+=(array[i]);
  return (sum & 0x00ff);
}

bool checksumOK(uint8_t array[]){
   return (calcchecksum(array)==array[1]);
}

// ============================================================
// addToBuffer and receive function courtesy of chucktodd

bool addToBuffer( uint8_t buf[], uint8_t *cnt, uint16_t timeout){
bool inSync = *cnt>0;
unsigned long start=millis();
while((*cnt<serMSGSIZE)&&(millis()-start<timeout)){
  if( serialDataAvail( fserialUSB0 ) ) { // grab new char, test for sync char, if so start adding to buffer
    buf[*cnt] = (uint8_t)serialGetchar( fserialUSB0 );
    if(inSync) *cnt += 1;  // my origional *cnt++ was updating the pointer address, not
                           // the pointed to sendbuffer
    else{
     if(buf[*cnt]==0xFF){
       inSync = true;
       *cnt +=1;
       }
     }
    }
  }
return (*cnt==serMSGSIZE);
}


//=============================================================

bool receive(uint8_t * buf, uint16_t timeout, uint8_t *cnt){ // by passing cnt in and out,
  // i can timeout and still save a partial buffer, so a resync costs less (less data lost)

  unsigned long start=millis();
  uint8_t * p;  // pointer into buf for reSync operation
  bool done=false;

  do {
    done = addToBuffer(buf,cnt,timeout); // if this return false, a timeout has occured, and the while will exit.
    if(done){                         // do checksumOK test of buffer;
      done=checksumOK(buf);
      if(!done){                      // checksumOK failed, scan buffer for next sync char
         p = (uint8_t*)memchr((buf+1),0xFF,(serMSGSIZE-1)); //forgot to skip the current sync at 0
         
         
         if(p){ // found next sync char, shift buffer content, refill buffer
           *cnt = serMSGSIZE -(p-buf); // count of characters to salvage from this failure
           memcpy(buf,p,*cnt); //cnt is now where the next character from Serial is stored!
           }
         else *cnt=0; // whole buffer is garbage
         }
      }
     
    } while(!done&&(millis()-start<timeout));
 
  return done; // if done then buf[] contains a sendbufid buffer, else a timeout occurred
}


/**************************************************************************************************************************
//                        UARTcomm                                                           UARTcomm
**************************************************************************************************************************/

void UARTcomm()
{
  int8_t   resOK=0;   
  static   uint8_t  cnt=0;
 
 
  //     Receive from Rx slave Arduino
  memset(sercurrbuf, 0, sizeof(sercurrbuf));
   
  resOK = receive ( sercurrbuf, 10000, &cnt); // test read  transmission
 
  if( resOK ) {                   // byte 0 == syncbyte ?
     cnt=0;                       // reset counts of transmitted bytes
     
     memcpy(rcrtlrecvbuf, sercurrbuf, sizeof(sercurrbuf)); // save transm buffer
     
     getdigvals(rcrtlrecvbuf, recvdigital);  // copy dig vals to digital array
     getanavals(rcrtlrecvbuf, recvanalog);   // copy ana vals to analog array
 
    //   build send array and send to Rx slave Arduino
    rcrtlsendbuf[0]=bsync;
   
    Int16ToByteArray( analogIn[6], rcrtlsendbuf, ANA6);
    Int16ToByteArray( analogIn[7], rcrtlsendbuf, ANA7);
    Int16ToByteArray( analogIn[8], rcrtlsendbuf, ANA8);
   
    Int32ToByteArray( motor[0].motenc, rcrtlsendbuf, ENC0);
    Int32ToByteArray( motor[1].motenc, rcrtlsendbuf, ENC1);
   
 
    rcrtlsendbuf[1]=calcchecksum(rcrtlsendbuf);
   
    memcpy(drctrlsendbuf, rcrtlsendbuf, sizeof(rcrtlsendbuf));
 
    for(uint8_t i=0; i<serMSGSIZE; i++) {                               // <<<<< better use write() ?
       serialPutchar( fserialUSB0, rcrtlsendbuf[i]);                     // <<<<< Send values to the Rx Arduino       
    }   

  }
 
 
}





/**************************************************************************************************************************
//                display values on TFT                        //                  display values on TFT
**************************************************************************************************************************/

void displayvalues() {
  int16_t cnt, line, lineH;
  char    sbuf[128];
 
  lineH=_fontsize_ +2;
 
  Fill(WHITE, 1);
 
  line = _scrheight_- 4*lineH;
  lcdprintxy(0, line+lineH, "rcrtlsendbuf");       // show send-array
  // #DEBUG
  Dot(10,10,false); Dot(12,10,false); Dot(14,10,false); Dot(16,10,false); Dot(18,10,false);
 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlsendbuf[cnt ]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line, sbuf);
  }
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlsendbuf[cnt+16]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line-lineH, sbuf);
  }
 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlsendbuf[cnt+32]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line- 2*lineH, sbuf);
  }
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlsendbuf[cnt+48]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line- 3*lineH, sbuf);
  }
 
  Fill(RED, 1);
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", cnt);      // print on TFT
    lcdprintxy(cnt*3*_fontsize_, line- 5*lineH, sbuf);
  }     
  Fill(WHITE, 1);
 
 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", senddigital[cnt]);   // print on TFT dig val
    lcdprintxy(cnt*3*_fontsize_, line- 6*lineH, sbuf);
  }   
   
  Fill(LIME, 1);
  for(cnt=0; cnt<MAXANALOG; ++cnt) {
    sprintf(sbuf, "%4d  ", cnt);      // print nr on TFT
    lcdprintxy(cnt*5*_fontsize_, line- 8*lineH, sbuf);
  }   
  Fill(WHITE, 1);
   
  for(cnt=0; cnt<MAXANALOG; ++cnt) { // analog values
    sprintf(sbuf, "%5d ", analogIn[cnt]);      // print on TFT
    lcdprintxy(cnt*5*_fontsize_, line- 9*lineH, sbuf);
  }
 
 
  Fill(WHITE, 1);
  line = _scrheight_-17*lineH;
  lcdprintxy(0, line+lineH, "rcrtlrecvbuf");         // show receive-array

 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlrecvbuf[cnt ]);        // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line, sbuf);
  }
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlrecvbuf[cnt+16]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line- lineH, sbuf);
  }
 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlrecvbuf[cnt+32]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line- 2*lineH, sbuf);
  }
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", rcrtlrecvbuf[cnt+48]);      // print on TFT raw
    lcdprintxy(cnt*3*_fontsize_, line- 3*lineH, sbuf);
  }
 
  Fill(RED, 1);
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", cnt);      // print nr on TFT
    lcdprintxy(cnt*3*_fontsize_, line- 5*lineH, sbuf);
  }   
  Fill(WHITE, 1);
 
 
 
  for(cnt=0; cnt<16; ++cnt) {
    sprintf(sbuf, "%3d ", recvdigital[cnt]);   // print on TFT dig val
    lcdprintxy(cnt*3*_fontsize_, line- 6*lineH, sbuf);
  }

 
  Fill(LIME, 1);
  for(cnt=0; cnt<MAXANALOG; ++cnt) {
    sprintf(sbuf, "%4d  ", cnt);      // print nr on TFT
    lcdprintxy(cnt*5*_fontsize_, line- 8*lineH, sbuf);
  }   
  Fill(WHITE, 1);
 
  for(cnt=0; cnt<MAXANALOG; ++cnt) { // analog values
    sprintf(sbuf, "%5d ", recvanalog[cnt]);      // print on TFT
    lcdprintxy(cnt*5*_fontsize_, line- 9*lineH, sbuf);
  }
 
  Fill(WHITE, 1);
  sprintf(sbuf, " enc0=%8ld  pwm=%6d     enc1=%8ld  pwm=%6d \n ",
         motor[0].motenc, motor[0].dirpwm,   motor[1].motenc, motor[1].dirpwm );
  lcdprintxy(0, line- 11*lineH, sbuf);
 
   
  if(_REMOTE_OK_) Fill(BLUE, 1);  else Fill(RED, 1);
  sprintf(sbuf, "missedbeats=%-8d", _missedbeats_);
  lcdprintxy(0, line- 13*lineH, sbuf);
 
  End();
 
}




//*************************************************************//*************************************************************
//*************************************************************//*************************************************************
//                             tasks                                                           tasks     
//*************************************************************//*************************************************************
//*************************************************************//*************************************************************


void* thread0Go (void* ) {          // low priority: display values               // low priority: display values
   
    //printf("init graph...: ");
    setupGraphics();
    // DEBUG
    //printf(" done. \n\n");
           
    while(_TASKS_ACTIVE_) {
      cls();
      displayvalues();
      delay(40);   
    }   
    End();

    FinishShapes(); 
   
    return NULL;   

}


//*************************************************************


void* thread1Go(void *)           // low priority:  keyboard monitoring          // low priority:  keyboard monitoring
{
   int   c, ch;  // keyboard scancode, letter char
   
   while(_TASKS_ACTIVE_) {   
            
   
      c=0; ch=0;     
      echoOff();     
     
      if (kbhit())    {
          cursxy_lxt(0,11); printf(cleol_lxt);
          ch=getchar();     
          if(ch>=32 && ch<127) {
              printf("%c", ch);
              fflush(stdout);
          }   
          else if (ch<32) {
              if (kbhit()) ch=getchar();
              ch=0;
          }
      }
     
      // DEBUG
      else  if(ch<32) {
        c = getkbscancode();
        if(c) {
        if( c==1 ) {     // ESC to quit program
               printf("\n\n ESC pressed - program terminated by user \n");
               _TASKS_ACTIVE_=0;  // semaphore to stop all tasks
               printf("\n wait for tasks to join main()... \n\n");
               return NULL;
          }

          else {   
             cursxy_lxt(0,11); printf(cleol_lxt); 
             cursxy_lxt(0,10); printf(cleol_lxt);
                  if(_keypress_==_BSP_)  printf("BSP ");
             else if(_keypress_==_RET_)  printf("RET ");
             else if(_keypress_==_F1_)   printf("F1  ");
             else if(_keypress_==_F2_)   printf("F2  ");
             else if(_keypress_==_F3_)   printf("F3  ");
             else if(_keypress_==_F4_)   printf("F4  ");
             else if(_keypress_==_F5_)   printf("F5  ");
             else if(_keypress_==_F6_)   printf("F6  ");
             else if(_keypress_==_F7_)   printf("F7  ");
             else if(_keypress_==_F8_)   printf("F8  ");
             else if(_keypress_==_F9_)   printf("F9  ");
             else if(_keypress_==_F10_)  printf("F10 ");
             else if(_keypress_==_F11_)  printf("F11 ");
             else if(_keypress_==_F12_)  printf("F12 ");
             else if(_keypress_==_CUp_)  printf("CUp ");
             else if(_keypress_==_CDn_)  printf("CDn ");
             else if(_keypress_==_CLe_)  printf("CLe ");
             else if(_keypress_==_CRi_)  printf("CRi ");
             else if(_keypress_==_PUp_)  printf("PUp ");
             else if(_keypress_==_PDn_)  printf("PDn ");
             else if(_keypress_==_Hom_)  printf("Hom ");
             else if(_keypress_==_End_)  printf("End ");
             else if(_keypress_==_DEL_)  printf("DEL ");
             else if(_keypress_==_INS_)  printf("INS ");

             if(_keyshift_)     printf(" +SHFT ");
             if(_keyctrl_)      printf(" +CTRL ");
             if(_keyalt_)       printf(" +ALT  ");
             printf("scancode=%6d \n", _kbscode_ ); 
             fflush(stdout);

         }         
       }
     }
     delay(50);
   }         
   return NULL;
}


//*************************************************************


void* thread2Go (void* ) {   // medium priority: motor remote control      // medium priority: motor remote control
   static int pwm0, pwm1, i;
   int deadzone = 10;
   
   printf(cls_lxt);
   while(_TASKS_ACTIVE_) {   
     
     
      if( ! _REMOTE_OK_ ) {
       for (i=0; i< MAXMOTORS; ++i ) {
          motorCoast(i);         
       }
     }
     else {
      if( _REMOTE_OK_ )  {  // _REMOTE_OK_ 
         pwm0 = recvanalog[0];
         pwm1 = recvanalog[1];

         if(abs(pwm0) < deadzone) pwm0=0;
         if(abs(pwm1) < deadzone) pwm1=0;
     
         
         motorOn(1, pwm1 );
         motorOn(0, pwm0 );
     
         // debug
         

         cursxy_lxt(0,6);
         printf(cleol_lxt);
         printf("pwm0=%7d pwm1=%7d \n", pwm0, pwm1);
         printf("enc0=%7d enc1=%7d \n",motor[0].motenc, motor[1].motenc);
       
       }     
       
       if (!_TASKS_ACTIVE_) return NULL;
       delay(10);
     }
   }
   return NULL;
}




//*************************************************************



void* thread3Go (void* ) {       // highest priority:  100µs encoder timer        // highest priority:  100µs encoder timer
   while(_TASKS_ACTIVE_) {   
      updateEncoders();
      usleep(100);
   }
   return NULL;
}


//*************************************************************



void* thread4Go (void* ) {       // medium priority:  UART RC                    // medium priority:  UART RC
    while(_TASKS_ACTIVE_) {   
      UARTcomm();
   }
    return NULL;
}


//*************************************************************


void* thread5Go(void *)          // medium priority:  IMU + odometry            // medium priority:  IMU + odometry
{
   uint8_t  high_byte, low_byte;
   int16_t  angle16;
   
   int32_t  encLeft=0, encRight=0,
             encLeftOld, encRightOld, dEncLeft=0, dEncRight=0;
   double   fxold=0, fyold=0, rollRW, rollLW, rollM;
   double   MotencHeading=0, dMencHdgRad, MencHdgRad=0;    // Heading by odometry
   double   IMUHdgRad=0, oldIMUHdg, dIMUHdgRad, dIMUHdg;   // Heading by IMU
   double   fHdgRad=0, oldfHdg, dfHdgRad;                  // Heading by sensor fusion

   
   while(_TASKS_ACTIVE_) {     
     
    //-----------------------------------------------------------------------
    // IMU Gyro Compass
   
    oldIMUHdg = IMUHeading;                     // save old IMUHeading degree
     
     high_byte = wiringPiI2CReadReg8 (fcmps11, 2) ;
     low_byte  = wiringPiI2CReadReg8 (fcmps11, 3) ;
     //pitch     = wiringPiI2CReadReg8 (fcmps11, 4) ;
     //roll      = wiringPiI2CReadReg8 (fcmps11, 5) ;
     
     angle16 =   high_byte << 8;                // Calculate 16 bit angle degree
     angle16 +=  low_byte;
         
     IMUHeading = (double)angle16/10.0;        // new IMUHeading degree
     dIMUHdg    = IMUHeading - oldIMUHdg;       // delta IMU Heading degree
     IMUHdgRad  = IMUHeading * deg2rad;         // degree Hdg => rad
     dIMUHdgRad = dIMUHdg  * deg2rad;           // delta degree Hdg => rad     
         
     //-----------------------------------------------------------------------
     // odometry
     
     encLeftOld =encLeft;                       // save old encoder values
     encRightOld=encRight;
     encLeft = motorLeft.motenc;                // get new encoder values
     encRight= motorRight.motenc;

     dEncLeft=encLeft-encLeftOld;               // delta encooder
     dEncRight=encRight-encRightOld;           
     rollLW= Wheelrollperencdeg*dEncLeft;               // left wheel roll way
     rollRW= Wheelrollperencdeg*dEncRight;              // right wheel roll way

     rollM=(rollRW + rollLW)/2;                         // center roll way

     dMencHdgRad=atan2((rollLW - rollRW), Wheelgauge);  // delta encoder heading !! GEO !!

     //-----------------------------------------------------------------------
     // fusioned Heading:  to do: IMU + odometry + GPS !!!!!         
     
     fHeading = IMUHeading;                  // fusioned Hdg deg: IMU only << to do !
     fHdgRad  = IMUHdgRad;                   // fusioned Hdg rad
     dfHdgRad = dIMUHdgRad;                  // fusioned delta Hdg rad
     
     //-----------------------------------------------------------------------
     analogIn[8] = round(fHeading);          // fusioned Hdg deg ->-> analog array[8]
                                             // ->-> send array back to Arduino dash board
     
     //-----------------------------------------------------------------------
     // calculate new position coordinates
     
     fxold=fxpos; fyold=fypos;               // save old pos (x,y)

     fxpos = fxold + rollM*(sin( (fHdgRad +(dfHdgRad /2))));  // new xpos !! GEO !!
     fypos = fyold + rollM*(cos(-(fHdgRad +(dfHdgRad /2))));  // new ypos !! GEO !!
     
     //-----------------------------------------------------------------------
     analogIn[6] = round(fxpos);             // new x-pos ->-> analog array[6]
     analogIn[7] = round(fypos);             // new y-pos ->-> analog array[7]
                                             // ->-> send array back to Arduino dash board
     
     //-----------------------------------------------------------------------


     MencHdgRad    = MencHdgRad + dMencHdgRad;  // new motor enc Heading
     MotencHeading = MencHdgRad * 180.0/M_PI;
                                                        //----------------------------//
     while (MotencHeading >=360) MotencHeading-=360;    // odometry heading           //
     while (MotencHeading < 0)   MotencHeading+=360;    //----------------------------//     
     
     
   
     //-----------------------------------------------------------------------
   
     delay(2);  //  delay before next iteration loop
   }         
   return NULL;
}

//*************************************************************

void* thread6Go (void* ) {              // medium priority:  RC heartbeat                // medium priority:  RC heartbeat
   while(_TASKS_ACTIVE_) {   
      _lastbeat_ = _heartbeat_;                      // store old value
      delay(10);
      _heartbeat_ = rcrtlrecvbuf[TERM];  // read increasing number from Arduino
      if(abs(_heartbeat_ - _lastbeat_) == 0)         // no change:
        { if(_missedbeats_ <998) _missedbeats_++; }  // transmission error count++
       
      else  if(abs(_heartbeat_ - _lastbeat_) <= 3)   // small increase:
        { _missedbeats_ = 0;  }                      // transmission ok
       
      else  if(abs(_heartbeat_ - _lastbeat_) > 3 )   // large increase:
        { if(_missedbeats_ <998) _missedbeats_++; }  // transmission error count++ 
       
      _REMOTE_OK_ = ( _missedbeats_ < 100 );         // missed beats tolerance
      delay(20);
   }
   return NULL;

     

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


void setupMotors() {
  int i, err;   
 
   
  // motor pin settings
  // encoder pin settings
  // setup for L293D motor driver
 
  // motor pins, wiringPi numbering (in parenthesis: BCM numbering)
 
 
     motor[0].pinQa = WP5;   // (BCM 24)  change for rotation direction
     motor[0].pinQb = WP4;   // (BCM 23)
     motor[0].pind1 =WP24;   // (BCM 19)
     motor[0].pind2 =WP25;   // (BCM 26)
     motor[0].pinpwm=WP26;   // (BCM 12)  pwm
     
     motor[1].pinQa = WP0;   // (BCM 17)  change for rotation direction
     motor[1].pinQb = WP2;   // (BCM 27)
     motor[1].pind1 =WP21;   // (BCM  5)
     motor[1].pind2 =WP22;   // (BCM  6)
     motor[1].pinpwm=WP23;   // (BCM 13)  pwm   
     
     
     for( i=0; i< MAXMOTORSLOCAL; ++i)  {       
        pinMode( motor[i].pinQa, INPUT);        // encA   
        pinMode( motor[i].pinQb, INPUT);        // encB   
        pinMode( motor[i].pind1, OUTPUT);       // dir-1   
        pinMode( motor[i].pind2, OUTPUT);       // dir-2   
        pinMode( motor[i].pinpwm, PWM_OUTPUT);  // pwm
       
        err= softPwmCreate( motor[i].pinpwm, 0, MAXPWMRANGE);
        // #DEBUG
        printf("err %-4d  qa %-4d  qb %-4d  d1 %-4d  d2 %-4d  pwm %-4d \n",
          err, motor[i].pinQa, motor[i].pinQb, motor[i].pind1, motor[i].pind2, motor[i].pinpwm);
       
        motor[i].motenc = 0;
        motor[i].oldenc = 0;
        ISRab[i] = 0;
       
   }
   // #DEBUG
   //printf("press ENTER");
   //getchar();
}

//*************************************************************

void setupI2C() {
   uint8_t  ver;
   char sbuf[128];
       
   fcmps11 = wiringPiI2CSetupInterface( i2c1, CMPS11_ADDR );   
   ver     = wiringPiI2CReadReg8 (fcmps11, 0) ;
   sprintf(sbuf, "\nCMPS11 addr 0x%x - fw version: %3d\n", CMPS11_ADDR, ver);    //
   printf(sbuf);
   // #DEBUG
   // printf("press ENTER");
   // getchar();
   
}

void initarrays() {   
    memset(analogIn, 0, sizeof(analogIn) );
    memset(digitalIn, 0, sizeof(digitalIn) );
    memset(recvdigital, 0, sizeof(recvdigital) );
    memset(senddigital, 0, sizeof(senddigital) );
    memset(rcrtlrecvbuf, 0, sizeof(rcrtlrecvbuf) );
    memset(rcrtlsendbuf, 0, sizeof(rcrtlsendbuf) );
    memset(drctrlrecvbuf, 0, sizeof(drctrlrecvbuf) );
    memset(drctrlsendbuf, 0, sizeof(drctrlsendbuf) );
    memset(sercurrbuf, 0, sizeof(sercurrbuf) );
    memset(analogIn, 0, sizeof(analogIn) );   
}


//*************************************************************//*************************************************************
//*************************************************************//*************************************************************
//                             main                            //                             main
//*************************************************************//*************************************************************
//*************************************************************//*************************************************************

int main() {
    int   iores;
   
    void  *retval0=NULL, *retval1=NULL, *retval2=NULL, *retval3=NULL, *retval4=NULL, *retval5=NULL, *retval6=NULL;
   
    sleep(1);
    // LXTerminal window at pos 0,520, size 320x320
    system("wmctrl -r LXTerminal -e 0,520,0,320,320");  // LXTerminal wmctrl screen
    printf(cls_lxt);
 
    // wiringPi   
   
    putenv ("WIRINGPI_GPIOMEM=1") ;                     // no sudo for gpios required
    iores = wiringPiSetupGpio();                        // init by BCM pin numbering
    if( iores == -1 ) return 1;     
    printf(cls_lxt);
       
    // UART Serial com nr
    fserialUSB0 = serialOpen (usb0, UARTclock);         //  UART baud rate     
       
    initarrays();       
    setupMotors();
    setupI2C();
    setupKbdin(); 

    printf(cls_lxt);
   
    pthread_t thread0, thread1, thread2, thread3, thread4, thread5, thread6;
    struct  sched_param  param;

    pthread_create(&thread0, NULL, thread0Go, NULL);    // low priority task: screen output
    param.sched_priority = 20;
    pthread_setschedparam(thread0, SCHED_RR, &param);
   
    pthread_create(&thread1, NULL, thread1Go, NULL);    // low priority: keyboard monitoring (stop program)
    param.sched_priority = 20;
    pthread_setschedparam(thread1, SCHED_RR, &param);
   
    pthread_create(&thread2, NULL, thread2Go, NULL);    // medium  priority: motor control
    param.sched_priority = 40;
    pthread_setschedparam(thread2, SCHED_RR, &param);
   
    pthread_create(&thread3, NULL, thread3Go, NULL);    // highest priority: encoder reading     
    param.sched_priority = 80;
    pthread_setschedparam(thread3, SCHED_FIFO, &param);
   
    pthread_create(&thread4, NULL, thread4Go, NULL);    // medium priority:  UART comm   <<< test !!
    param.sched_priority = 40;
    pthread_setschedparam(thread4, SCHED_RR, &param);
 
    pthread_create(&thread5, NULL, thread5Go, NULL);    // medium priority: navigation
    param.sched_priority = 40;
    pthread_setschedparam(thread5, SCHED_RR, &param);
   
    pthread_create(&thread6, NULL, thread6Go, NULL);    // medium priority: RC heartbeat
    param.sched_priority = 40;
    pthread_setschedparam(thread6, SCHED_RR, &param);
 


    while(_TASKS_ACTIVE_) { delay(1); }
     
    // wait for threads to join before exiting
    pthread_join(thread0, &retval0);
    pthread_join(thread1, &retval1);
    pthread_join(thread2, &retval2);
    pthread_join(thread3, &retval3);
    pthread_join(thread4, &retval4);
    pthread_join(thread5, &retval5);
    pthread_join(thread6, &retval6);
   
    serialClose( fserialUSB0 );
    printf(cls_lxt);
   
    exit(0);
}

//======================================================================
//======================================================================




to do list für Motoren:
a) ramp-up/down Funktion für Motoren ( schrittweise Veränderung von augenblicklicher bis hin zur neuen Zielgeschwindigkeit)
b) PID Steuerung (in Arbeit; herzlichen Dank an "botty", Roboternetz.de/forums, für seine super-Hilfestellung!)
c) motorSync ( mot1st, mot2nd, pwm, syncratio)
d) Erweiterung der Motor-API auf Remote-Motoren (per MCP23017, ggf. Huckepack-Arduino)

bisher implementierte motor-API Funktionen:
motorOn (uint motnr, int dirpwm) // On forward or reverse by pwm power (signed)
motorCoast (uint motnr) // motor coast == float
motorBrake (uint motnr, int pwm) // motor brake by pwm force
RotatePIDtoTarget (uint motnr, int32_t Target, double RotatSpeed); // approach absolute target once
RotatePIDdegrees (uint motnr, int32_t Target, double RotatSpeed); // turn relative degrees
RotatePIDcontinue (uint motnr, int32_t Target, double RotatSpeed); // approach target continuously
StopPIDcontrol (uint motnr);
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


Zurück zu „Projekte, Showcase, Bauanleitungen“

Wer ist online?

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

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss