Automatic Teeth Brushing Robot

//diyprojectslab.com for more info
#include <AccelStepper.h>
#include <Servo.h>


int brush_rail_last_pos = 0;
int brush_last_pos = 0;
int bowl_last_pos  = 0;
int water = 8;
int paste = 5;

const int trigPin = 3;
const int echoPin = 4;
long duration;
int distance = 100;

int brush_rail = 0;
int brush = 0;
int bowl = 0;

Servo brush_servo;
Servo water_servo;
AccelStepper brush_rail_stepper(1, 35, 33);  // step, dir  35,33
AccelStepper brush_stepper(1, 47, 45);  // step, dir
AccelStepper bowl_stepper(1, 41, 39);
int sread = 2;
int all_distance_to_go = 1;

// paste pos is 10
// brush pos is 80
// brush water pos is BRUSH 160 WATER 140.


void setup() {

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  brush_servo.attach(7);
  water_servo.attach(6);

  pinMode(water, OUTPUT);
  pinMode(paste, OUTPUT);
  digitalWrite(water, HIGH);
  digitalWrite(paste, HIGH);
  Serial.begin(9600);
  brush_rail_stepper.setPinsInverted(true, false, true);
  brush_rail_stepper.setAcceleration(1000);
  brush_rail_stepper.setMaxSpeed(10000);

  brush_stepper.setPinsInverted(false, false, true);
  brush_stepper.setAcceleration(300);
  brush_stepper.setMaxSpeed(10000);

  bowl_stepper.setPinsInverted(false, false, true);
  bowl_stepper.setAcceleration(1000);
  bowl_stepper.setMaxSpeed(10000);


  //while (Serial.available() == 0);
  delay(1000);
  while (distance > 50) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    // Calculating the distance
    distance = duration * 0.034 / 2;
    // Prints the distance on the Serial Monitor
    Serial.print("Distance: ");
    Serial.println(distance);
    delay(100);
  }

  brush_rail_stepper.moveTo(50);
  brush_rail_stepper.runToPosition();
  //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
  bowl_stepper.moveTo(50);
  bowl_stepper.runToPosition();

  //delay(4000);
  brush_rail_stepper.setCurrentPosition(0);

  brush_rail_stepper.setCurrentPosition(0);
  brush_rail_stepper.setAcceleration(20000);
  brush_rail_stepper.setMaxSpeed(9000);

  brush_stepper.setAcceleration(20000);
  brush_stepper.setMaxSpeed(9000);

  bowl_stepper.setCurrentPosition(0);
  bowl_stepper.setAcceleration(20000);
  bowl_stepper.setMaxSpeed(9000);





}

void loop() {


  water_servo.write(140);
  brush_servo.write(160);
  brush_rail = 20169;   /// going for brush wahshig under the water
  brush = 2150;
  bowl = 0;
  go();



  //brush water pos is BRUSH 160 WATER 140.


  digitalWrite(water, LOW);
  delay(2000);
  digitalWrite(water, HIGH);
  delay(1000);
  // pump water to the brush

  brush_rail = 455;
  brush = 2430;    //// going under the paste
  bowl = 12518;
  go();


  brush_rail = 400;
  brush = 6800;   //// under the paste, brush turn
  bowl = 12518;
  go();

  brush_servo.write(10);

  digitalWrite(paste, LOW);
  delay(700);
  digitalWrite(paste, HIGH);

  delay(1000);

  brush_rail = 800;
  brush = 6850;     //after paste move
  bowl = 12518;
  go();

  delay(700);//xxxxxxxx dispance paste



  brush_rail = 848;
  brush = 6800;    /// moving bowl to the face
  bowl = 12518;
  go();

  water_servo.write(40);
  digitalWrite(water, LOW);
  delay(1500);
  digitalWrite(water, HIGH);
  delay(2000);// pump water to the mouth

  brush_rail = 848;
  brush = 6800;      //moving bowl back to zero after mouth wash
  bowl = 0;
  go();



  brush_rail = 17293;
  brush = 6800;             /// bringing pasted brush to the mouth
  bowl = 0;
  go();


  brush_servo.write(80);
  delay(1000);



  /*for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
    }
  */



  for (int i = 0; i <= 18; i++) {
    brush_rail = 17293 + 200;
    brush = 6800;
    bowl = 0;
    go();
    //delay(10); //xxxxxxx     brush rail move 100 each side to brush with for loop
    brush_rail = 17293 - 200 ;
    brush = 6800;
    bowl = 0;
    go();
  }

  brush_rail = 17293;
  brush = 6800;             /// brush at center before moving out
  bowl = 0;
  go();

  delay(2000);
  brush_rail = 7487;
  brush = 2500;       /// switching brushing side
  bowl = 0;
  go();


  delay(1000);
  for (int i = 0; i <= 18; i++) {
    brush_rail = 7487 + 200;
    brush = 2500;
    bowl = 0;
    go();
    //delay(10); //xxxxxxx     brush rail move 100 each side to brush with for loop
    brush_rail = 7487 - 200 ;
    brush = 2500;
    bowl = 0;
    go();
  }

  brush_rail = 7487;
  brush = 2500;             /// brush at center before moving out
  bowl = 0;
  go();

  delay(1000);
  brush_rail = 0;
  brush = 1402;     // moving the brush the side and bringing the bowl to spit
  bowl = 12570;
  go();

  delay(2000);

  brush_rail = 7487;
  brush = 2500;      /// switching brushing side bringing back the brush after spit
  bowl = 0;
  go();




  delay(1000);
  for (int i = 0; i <= 18; i++) {
    brush_rail = 7487 + 200;
    brush = 2500;
    bowl = 0;
    go();
    //delay(10); //xxxxxxx     brush rail move 100 each side to brush with for loop
    brush_rail = 7487 - 200 ;
    brush = 2500;
    bowl = 0;
    go();
  }

  brush_rail = 7487;
  brush = 2500;             /// brush at center before moving out
  bowl = 0;
  go();
  //xxxxxxx     brush rail move 100 each side to brush with for loop


  delay(1000);




  brush_rail = 17293;
  brush = 6800;              /// bringing pasted brush to the mouth
  bowl = 0;
  go();






  for (int i = 0; i <= 18; i++) {
    brush_rail = 17293 + 200;
    brush = 6800;
    bowl = 0;
    go();
    //delay(10); //xxxxxxx     brush rail move 100 each side to brush with for loop
    brush_rail = 17293 - 200 ;
    brush = 6800;
    bowl = 0;
    go();
  }

  brush_rail = 17293;
  brush = 6800;             /// brush at center before moving out
  bowl = 0;
  go();

  delay(1000);
  //xxxxxxx     brush rail move 100 each side to brush with for loop






  brush_rail = 0;
  brush = 1402;      // moving the brush the side and bringing the bowl to spit
  bowl = 12570;
  go();
  delay(3000);
  water_servo.write(40);
  delay(500);
  digitalWrite(water, LOW);
  delay(2000);
  digitalWrite(water, HIGH);
  delay(3500);// pump water to the mouth
  //xxx after some second pup water to wash mouth with servo to the mouth xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
  digitalWrite(water, LOW);
  delay(2000);
  digitalWrite(water, HIGH);
  delay(3500);// pump water to the mouth

  brush_rail = 20179;   /// going for brush wahshig under the water
  brush = 2150;
  bowl = 0;
  go();

  //brush water pos is BRUSH 160 WATER 140.
  brush_servo.write(160);

  water_servo.write(140);
  delay(500);
  digitalWrite(water, LOW);
  delay(1500);
  brush_rail = 20279;   /// going for brush wahshig mvoe to cleane full
  brush = 2150;
  bowl = 0;
  go();
  delay(1500);
  digitalWrite(water, HIGH);
  delay(1000);// pump water to the mouth



  //brush_rail = 0;   /// goinng to zero expect brush
  // brush = 2430;
  //bowl = 0;
  //go();

  brush_stepper.setAcceleration(5000);
  brush_stepper.setMaxSpeed(10000);

  brush_rail = 50;   /// all zero
  brush = 0;
  bowl = 0;
  go();




  while (true);


}






void go() {

  brush_rail_stepper.moveTo(brush_rail);
  brush_stepper.moveTo(brush);
  bowl_stepper.moveTo(bowl);


  while (all_distance_to_go != 0) {

    all_distance_to_go = brush_rail_stepper.distanceToGo() + brush_stepper.distanceToGo() + bowl_stepper.distanceToGo();
    brush_rail_stepper.run();
    brush_stepper.run();
    bowl_stepper.run();
  }

  all_distance_to_go = 1;
}