pad ps2

pad.h

#include <avr/io.h>
#include <util/delay.h>

// CMD - wyjście
#define CMD_SET PORTC |= (1<<4)
#define CMD_CLR PORTC &= ~(1<<4)
#define CMD_DIR_OUT DDRC |= (1<<4)
// ATT - wyjście
#define ATT_SET PORTC |= (1<<5)
#define ATT_CLR PORTC &= ~(1<<5)
#define ATT_DIR_OUT DDRC |= (1<<5)
// DATA - wejście
#define DATA_DIR_IN DDRC &= ~(1<<2)
#define DATA_IN    (PINC&(1<<2))
// CLK - wyjście
#define CLK_SET PORTC |= (1<<3)
#define CLK_CLR PORTC &= ~(1<<3)
#define CLK_DIR_OUT DDRC |= (1<<3)
// LED
#define LED_SET DDRB |= (1<<5)
#define LED_ON PORTB |= (1<<5)
#define LED_OFF PORTB &= ~(1<<5)

extern unsigned char tab[9];
void pad_init(void);
void pad_cmd(unsigned char t[],unsigned char n);
bool pad_get_state();
void pad_loop();

pad.c:

#include "pad.h"
#include "motor.h"
#include "buzzer.h"

// Krotkie opóźnienie
#define _NOP_ asm volatile("nop\n\t""nop\n\t" "nop\n\t" "nop\n\t" ::)

unsigned char tab[9];

// Wysyła/odczytuje jeden bajt do/z gamepada
unsigned char pad_byte(unsigned char byte)
{
   unsigned char i,r=0;

   for(i=0; i<8; i++, byte>>=1)
   {
       CLK_CLR;
       if(byte& 0x01) CMD_SET; else CMD_CLR;
       _NOP_;
       CLK_SET;
       r>>=1;
       if(DATA_IN) r|=0x80;
   }
   CMD_SET;
   _delay_us(20);
   return r;
}

void pad_cmd(unsigned char t[],unsigned char n)
{
   unsigned char i;
    LED_ON;
   ATT_CLR;

   for(i=0; i<n; i++)
      t[i] = pad_byte(t[i]);

   ATT_SET;
   _delay_us(50);
   LED_OFF;
}


void pad_config(void)
{
   // Komenda 0x43 "Go into configuration mode"
   tab[0]= 0x01;
   tab[1]= 0x43;
   tab[2]= 0x00;
   tab[3]= 0x01;
   tab[4]= 0x00;
   pad_cmd(tab, 5);

   // Komenda 0x44 "Turn on analog mode"
   tab[0]= 0x01;
   tab[1]= 0x44;
   tab[2]= 0x00;
   tab[3]= 0x01;
   tab[4]= 0x03; //
   tab[5]= 0x00;
   tab[6]= 0x00;
   tab[7]= 0x00;
   tab[8]= 0x00;
   pad_cmd(tab, 9);

   // Komenda 0x43 "Exit config mode", niekonieczna, gdyż 0x42 automatycznie wychodzi z config mode
   tab[0] = 0x01;
   tab[1] = 0x43;
   tab[2] = 0x00;
   tab[3] = 0x00;
   tab[4] = 0x5A;
   tab[5] = 0x5A;
   tab[6] = 0x5A;
   tab[7] = 0x5A;
   tab[8] = 0x5A;
   pad_cmd(tab, 9);
}

void pad_init(void)
{
    CMD_DIR_OUT;
    ATT_DIR_OUT;
    CLK_DIR_OUT;

    DATA_DIR_IN;

    ATT_SET;
    CLK_SET;
    CMD_SET;

    LED_SET;

    pad_config();
}

bool pad_get_state()
{
    // komenda 0x42 - Controller poll
      tab[0] = 0x01;
      tab[1] = 0x42;
      tab[2] = 0x00;

      tab[3] = 0x00; // przyciski
      tab[4] = 0x00; // przyciski
      tab[5] = 0x00; // dżojstik prawy  lewo-prawo
      tab[6] = 0x00; // dżojstik prawy  góra-dół
      tab[7] = 0x00; // dżojstik lewy   lewo-prawo
      tab[8] = 0x00; // dżojstik lewy   gora-dół
      pad_cmd(tab, 9);
      // analogi w stanie spocczynkowym (na środku) zwracają 0b10000000 = 128, góra = 0, lewo = 0, dół = 255, prawo = 255

      return (tab[0] == 255 && tab[1] == 115);      //zwracamy czy komunikacja zakończyła się sukcesesm
}

void pad_loop()
{
            int tmpl = 0, tmpr = 0;
            bool back = false;
            if(tab[8] > 128) // jeśli analog w górę
            {
                beep(500, 200);
                back = true;
                tmpl = tab[8]-127;
                tmpr = tmpl;
            }
            else if(tab[8] < 128) // jeśli w dół
            {
                tmpl = 128-tab[8];
                tmpr = tmpl;
            }

            int p = 20;
            if(tab[7] > 128) // jeśli analog w prawo
            {
                tmpr -= (p*(tab[7]-127))/100;

            }
            else if(tab[7] < 128) // jeśli w lewo
            {
                tmpl -= (p*(127-tab[7]))/100;
            }

            if(!(tab[4] & (1 << 3))) // jeśli wciśnięty R1 to turbo lvl1
            {
                if(tab[8] == 128) // jesli stoimy i chcemy zrobic baczka
                {
                        if(tab[7] > 128) // jeśli analog w prawo
                        {
                            tmpl = -1 * tmpr;

                        }
                        else if(tab[7] < 128) // jeśli w lewo
                        {
                            tmpr = -1 * tmpl;
                        }
                }
                else
                {
                    tmpl += 47;
                    tmpr += 47;
                }
            }

            if(!(tab[4] & (1 << 1))) // jeśli wciśnięty R2 to turbo lvl2
            {
                    tmpl += 80;
                    tmpr += 80;
            }

            if(back)
            {
                tmpl *= -1;
                tmpr *= -1;
            }

            if(tab[8] == 128 && tab[7] == 128)
            {
                m1_stop();
                m2_stop();
            }
            else
            {
                tmpl = tmpl * 100 / 255;
                tmpr = tmpr * 100 / 255;

                m1_set(tmpl);
                m2_set(tmpr);

                m1_start();
                m2_start();
            }


            if(!(tab[4] & (1 << 6))) // jeśli wciśnięty X to klakson
            {
                beep(4000, 200);
            }
}