1 files changed, 47 insertions, 0 deletions
diff --git a/week7/week7.ino b/week7/week7.ino
new file mode 100644
index 0000000..9a27785
--- /dev/null
+++ b/week7/week7.ino
@@ -0,0 +1,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;
+}