GRawler - The Glass Roof Cleaner

/*
Pin 2  - 
Pin 3  - 
Pin 4  - enable motors
Pin 5  - DIR MOTOR 2
Pin 6  - Servo
Pin 7  - STEP MOTOR 1
Pin 8  - DIR MOTOR 1
Pin 9  - 
Pin 10 - - Micro switch LEFT Roller
Pin 16  - Micro switch RIGHT Roller 
Pin 14  - STEP MOTOR 2
Pin 15  - BrusherMotor
Pin A0  - accu stepper
Pin A1  - accu motor
Pin A2
Pin A3
*/
//Bluetooth input stuff------------------------------------------


#define    STX          0x02
#define    ETX          0x03
#define    ServoDown    40    // between 30-60
#define    ServoUp      50
#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))

#define ZERO_SPEED 65535

float joyY, joyX;                                         //smartphone joystick input values
int16_t speed_M1, speed_M2;        // Actual speed of motors
int8_t  dir_M1, dir_M2;            // Actual direction of steppers motors
uint8_t cmd[8] = {0, 0, 0, 0, 0, 0, 0, 0};                 // bytes received
uint8_t buttonStatus = 0;                                  // first Byte sent to Android device


//int analogPin = 0;   // potentiometer connected to analog pin 3
int val = 0;         // variable to store the read value
int Brusher = 15;      // relay brusher motor
int LEFTswitch = 10;
int RIGHTswitch = 16;


  // TIMER 1 : STEPPER MOTOR1 SPEED CONTROL
ISR(TIMER1_COMPA_vect)
{
  if (dir_M1 == 0) // If we are not moving we dont generate a pulse
    return;
  // We generate 1us STEP pulse
 
  SET(PORTE, 6); // STEP MOTOR 1               ---------------------------------   D7   ---------------------------------
 delay_1us();
  CLR(PORTE, 6);
}
// TIMER 3 : STEPPER MOTOR2 SPEED CONTROL
ISR(TIMER3_COMPA_vect)
{
  if (dir_M2 == 0) // If we are not moving we dont generate a pulse
    return;
  // We generate 1us STEP pulse
  
  SET(PORTB, 3); // STEP MOTOR 2                                ---------------------------   PORTD6/D12 -> PORTB3/D14    ------------------------
    delay_1us();
  
  CLR(PORTB, 3); //                                            ---------------------------   PORTD6/D12 -> PORTB3/D14    ------------------------
}










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

//-------------------- setup timer4 for servo pin6 ----------------------------------------------

    pinMode(6, OUTPUT);
    TCCR4B &= ~(_BV(CS43) | _BV(CS42) | _BV(CS41) | _BV(CS40));
    TCCR4B |= _BV(CS43) | _BV(CS41);
    TCCR4D &= ~(_BV(WGM41) | _BV(WGM40));
    TC4H = 624 >> 8; // B10 0x2
    OCR4C = 624 & 0xff;//B1110000 0x70
    TCCR4C |= (1<<COM4D1)|(1<<PWM4D); 




 
  pinMode(LEFTswitch, INPUT_PULLUP );
  pinMode(RIGHTswitch, INPUT_PULLUP );
  // STEPPER PINS 
  pinMode(4, OUTPUT); // ENABLE MOTORS
  pinMode(7, OUTPUT); // STEP MOTOR 1 PORTE,6
  pinMode(8, OUTPUT); // DIR MOTOR 1  PORTB,4
  pinMode(14, OUTPUT); // STEP MOTOR 2 PORTB,3               //  ---------------------------   PORTD6/D12 -> PORTB3/D14    ------------------------
  pinMode(5, OUTPUT); // DIR MOTOR 2  PORTC,6
  
  pinMode(Brusher, OUTPUT);
  digitalWrite(4, HIGH);  // Disbale motors
  digitalWrite(Brusher, HIGH);


  Serial.begin(115200); // Serial output to console
  Serial1.begin(115200);
delay(2000);



   // STEPPER MOTORS INITIALIZATION
  Serial.println("Steper motors initialization...");
  // MOTOR1 => TIMER1
  TCCR1A = 0;                       // Timer1 CTC mode 4, OCxA,B outputs disconnected
  TCCR1B = (1 << WGM12) | (1 << CS11); // Prescaler=8, => 2Mhz  | (1 << CS10)
  OCR1A = ZERO_SPEED;               // Motor stopped
  dir_M1 = 0;
  TCNT1 = 0;

  // MOTOR2 => TIMER3
  TCCR3A = 0;                       // Timer3 CTC mode 4, OCxA,B outputs disconnected
  TCCR3B = (1 << WGM32) | (1 << CS31); // Prescaler=8, => 2Mhz  | (1 << CS30)
  OCR3A = ZERO_SPEED;   // Motor stopped
  dir_M2 = 0;
  TCNT3 = 0;

 // Enable stepper drivers and TIMER interrupts
 // digitalWrite(4, LOW);   // Enable stepper drivers
  // Enable TIMERs interrupts
  TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt
  TIMSK3 |= (1 << OCIE1A); // Enable Timer1 interrupt




}



void loop()

{

  //val = analogRead(analogPin);   // read 1S Akku

  
bluetooth() ;
delay(10);
 //digitalWrite(4, LOW);  // Motors enable

  if (joyY>0){
    if (digitalRead(LEFTswitch)==LOW)
         {setMotorSpeedM1(joyY*3);
          setMotorSpeedM2(0);}
          
     else if (digitalRead(RIGHTswitch)==LOW)
         {setMotorSpeedM1(0);
          setMotorSpeedM2(joyY*3);}
          
     else {setMotorSpeedM1(joyY*3);
           setMotorSpeedM2(joyY*3);}
 
    
  }



   else if (joyY<0){
    if (digitalRead(LEFTswitch)==LOW)
         {setMotorSpeedM1(0);
          setMotorSpeedM2(joyY*3);}
          
     else if (digitalRead(RIGHTswitch)==LOW)
         {setMotorSpeedM1(joyY*3);
          setMotorSpeedM2(0);}
          
     else {setMotorSpeedM1(joyY*3);
           setMotorSpeedM2(joyY*3);}
 
  }
    else {setMotorSpeedM1(0);
           setMotorSpeedM2(0);}

           
Serial.println(joyY);
    Serial.print(" ");
    Serial.println(joyX);  

}






void bluetooth()
{
  if (Serial1.available())   // data received from smartphone
  {
    cmd[0] =  Serial1.read();
    if (cmd[0] == STX)
    {
      int i = 1;
      while (Serial1.available())
      {
        cmd[i] = Serial1.read();
        if (cmd[i] > 127 || i > 7)                   break; // Communication error
        if ((cmd[i] == ETX) && (i == 2 || i == 7))   break; // Button or Joystick data
        i++;
      }
      if      (i == 2)          getButtonState(cmd[1]);    // 3 Bytes  ex: < STX "C" ETX >
      else if (i == 7)          getJoystickState(cmd);     // 6 Bytes  ex: < STX "200" "180" ETX >
    }
  }
}






void getJoystickState(byte data[8])    {
  joyX = (data[1] - 48) * 100 + (data[2] - 48) * 10 + (data[3] - 48); // obtain the Int from the ASCII representation
  joyY = (data[4] - 48) * 100 + (data[5] - 48) * 10 + (data[6] - 48);
  joyX = joyX - 200;                                                  // Offset to avoid
  joyY = joyY - 200;                                                  // transmitting negative numbers

  if (joyX < -100 || joyX > 100 || joyY < -100 || joyY > 100)     return; // commmunication error
}











void getButtonState(int bStatus)  {
  switch (bStatus) {
    // -----------------  BUTTON #1  -----------------------
    case 'A':
      buttonStatus |= B000001;        // ON

      digitalWrite(Brusher, LOW);

      break;
    case 'B':
      buttonStatus &= B111110;        // OFF

      digitalWrite(Brusher, HIGH);

      break;

    // -----------------  BUTTON #2  -----------------------  enable/disable motors
   case 'C':
      buttonStatus |= B000010;        // ON
       digitalWrite(4, LOW);    
      break;
  case 'D':
     buttonStatus &= B111101;        // OFF
       digitalWrite(4, HIGH);
     break;

  // -----------------  BUTTON #3  -----------------------
    case 'E':
      buttonStatus |= B000100;        // ON

      TC4H = ServoUp >> 8;
      OCR4D = ServoUp & 0xff;

      break;
     case 'F':
      buttonStatus &= B111011;        // OFF

       TC4H = ServoDown >> 8;
       OCR4D = ServoDown & 0xff;

      break;


  }}

// 16 single cycle instructions = 1us at 16Mhz
void delay_1us()
{
  __asm__ __volatile__ (
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop" "\n\t"
    "nop");
}


  // Set speed of Stepper Motor1
// tspeed could be positive or negative (reverse)
void setMotorSpeedM1(int16_t speed)
{
  long timer_period;
  //int16_t speed;

  
  if (speed == 0)
  {
    timer_period = ZERO_SPEED;
    dir_M1 = 0;
  }
  else if (speed > 0)
  {
    timer_period = 2000000 / speed; // 2Mhz timer
    dir_M1 = 1;
    SET(PORTB, 4); // DIR Motor 1 (Forward)                     D4  ---------------------------------
  }
  else
  {
    timer_period = 2000000 / -speed;
    dir_M1 = -1;
    CLR(PORTB, 4); // Dir Motor 1
  }
  if (timer_period > 65535)   // Check for minimun speed (maximun period without overflow)
    timer_period = ZERO_SPEED;

  OCR1A = timer_period;
  // Check  if we need to reset the timer...
  if (TCNT1 > OCR1A)
    TCNT1 = 0;
}

// Set speed of Stepper Motor2
// tspeed could be positive or negative (reverse)
void setMotorSpeedM2(int16_t speed)
{
  long timer_period;
  //int16_t speed;

 

  if (speed == 0)
  {
    timer_period = ZERO_SPEED;
    dir_M2 = 0;
  }
  else if (speed > 0)
  {
    timer_period = 2000000 / speed; // 2Mhz timer
    dir_M2 = 1;
    CLR(PORTC, 6);   // Dir Motor2 (Forward)             D5 -------------------------------------
  }
  else
  {
    timer_period = 2000000 / -speed;
    dir_M2 = -1;
    SET(PORTC, 6);  // DIR Motor 2
  }
  if (timer_period > 65535)   // Check for minimun speed (maximun period without overflow)
    timer_period = ZERO_SPEED;

  OCR3A = timer_period;
  // Check  if we need to reset the timer...
  if (TCNT3 > OCR3A)
    TCNT3 = 0;
}