RCTank v1.2 Synchronous Drive using Hall Effect Sensors

Four magnets OD6mmx10mm were mounted in the main rotor tracks.

Using two ratiometric hall effect sensors.


UGN3503UA Hall Effect Sensor

Feed into an Arduino Leonardo through the analog ports A0 and A1.

CODE


#include <avr/interrupt.h>
#define DEBUGint v1,v2;int counthall1=0;int counthall2=0;
boolean rise1;uint32_t result1;uint32_t ulStart1;  uint32_t count1;unsigned int result_array1[20];boolean rise2;uint32_t result2;uint32_t ulStart2;  uint32_t count2;unsigned int result_array2[20];uint32_t hall_start1,hall_start2;uint32_t hall_result1,hall_result2;boolean trigger3,trigger4;/////////////////////////////////////////////////////////////////////////////////////////////////                SETUP/////////////////////////////////////////////////////////////////////////////////////////////////void setup(void){        pinMode(8,OUTPUT);//DIRA1  pinMode(9,OUTPUT);//DIRA2  pinMode(10,OUTPUT);//PWM SpeedA
  pinMode(5,OUTPUT);//DIRB1  pinMode(7,OUTPUT);//DIRB2  pinMode(6,OUTPUT);//PWM SpeedB    analogWrite(10,0);//slow stop  digitalWrite(8,LOW);  digitalWrite(9,LOW);
  analogWrite(6,0);//slow stop  digitalWrite(5,LOW);  digitalWrite(7,LOW);      pinMode(2,INPUT);//wireless channel 1   pinMode(3,INPUT);//wireless channel 2  pinMode(A1 ,INPUT);           // set pin to input//digitalWrite(1, HIGH);       // turn on pullup resistors  pinMode(A0, INPUT);           // set pin to input   
 rise1=false; rise2=false;
  result1=0; result2=0;  count1=0;count2=0; Serial.begin(115200);while(!Serial){}

Serial.println("hello");
  // Print a message to the Serial.  Serial.print("Tankie v1");
//delay(1000);//5-rs//6-rw//11-e  /*             digitalWrite(12, HIGH); //Establishes forward direction of Channel A     digitalWrite(9, LOW);   //Disengage the Brake for Channel A      analogWrite(3,0);   //Spins the motor on Channel A at full speed         digitalWrite(13, HIGH); //Establishes forward direction of Channel A     digitalWrite(8, LOW);   //Disengage the Brake for Channel B      analogWrite(11,0);   //Spins the motor on Channel A at full speed      delay(1000);*/  attachInterrupt(1, trigger1,CHANGE);  attachInterrupt(0, trigger2,CHANGE); }







///////////////////////////////////////////////////////////////////////////////////////////////////    MAIN LOOP//////////////////////////////////////////////////////////////////////////////////////////////
long t1=0,t2=0;double ratio=1.0,diff;double v=0.0;                              //void loop(void){   //  Serial.print("t1:");Serial.print(result1);//Serial.print("\t");   //Serial.setCursor(0, 1);  // Serial.print("t2:");Serial.println(result2);      int throttle1=(result1-1500)/2;//(result2-1500)/2+(result1-1500)/4;  int throttle2=(result2-1500)/2;//(result2-1500)/2-(result1-1500)/4;    if(throttle1>5)v=v+(double)throttle1/300.0;  else if(throttle1<-5)v=v+(double)throttle1/300.0; // else v=v/1.7;Serial.print("::V::("); Serial.print((int)v);Serial.print(")::");  if (v>255.9)v=255.0;  if(v<-255.9)v=-255.0;    if (v<50.0 && v>-50.0){             digitalWrite(8, LOW);       digitalWrite(9,LOW);      analogWrite(10,0);        digitalWrite(5, LOW);       digitalWrite(7,LOW);      analogWrite(6,0);           }    else if(v>=50.0 || v<=-50.0){      if(v>=50.0){           digitalWrite(8, HIGH);           digitalWrite(9,LOW);          digitalWrite(7, HIGH);           digitalWrite(5,LOW);      }      if(v<=-50.0){             digitalWrite(9, HIGH);            digitalWrite(8,LOW);            digitalWrite(5, HIGH);            digitalWrite(7,LOW);          }            float delta_count=1.0*abs(counthall1-counthall2);      float K=50.0;      float dc_k=delta_count/K;      float vr1=abs(v)*dc_k;      float vr2=abs(v)/dc_k;            if(counthall1<counthall2){ v1=vr1;}      else if(counthall1>counthall2){ v1=vr2;}      else v1=abs(v);      if(v1>255)v1=255;      analogWrite(10,v1);              if(counthall1>counthall2){ v2=vr2;}      else if(counthall1<counthall2){v2=vr1;}      else v2=abs(v);      if(v2>255)v2=255;      analogWrite(6,v2);   Serial.print("velocity("); Serial.print(v1);Serial.print(".||.");Serial.print(v2);Serial.print(")::");        }    t2=t1;    t1=micros();    int delta_t=t1-t2;Serial.print("delta_t:");Serial.print(delta_t);Serial.print(":::");int hall1=analogRead(A0);int hall2=analogRead(A1);if(hall1>500)trigger3=true;if(hall1<490 && trigger3){  trigger3=false; counthall1++; }
if(hall2>620)trigger4=true;if(hall2<590 && trigger4){  trigger4=false; counthall2++; }
Serial.print(" hallcount:(");Serial.print(counthall1);Serial.print(",");Serial.print(counthall2);Serial.print(")::   ");
Serial.print(hall1);Serial.print(",");
Serial.println(hall2);      //  Serial.print("ratio:");Serial.print(ratio); // Serial.print("v:");Serial.println(v);  // print the number of seconds since reset: // Serial.print(micros());//  Serial.print("\t");

}//END LOOP                              ////////////////////////////////




/////////////////////////////////////////////////////////////////////////////////// Interrupt Service Routine attached to INT0 vector///////////////////////////////////////////////////////////////////////////////




void trigger1(){ 
  if(digitalRead(2) )  {    rise1=true;    ulStart1 = (uint32_t)micros();
  }  else     if(rise1)  { result1=(unsigned int)((uint32_t)(micros()) - ulStart1);    rise1=false;  }  }
void trigger2(){     if(digitalRead(3) )  {    rise2=true;    ulStart2 = (uint32_t)micros();
  }  else     if(rise2)  { result2=(unsigned int)((uint32_t)(micros()) - ulStart2);        rise2=false;  }}