include <digitalwritefast.h>

include <stdint.h>

include <stdlib.h>

include <ros.h>

include <std_msgs float32.h="">

include <std_msgs string.h="">

include <sensor_msgs joy.h="">

#include <digitalWriteFast.h>
#include <stdint.h>
#include <stdlib.h>
#include <ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Joy.h> 

ros::NodeHandle  nh;
//std_msgs::String str_msg;
 
str_msg;


float xhizi;
float yhizi;
int hizcarpani;
 
hizcarpani;

const int in1 = 4; /sag /*sag arka teker/
teker*/
const int in2 = 2;
 
2;

const int in3 = 7; /sol /*sol arka teker/
teker*/
const int in4 = 6;
 
6;

const int ena1 = 9;/pwm 9;/*pwm cıkisi sag arka teker/
teker*/
const int enb1 = 3; /pwm /*pwm cıkisi sol arka teker/
 
teker*/


const int in2_1 = 13; /sag /*sag on teker/
teker*/
const int in2_2 = 12;
 
12;

const int in2_3 = 8;/sol 8;/*sol on teker/
teker*/
const int in2_4 = 5;
 
5;

const int ena2 = 11; /pwm /*pwm cıkisi sag on teker/
teker*/
const int enb2 = 10;/pwm 10;/*pwm cıkisi sol on teker/
 
teker*/


void LHLH() {
  digitalWriteFast(in1, LOW);/* sag arka teker geri git/
git*/
  digitalWriteFast(in2, HIGH);
  digitalWriteFast(in3, LOW); / /* sol arka teker geri git*/
  digitalWriteFast(in4, HIGH);
 
HIGH);

  digitalWriteFast(in2_1, LOW);
  digitalWriteFast(in2_2, HIGH);
  digitalWriteFast(in2_3, LOW);
  digitalWriteFast(in2_4, HIGH);
}
 
}

void HLHL() {
  digitalWriteFast(in1, HIGH);
  digitalWriteFast(in2, LOW);
  digitalWriteFast(in3, HIGH);
  digitalWriteFast(in4, LOW);
 
LOW);

  digitalWriteFast(in2_1, HIGH);
  digitalWriteFast(in2_2, LOW);
  digitalWriteFast(in2_3, HIGH);
  digitalWriteFast(in2_4, LOW);
}
 
}

void HLLH() {
  digitalWriteFast(in1, HIGH);
  digitalWriteFast(in2, LOW);
  digitalWriteFast(in3, LOW);
  digitalWriteFast(in4, HIGH);
 
HIGH);

  digitalWriteFast(in2_1, HIGH);
  digitalWriteFast(in2_2, LOW);
  digitalWriteFast(in2_3, LOW);
  digitalWriteFast(in2_4, HIGH);
}
 
}

void LHHL() {
  digitalWriteFast(in1, LOW);
  digitalWriteFast(in2, HIGH);
  digitalWriteFast(in3, HIGH);
  digitalWriteFast(in4, LOW);
 
LOW);

  digitalWriteFast(in2_1, LOW);
  digitalWriteFast(in2_2, HIGH);
  digitalWriteFast(in2_3, HIGH);
  digitalWriteFast(in2_4, LOW);
}
void sagtamgaz() {
  analogWrite(ena1, 92 * hizcarpani); /sag /*sag arka teker */
  analogWrite(enb1, 35 * hizcarpani); /sol /*sol arka teker /
*/
  analogWrite(ena2, 92 * hizcarpani); /Sag /*Sag on teker /
*/
  analogWrite(enb2, 35 * hizcarpani); /sol /*sol on teker /
*/
}
void soltamgaz() {
  analogWrite(ena1, 35 * hizcarpani); /sag /*sag arka teker /
*/
  analogWrite(enb1, 92 * hizcarpani); /sol /*sol arka teker /
*/
  analogWrite(ena2, 35 * hizcarpani ); /Sag /*Sag on teker /
*/
  analogWrite(enb2, 92 * hizcarpani); /sol /*sol on teker */
}
void tamgaz() {
  analogWrite(ena1, 92 * hizcarpani);
  analogWrite(enb1, 92 * hizcarpani);
  analogWrite(ena2, 92 * hizcarpani);
  analogWrite(enb2, 92 * hizcarpani);
}
void fren() {
  digitalWriteFast(in1, LOW);
  digitalWriteFast(in2, LOW);
  digitalWriteFast(in3, LOW);
  digitalWriteFast(in4, LOW);
  analogWrite(ena1, 0);
  analogWrite(enb1, 0);
 
0);

  digitalWriteFast(in2_1, LOW);
  digitalWriteFast(in2_2, LOW);
  digitalWriteFast(in2_3, LOW);
  digitalWriteFast(in2_4, LOW);
  analogWrite(ena2, 0);
  analogWrite(enb2, 0);
}
 
}

void Hareketet(float xhizi, float yhizi, int hizcarpani) {
  if (xhizi >= 0.2 && yhizi <= -0.2) { /*  solgeri /
*/
    LHLH();
    sagtamgaz();
  }
  else if ( xhizi <= -0.2 && yhizi >= 0.2) { / /* sagcaprazileri /
*/
    HLHL();
    soltamgaz();
  }
  else if (xhizi >= 0.2  && yhizi <= 0.2) {/sol/
{/*sol*/
    HLLH();
    tamgaz();
  }
  else if (  xhizi >= 0.2 && yhizi >= 0.2) {/solileri/
{/*solileri*/
    HLHL();
    sagtamgaz();
  }
  else if ( xhizi <= 0.2 &&  yhizi >= 0.2) {/ileri git/
{/*ileri git*/
    HLHL();
    tamgaz();
  }
  else if ( xhizi <= -0.2 && yhizi >= 0) {    / /*  sag /
*/
    LHHL();
    tamgaz();
  }
  else if (xhizi <= -0.2 && yhizi <= -0.2) { / /*  saggeri /
*/
    LHLH();
    soltamgaz();
  }
  else if ( xhizi >= -0.2 && yhizi <= -0.2) { / /*  geri */
    LHLH();
    tamgaz();
  }
  else {
    fren();
  }
}
 
}

void joydata ( const sensor_msgs::Joy& joy)
{
 
{

  int tetik = joy.buttons[4];
 //joy.axes[0]; 
 //joy.axes[1];
 
//joy.axes[1];

  if (tetik >= 0.5 && ((joy.axes[0] <= 0.7 && joy.axes[0] >= 0.2 ) || (joy.axes[0] <= -0.2 && joy.axes[0] >= -0.7) || (joy.axes[1] <= 0.7 && joy.axes[1] >= 0.2 ) || (joy.axes[1] <= -0.2 && joy.axes[1] >= -0.7))) {
    hizcarpani = 1;
    Hareketet(joy.axes[0], joy.axes[1], hizcarpani);
  }
 
}

  else if (tetik >= 0.5 && ((joy.axes[0] <= 1 && joy.axes[0] > 0.7 ) || (joy.axes[0] < -0.7 && joy.axes[0] >= -1) ||   (joy.axes[1] < -0.7 && joy.axes[1] >= -1) || (joy.axes[1] <= 1 && joy.axes[1] > 0.7 )))  {
    hizcarpani = 2.75;
    Hareketet(joy.axes[0], joy.axes[1], hizcarpani);
  }
 
}

  else {
    fren();
  }
}
 
ros::Subscriber<sensor_msgs::joy> }

ros::Subscriber<sensor_msgs::Joy> sub1("joy", joydata);
 
joydata);


void setup()
{
  pinMode(3, OUTPUT); 
  pinMode(9, OUTPUT); //set up the LED
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  nh.initNode();
  nh.subscribe(sub1);
  TCCR1B = TCCR1B & B11111000 | B00000001;/pwm B00000001;/*pwm frekansı ayarlandı 31372.55 D9 ve D10 pin/
pin*/
  TCCR2B = TCCR2B & B11111000 | B00000001;/pwm B00000001;/*pwm frekansı ayarlandı 31372.55 D3 ve D11 pin/
}
 
pin*/
}

void loop()   {
 
nh.spinOnce();
 {

  nh.spinOnce();