1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
|
#define AIN1 0 // H-bridge A, input pin 1
#define AIN2 1 // H-bridge A, input pin 2
#define BIN1 2 // H-bridge B, input pin 1
#define BIN2 3 // H-bridge B, input pin 2
#define hallA A0 // Hall A input pin
#define hallB A1 // Hall B input pin
#define threshold 128 // Hall sensor is near north pole if value > threshold
int polarity,old_polarity,ticks,start_time,speed;
void setup(){
analogReadResolution(8); // set A/D for 8 bit resolution
pinMode(AIN1,OUTPUT); // set H-bridge control pins as Teensy outputs
pinMode(AIN2,OUTPUT);
pinMode(BIN1,OUTPUT);
pinMode(BIN2,OUTPUT);
Serial.begin(9600);
old_polarity = 0;
ticks = 0;
start_time = millis();
}
void off_on(int sensor,int pin1,int pin2){
if(sensor<threshold){
digitalWrite(pin2,LOW); // turn first control pin off
digitalWrite(pin1,HIGH); // then turn second control pin on
}else{
digitalWrite(pin1,LOW); // turn first control pin off
digitalWrite(pin2,HIGH); // then turn second control pin on
}
}
void loop() {
off_on(analogRead(hallA),AIN1,AIN2);
off_on(analogRead(hallB),BIN2,BIN1);
polarity = analogRead(hallA)>threshold;
if(polarity==1 && old_polarity==0){
ticks += 1;
if(ticks>300){
speed = 100*1000*60/(millis()-start_time);
Serial.println(speed);
ticks = 0;
start_time = millis();
}
}
old_polarity = polarity;
}
|
