---
title: Motor control
tags: Implementation of IoT class
---

# Motor Control
## Motor...
JGB37-520

## Motor Driver
~~L298N~~
==Apparently, with JGB37-530 motor, most likely to fry this board==

#define outputA 6
#define outputB 7
#define in3 8
#define in4 9
#define enB 10
int counter = 0;
int aState;
int bState;
int aLastState;

void setup() {
  pinMode(outputA, INPUT);
  pinMode(outputB, INPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enB, OUTPUT);

  Serial.begin (9600);
  // Reads the initial state of the outputA
  aLastState = digitalRead(outputA);
}

Function that makes almost perfect 1 rotation

void rotR() {
  int state;
  //Check var will be used to see whther or not it passed slit or not.
  bool check = false;
  //Around 21 clicks are 1 rotation with 6V and analogwrite 255, this cahnges by voltage and analogwrite value.
  for(int i = 0; i < 21; i++){
    check = false;
    //While it's still in the same slit, motor will keep turning.
    while(check == false){
      if(digitalRead(outputA) == HIGH){
        state = 1;
      }else{
        state = 0;
      }
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW);
      analogWrite(enB, 255);
      if(digitalRead(outputA) != state){
        check = true;
        digitalWrite(in3, LOW);
        digitalWrite(in4, LOW);
      }else{
      check = false;
      }
      
    }
  }
}

void loop() {
  int i, aCnt=0, bCnt=0;
  int cur_cnt = counter;
  rotR();
  delay(1000);
  if(cur_cnt > counter){
      Serial.println("Right!!!");
  }
  if(counter > cur_cnt){
      Serial.println("Left!!!");
  }

  delay(100);
  aLastState = aState; // Updates the previous state of the outputA with the current state
}

## Thoughts
So far, I beleive this code is good enough to be accurate, but drift will accumulate by time, which might cause large drift. But we'll see.
And curretnly, its under voltaged, so, I don't kno w what 12V could do, this code might not be enough at all.

void check_motor1_stats(int  *state_1A, int *state_1B){
  if(digitalRead(output_motor1_A) == HIGH && digitalRead(output_motor1_B) == HIGH){
        *state_1A = 1;
        *state_1B = 1;
      }
      else if(digitalRead(output_motor1_A) == HIGH && digitalRead(output_motor1_B) == LOW){
        *state_1A = 1;
        *state_1B = 0;
      }
      else if(digitalRead(output_motor1_A) == LOW && digitalRead(output_motor1_B) == HIGH){
        *state_1A = 0;
        *state_1B = 1;
      }
      else if(digitalRead(output_motor1_A) == LOW && digitalRead(output_motor1_B) == LOW){
        *state_1A = 0;œ
        *state_1B = 0;
      }
}
void check_motor2_stats(int *state_2A, int *state_2B){
  if(digitalRead(output_motor2_A) == HIGH && digitalRead(output_motor2_B) == HIGH){
        *state_2A = 1;
        *state_2B = 1;
      }
      else if(digitalRead(output_motor2_A) == HIGH && digitalRead(output_motor2_B) == LOW){
        *state_2A = 1;
        *state_2B = 0; 
      }
      else if(digitalRead(output_motor2_A) == LOW && digitalRead(output_motor2_B) == HIGH){
        *state_2A = 0;
        *state_2B = 1;
      }
      else if(digitalRead(output_motor2_A) == LOW && digitalRead(output_motor2_B) == LOW){
        *state_2A = 0;
        *state_2B = 0; 
      }
}

void rotR(){
  int state1A, state1B, state2A, state2B;
  bool check_A = false;
  bool check_B = false;
  
  for(int i = 0; i < 220; i++){
    check_A = false; 
    check_B = false;
    
    while(check_A == false && check_B == false){
      
      //Reads intial conditon of motor position
      int a,b;
      check_motor1_stats(&a, &b);
      state1A = a, state1B = b;
      check_motor2_stats(&a, &b);
      state2A = a, state2B = b;
      
      //Serial.println("Called !!!");
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW); 
      analogWrite(enA, 225);
      analogWrite(enB, 225);
      //delay(5);
      
      //Checks motor position changed or not 
      int test1A, test1B, test2A, test2B;
      check_motor1_stats(&a, &b);
      test1A = a, test1B = b;
      check_motor2_stats(&a, &b);
      test2A = a, test2B = b;
      if((state1A != test1A || state1B != test1B) && (state2A != test2A || state2B != test2B)){
        check_A = true;
        check_B = true;

      }else{
        check_A = false;
        check_B = false;
      }
      
    }

  }
  /*When rotation is done, motor should stop, but because we use 12V, just turning off the motor won't be enough, due to
  charactaristics of coil (coil is very analog not good at On/Off, it can only gradually On/Off), so below code
  spins motor backwards to stop motor efficiently. By reversing for short period, it offsets "gradually stop" section.
  */
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    analogWrite(enA, 255);
    analogWrite(enB, 255);
    delay(80);
    //analogWrite(enA, 227);
    //analogWrite(enB, 227);
    //delay(20);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);  
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
}