#include <PS2X_lib.h>  //for v1.6

/******************************************************************
 * set pins connected to PS2 controller:
 *   - 1e column: original 
 *   - 2e colmun: Stef?
 * replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        50  //14 MISO  
#define PS2_CMD        51  //15 MOSI
#define PS2_SEL        53  //16 SS
#define PS2_CLK        52  //17 SCK


#define pressures   true
//#define pressures   false
#define rumble      true
//#define rumble      false

PS2X ps2x; // create PS2 Controller Class

/***************************************************************
 * Definition of OUTPUT Pin
 * **************************************************************/
#define TOP_L_RIGHT   30
#define TOP_L_UP      31
#define SKY_FALL_A    2
#define SKY_FALL_B    3
#define SKY_RISE_B    4
#define TOP_R_RISE    5
#define TOP_R_FALL    6
#define TOP_R_RIGHT   7
#define TOP_R_UP      22
#define TOP_L_FALL    23
#define TOP_L_LEFT    24
#define TOP_L_DOWN    25
#define SKY_RISE_A    26
#define TOP_R_LEFT    27
#define TOP_R_DOWN    28
#define TOP_L_RISE    29 

#define ACC_L_LEFT    38  

#define ACC_L_UP      36 
#define SKY_BACK_A    35 //
#define SKY_BACK_B    33 //
#define SKY_FWD_B     32 //

#define ACC_R_RISE    47
#define ACC_R_FALL    46
#define ACC_R_LEFT    44
#define ACC_R_UP      42

#define UNUSED_1      41

#define ACC_L_RIGHT   39
#define ACC_L_DOWN    37

#define SKY_FWD_A     34 //

#define ACC_R_RIGHT   45
#define ACC_R_DOWN    43




//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you connect the controller, 
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;



int TOP_L_DOWN_SC = 0;
int TOP_L_UP_SC = 0;
int TOP_L_LEFT_SC = 0;
int TOP_L_RIGHT_SC = 0;
int TOP_L_FALL_SC = 0;
int TOP_L_RISE_SC = 0;

int TOP_R_DOWN_SC = 0;
int TOP_R_UP_SC = 0;
int TOP_R_LEFT_SC = 0;
int TOP_R_RIGHT_SC = 0;
int TOP_R_FALL_SC = 0;
int TOP_R_RISE_SC = 0;

int SKY_FALL_SC = 0;
int SKY_RISE_SC = 0;
int SKY_FWD_SC = 0;
int SKY_BACK_SC = 0;

int SKY_FALL_A_SC = 0;
int SKY_RISE_A_SC = 0;
int SKY_FWD_A_SC = 0;
int SKY_BACK_A_SC = 0;

int SKY_FALL_B_SC = 0;
int SKY_RISE_B_SC = 0;
int SKY_FWD_B_SC = 0;
int SKY_BACK_B_SC = 0;

int ACC_ACTIVE_SC = 0;

int ACC_L_ACTIVE_SC = 0;
int ACC_L_UP_SC = 0;
int ACC_L_DOWN_SC = 0;
int ACC_L_LEFT_SC = 0;
int ACC_L_RIGHT_SC = 0;

int ACC_R_ACTIVE_SC = 0;
int ACC_R_UP_SC = 0;
int ACC_R_DOWN_SC = 0;
int ACC_R_LEFT_SC = 0;
int ACC_R_RIGHT_SC = 0;
int ACC_R_RISE_SC = 0;
int ACC_R_FALL_SC = 0;

int SQUARE_SC = 0;
int CIRCLE_SC = 0;
int TRIANGLE_SC = 0;
int CROSS_SC = 0;
int START_SC = 0;
int SELECT_SC = 0;

int SPIKE_THRESHOLD = 3;

bool SPIKE = false;
int ERROR_COUNT = 0;

void setup(){
  pinMode( TOP_L_LEFT , OUTPUT);
  pinMode( TOP_L_RIGHT , OUTPUT);
  pinMode( TOP_L_DOWN , OUTPUT);  
  pinMode( TOP_L_UP , OUTPUT);
  pinMode( TOP_L_RISE , OUTPUT);
  pinMode( TOP_L_FALL , OUTPUT);
  
  pinMode( TOP_R_LEFT , OUTPUT);
  pinMode( TOP_R_RIGHT , OUTPUT);
  pinMode( TOP_R_UP , OUTPUT);
  pinMode( TOP_R_DOWN , OUTPUT);
  pinMode( TOP_R_RISE , OUTPUT);
  pinMode( TOP_R_FALL , OUTPUT);
  
  pinMode( SKY_FALL_A , OUTPUT);
  pinMode( SKY_FALL_B , OUTPUT);
  pinMode( SKY_RISE_A , OUTPUT);
  pinMode( SKY_RISE_B , OUTPUT);

  pinMode( SKY_FWD_A , OUTPUT);
  pinMode( SKY_FWD_B , OUTPUT);
  pinMode( SKY_BACK_A , OUTPUT);
  pinMode( SKY_BACK_B , OUTPUT);

  pinMode( ACC_L_LEFT , OUTPUT);
  pinMode( ACC_L_RIGHT , OUTPUT);
  pinMode( ACC_L_UP , OUTPUT);
  pinMode( ACC_L_DOWN , OUTPUT);

  pinMode( ACC_R_LEFT , OUTPUT);
  pinMode( ACC_R_RIGHT , OUTPUT);
  pinMode( ACC_R_UP , OUTPUT);
  pinMode( ACC_R_DOWN , OUTPUT);
  pinMode( ACC_R_RISE , OUTPUT);
  pinMode( ACC_R_FALL , OUTPUT);


  Serial.begin(57600);
  
  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
  
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    error = 0;

  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
	if (pressures)
	  Serial.println("true ");
	else
	  Serial.println("false");
	Serial.print("rumble = ");
	if (rumble)
	  Serial.println("true");
	else
	  Serial.println("false");
  }  
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  
//  Serial.print(ps2x.Analog(1), HEX);
    type = ps2x.readType(); 
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
  case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
   
 
}

void loop(){ 

  ps2x.read_gamepad(false, vibrate);

  if( (ps2x.Analog(PSS_LX) == 255) && (ps2x.Analog(PSS_LY) == 255) && ( ps2x.Analog(PSS_RX) == 255) && (ps2x.Analog(PSS_RY) == 255 )){
  }else{
    if(ps2x.Analog(PSS_LX) < 64 ) {
        TOP_L_LEFT_SC++;
        ACC_L_LEFT_SC++;
      }  else  {
        TOP_L_LEFT_SC = 0;
        ACC_L_LEFT_SC = 0;
      }
    if(ps2x.Analog(PSS_LX) > 192 ) {
        TOP_L_RIGHT_SC++;
        ACC_L_RIGHT_SC++;
      }  else  {
        TOP_L_RIGHT_SC = 0;
        ACC_L_RIGHT_SC = 0;
      }
    if(ps2x.Analog(PSS_LY) < 64 ) {
        TOP_L_UP_SC++;
        ACC_L_UP_SC++;
      }  else  {
        TOP_L_UP_SC = 0;
        ACC_L_UP_SC = 0;
      }
    if(ps2x.Analog(PSS_LY) > 192 ) {
        TOP_L_DOWN_SC++;
        ACC_L_DOWN_SC++;
      }  else  {
        TOP_L_DOWN_SC = 0;
        ACC_L_DOWN_SC = 0;
      }
    if(ps2x.Analog(PSS_RX) < 64 ) {
        TOP_R_LEFT_SC++;
        ACC_R_LEFT_SC++;
      }  else  {
        TOP_R_LEFT_SC = 0;
        ACC_R_LEFT_SC = 0;
      }
    if(ps2x.Analog(PSS_RX) > 192 ) {
        TOP_R_RIGHT_SC++;
        ACC_R_RIGHT_SC++;
      }  else  {
        TOP_R_RIGHT_SC = 0;
        ACC_R_RIGHT_SC = 0;
      }
    if(ps2x.Analog(PSS_RY) < 64 ) {
        TOP_R_UP_SC++;
        ACC_R_UP_SC++;
      }  else  {
        TOP_R_UP_SC = 0;
        ACC_R_UP_SC = 0;
      }
    if(ps2x.Analog(PSS_RY) > 192 ) {
        TOP_R_DOWN_SC++;
        ACC_R_DOWN_SC++;
      }  else  {
        TOP_R_DOWN_SC = 0;
        ACC_R_DOWN_SC = 0;
      }

    if(ps2x.Button(PSB_START)) START_SC++; else START_SC = 0;
    if(ps2x.Button(PSB_SELECT)) SELECT_SC++; else SELECT_SC = 0;
    if(ps2x.Button(PSB_GREEN)) TRIANGLE_SC++; else TRIANGLE_SC = 0;
    if(ps2x.Button(PSB_BLUE)) CROSS_SC++; else CROSS_SC = 0;
    if(ps2x.Button(PSB_PINK)) SQUARE_SC++; else SQUARE_SC = 0;
    if(ps2x.Button(PSB_RED)) CIRCLE_SC++; else CIRCLE_SC = 0;

    if(ps2x.Button(PSB_L1)) TOP_L_RISE_SC++; else TOP_L_RISE_SC = 0;
    if(ps2x.Button(PSB_L2)) TOP_L_FALL_SC++; else TOP_L_FALL_SC = 0;
    if(ps2x.Button(PSB_R1)) {
      TOP_R_RISE_SC++;
      ACC_R_RISE_SC++;
      } else {
        TOP_R_RISE_SC = 0;
        ACC_R_RISE_SC = 0;
      }

    if(ps2x.Button(PSB_R2)) {
      TOP_R_FALL_SC++;
      ACC_R_FALL_SC++;
    }else{
      TOP_R_FALL_SC = 0;
      ACC_R_FALL_SC = 0;
    }
  
    if(ps2x.Button(PSB_L3)){
      ACC_L_ACTIVE_SC++;
      ACC_ACTIVE_SC++;
    } else{
      ACC_L_ACTIVE_SC = 0;
    }
    
    if(ps2x.Button(PSB_R3)) {
      ACC_R_ACTIVE_SC++;
      ACC_ACTIVE_SC++;
    } else {
      ACC_R_ACTIVE_SC = 0;
    }
   if(!ps2x.Button(PSB_R3) && !ps2x.Button(PSB_L3)) ACC_ACTIVE_SC = 0;

    if(ps2x.Button(PSB_PAD_UP)) SKY_RISE_SC++; else SKY_RISE_SC = 0;
    if(ps2x.Button(PSB_PAD_DOWN)) SKY_FALL_SC++; else SKY_FALL_SC = 0;
    if(ps2x.Button(PSB_PAD_LEFT)) SKY_BACK_SC++; else SKY_BACK_SC = 0;
    if(ps2x.Button(PSB_PAD_RIGHT)) SKY_FWD_SC++; else SKY_FWD_SC = 0;

  }


/**************TOP Left  *******************/
//  if(  (ACC_L_ACTIVE_SC < SPIKE_THRESHOLD) && ACC_L_ACTIVE_SC < SPIKE_THRESHOLD ){
  if(ACC_ACTIVE_SC <SPIKE_THRESHOLD){
    // LEFT
    if(TOP_L_LEFT_SC  >  SPIKE_THRESHOLD ){
      Serial.print ("Top L LEFT ");
      Serial.println (TOP_L_LEFT_SC);
      digitalWrite(TOP_L_LEFT,HIGH);
    }else digitalWrite(TOP_L_LEFT,LOW);
    
    // RIGHT
    if(TOP_L_RIGHT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top L RIGHT ");
      digitalWrite(TOP_L_RIGHT,HIGH);
    }else  digitalWrite(TOP_L_RIGHT,LOW);

    // UP
    if(TOP_L_UP_SC  >  SPIKE_THRESHOLD ){
      digitalWrite(TOP_L_UP,HIGH);
      Serial.println ("Top L UP ");
    }else  digitalWrite(TOP_L_UP,LOW);

    // DOWN
    if(TOP_L_DOWN_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top L DOWN ");
      digitalWrite(TOP_L_DOWN,HIGH);
    }else digitalWrite(TOP_L_DOWN,LOW);

    // RISE
    if(TOP_L_RISE_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top L RISE ");
      digitalWrite(TOP_L_RISE,HIGH);
    }else  digitalWrite(TOP_L_RISE,LOW);

    // FALL
    if(TOP_L_FALL_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top L FALL ");
      digitalWrite(TOP_L_FALL,HIGH);
    }else  digitalWrite(TOP_L_FALL,LOW);
  }
   else {
      digitalWrite(TOP_L_LEFT,LOW);
      digitalWrite(TOP_L_RIGHT,LOW);
      digitalWrite(TOP_L_UP,LOW);
      digitalWrite(TOP_L_DOWN,LOW);
      digitalWrite(TOP_L_RISE,LOW);
      digitalWrite(TOP_L_FALL,LOW);
      TOP_L_LEFT_SC = 0;
      TOP_L_RIGHT_SC = 0;
      TOP_L_UP_SC = 0;
      TOP_L_DOWN_SC = 0;
      TOP_L_RISE_SC = 0;
      TOP_L_FALL_SC = 0;
 }

/************** Top Right  *******************/

 // if(  ACC_R_ACTIVE_SC < SPIKE_THRESHOLD){
  if(  ACC_ACTIVE_SC < SPIKE_THRESHOLD){
    // LEFT
    if(TOP_R_LEFT_SC  >  SPIKE_THRESHOLD ){
      Serial.print ("Top R LEFT ");
      Serial.println (TOP_R_LEFT_SC);
      digitalWrite(TOP_R_LEFT,HIGH);
    }else digitalWrite(TOP_R_LEFT,LOW);
    
    // RIGHT
    if(TOP_R_RIGHT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top R RIGHT ");
      digitalWrite(TOP_R_RIGHT,HIGH);
    }else  digitalWrite(TOP_R_RIGHT,LOW);

    // UP
    if(TOP_R_UP_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top R UP ");
      digitalWrite(TOP_R_UP,HIGH);
    }else  digitalWrite(TOP_R_UP,LOW);

    // DOWN
    if(TOP_R_DOWN_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top R DOWN ");
      digitalWrite(TOP_R_DOWN,HIGH);
    }else digitalWrite(TOP_R_DOWN,LOW);

    // RISE
    if(TOP_R_RISE_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top R RISE ");
      digitalWrite(TOP_R_RISE,HIGH);
    }else  digitalWrite(TOP_R_RISE,LOW);

    // FALL
    if(TOP_R_FALL_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Top R FALL ");
      digitalWrite(TOP_R_FALL,HIGH);
    }else  digitalWrite(TOP_R_FALL,LOW);
  } else {
      digitalWrite(TOP_R_LEFT,LOW);
      digitalWrite(TOP_R_RIGHT,LOW);
      digitalWrite(TOP_R_UP,LOW);
      digitalWrite(TOP_R_DOWN,LOW);
      digitalWrite(TOP_R_RISE,LOW);
      digitalWrite(TOP_R_FALL,LOW);
      TOP_R_LEFT_SC = 0;
      TOP_R_RIGHT_SC = 0;
      TOP_R_UP_SC = 0;
      TOP_R_DOWN_SC = 0;
      TOP_R_RISE_SC = 0;
      TOP_R_FALL_SC = 0;
  }

/****************SKY**********************/

  if(( SQUARE_SC <  SPIKE_THRESHOLD ) && ( CIRCLE_SC <  SPIKE_THRESHOLD )){
  
    // RISE
    if( SKY_RISE_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY Rise up"); 
      digitalWrite(SKY_RISE_A,HIGH);
      digitalWrite(SKY_RISE_B,HIGH);
    }else{
      digitalWrite(SKY_RISE_A,LOW);
      digitalWrite(SKY_RISE_B,LOW);
    }
  
    // FALL
    if( SKY_FALL_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY Fall down"); 
      digitalWrite(SKY_FALL_A,HIGH);
      digitalWrite(SKY_FALL_B,HIGH);
    }else{
      digitalWrite(SKY_FALL_A,LOW);
      digitalWrite(SKY_FALL_B,LOW);
    }
  
    // BACKWARD
    if( SKY_BACK_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY Backward"); 
      digitalWrite(SKY_BACK_A,HIGH);
      digitalWrite(SKY_BACK_B,HIGH);
    }else{
      digitalWrite(SKY_BACK_A,LOW);
      digitalWrite(SKY_BACK_B,LOW);
    }
  
    // FORWARD
    if( SKY_FWD_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY Forward"); 
      digitalWrite(SKY_FWD_A,HIGH);
      digitalWrite(SKY_FWD_B,HIGH);
    }else{
      digitalWrite(SKY_FWD_A,LOW);
      digitalWrite(SKY_FWD_B,LOW);
    }
  }
  
/***********************SKY_A MOTION***************************************/
  if(( SQUARE_SC >  SPIKE_THRESHOLD ) && ( CIRCLE_SC <  SPIKE_THRESHOLD )){
     // RISE
    if( SKY_RISE_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY A Rise up"); 
      digitalWrite(SKY_RISE_A,HIGH);
    }else{
      digitalWrite(SKY_RISE_A,LOW);
    }
  
    // FALL
    if( SKY_FALL_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY A Fall down"); 
      digitalWrite(SKY_FALL_A,HIGH);
    }else{
      digitalWrite(SKY_FALL_A,LOW);
    }
  
    // BACKWARD
    if( SKY_BACK_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY A Backward"); 
      digitalWrite(SKY_BACK_A,HIGH);
    }else{
      digitalWrite(SKY_BACK_A,LOW);
    }
  
    // FORWARD
    if( SKY_FWD_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY A Forward"); 
      digitalWrite(SKY_FWD_A,HIGH);
    }else{
      digitalWrite(SKY_FWD_A,LOW);
    }
  }

 
/***********************SKY_B MOTION***************************************/
  if(( SQUARE_SC <  SPIKE_THRESHOLD ) && ( CIRCLE_SC >  SPIKE_THRESHOLD )){
     // RISE
    if( SKY_RISE_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY B Rise up"); 
      digitalWrite(SKY_RISE_B,HIGH);
    }else{
      digitalWrite(SKY_RISE_B,LOW);
    }
  
    // FALL
    if( SKY_FALL_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY B Fall down"); 
      digitalWrite(SKY_FALL_B,HIGH);
    }else{
      digitalWrite(SKY_FALL_B,LOW);
    }
  
    // BACKWARD
    if( SKY_BACK_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY B Backward"); 
      digitalWrite(SKY_BACK_B,HIGH);
    }else{
      digitalWrite(SKY_BACK_B,LOW);
    }
  
    // FORWARD
    if( SKY_FWD_SC > SPIKE_THRESHOLD ){
      Serial.println("SKY B Forward"); 
      digitalWrite(SKY_FWD_B,HIGH);
    }else{
      digitalWrite(SKY_FWD_B,LOW);
    }
  }
/**************************************************/
/****************  Accscent  **********************/
/**************************************************/

/****************  Left  **********************/
  if( (ACC_ACTIVE_SC > SPIKE_THRESHOLD) ){
    // LEFT
    if(ACC_L_LEFT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc L LEFT ");
      digitalWrite(ACC_L_LEFT,HIGH);
    }else digitalWrite(ACC_L_LEFT,LOW);
    
    // RIGHT
    if(ACC_L_RIGHT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc L RIGHT ");
      digitalWrite(ACC_L_RIGHT,HIGH);
    }else  digitalWrite(ACC_L_RIGHT,LOW);

    // UP
    if(ACC_L_UP_SC  >  SPIKE_THRESHOLD ){
      digitalWrite(ACC_L_UP,HIGH);
      Serial.println ("Acc L UP ");
    }else  digitalWrite(ACC_L_UP,LOW);

    // DOWN
    if(ACC_L_DOWN_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc L DOWN ");
      digitalWrite(ACC_L_DOWN,HIGH);
    }else digitalWrite(ACC_L_DOWN,LOW);



/**************** Right *********************/

    // LEFT
    if(ACC_R_LEFT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc R LEFT ");
      digitalWrite(ACC_R_LEFT,HIGH);
    }else digitalWrite(ACC_R_LEFT,LOW);
    
    // RIGHT
    if(ACC_R_RIGHT_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc R RIGHT ");
      digitalWrite(ACC_R_RIGHT,HIGH);
    }else  digitalWrite(ACC_R_RIGHT,LOW);

    // UP
    if(ACC_R_UP_SC  >  SPIKE_THRESHOLD ){
      digitalWrite(ACC_R_UP,HIGH);
      Serial.println ("Acc R UP ");
    }else  digitalWrite(ACC_R_UP,LOW);

    // DOWN
    if(ACC_R_DOWN_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc R DOWN ");
      digitalWrite(ACC_R_DOWN,HIGH);
    }else digitalWrite(ACC_R_DOWN,LOW);

    // RISE
    if(ACC_R_RISE_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc R RISE ");
      digitalWrite(ACC_R_RISE,HIGH);
    }else  digitalWrite(ACC_R_RISE,LOW);

    // FALL
    if(ACC_R_FALL_SC  >  SPIKE_THRESHOLD ){
      Serial.println ("Acc R FALL ");
      digitalWrite(ACC_R_FALL,HIGH);
    }else  digitalWrite(ACC_R_FALL,LOW);
  } else {

      digitalWrite(ACC_L_LEFT,LOW);
      digitalWrite(ACC_L_RIGHT,LOW);
      digitalWrite(ACC_L_UP,LOW);
      digitalWrite(ACC_L_DOWN,LOW);
 //     digitalWrite(ACC_L_RISE,LOW);
 //     digitalWrite(ACC_L_FALL,LOW);
      ACC_L_LEFT_SC = 0;
      ACC_L_RIGHT_SC = 0;
      ACC_L_UP_SC = 0;
      ACC_L_DOWN_SC = 0;
 //     ACC_L_RISE_SC = 0;
 //     ACC_L_FALL_SC = 0;

    
      digitalWrite(ACC_R_LEFT,LOW);
      digitalWrite(ACC_R_RIGHT,LOW);
      digitalWrite(ACC_R_UP,LOW);
      digitalWrite(ACC_R_DOWN,LOW);
      digitalWrite(ACC_R_RISE,LOW);
      digitalWrite(ACC_R_FALL,LOW);
      ACC_R_LEFT_SC = 0;
      ACC_R_RIGHT_SC = 0;
      ACC_R_UP_SC = 0;
      ACC_R_DOWN_SC = 0;
      ACC_R_RISE_SC = 0;
      ACC_R_FALL_SC = 0;
    }
  delay(40);
}