Diff: main.cpp

Revision:

3:497ac7d026b5

Parent:

2:00535b62c344

Child:

4:abf0070897ff

--- a/main.cpp	Sun Feb 24 10:08:15 2019 +0000
+++ b/main.cpp	Sun Feb 24 11:32:05 2019 +0000
@@ -1,5 +1,8 @@
 #include "mbed.h"
 
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+
 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
 AnalogIn ANALOG0(A0);
 AnalogIn ANALOG1(A1);
@@ -19,8 +22,21 @@
 DigitalOut  LAIN1(PC_9);
 DigitalOut  LAIN2(PC_8);
 DigitalOut  LSTANDBY(PB_8);
-//InterruptIn   ENCODER1(PC_7); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
-//DigitalIn   ENCODER2(PB_6); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
+//InterruptIn MRB_ENC1(PB_6);
+//InterruptIn MRB_ENC2(PC_7);
+InterruptIn MRF_ENC1(PA_13);
+InterruptIn MRF_ENC2(PB_7);
+//InterruptIn MLB_ENC1(PD_2);
+//InterruptIn MLB_ENC2(PC_12);
+InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
+InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
+Ticker      control_timer;
+
+bool        control_loop_flag = false;
+
+//motor associations within arrays
+//MRB = 0; MRF = 1; MLB = 2; MLF = 3
+int m_count [4] = {0,0,0,0}; //setup array for 4 encoder counts
 int m_speed_ref [4] = {0,0,0,0}; //setup array for 4 motor speeds
 float m_duty [4] = {0.0, 0.0, 0.0, 0.0}; //setup array for motor pwm duty
 int m_distance_ref [4] = {0,0,0,0}; //setup array for 4 motor distances
@@ -34,26 +50,45 @@
 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
 void spiComms(); //do spi communications with raspberry pi
 
+//void mrbEncoderIsr1();
+//void mrbEncoderIsr2();
+void mrfEncoderIsr1();
+void mrfEncoderIsr2();
+//void mlbEncoderIsr1();
+//void mlbEncoderIsr2();
+void mlfEncoderIsr1();
+void mlfEncoderIsr2();
+
 
 int main()
 {
     int i = 0; //temp counter variable 
+    //setup interrupts for encoders
+    MRB_ENC1.fall(&mrbEncoderIsr1);
+    MRB_ENC2.fall(&mrbEncoderIsr2);
+    MRF_ENC1.fall(&mrfEncoderIsr1);
+    MRF_ENC2.fall(&mrfEncoderIsr2);
+    MLB_ENC1.fall(&mlbEncoderIsr1);
+    MLB_ENC2.fall(&mlbEncoderIsr2);
+    MLF_ENC1.fall(&mlfEncoderIsr1);
+    MLF_ENC2.fall(&mlfEncoderIsr2);
+    
     //setup driver pins
     setLMotorDir(1);
     setRMotorDir(1);
     // Set PWM period in us
     setPeriod(100);
+    pc.printf("Starting up");
+    
     while (1) {
+        pc.printf("Motor count %i\r\n", m_count[0]);
         //write motor speed
         for (i=0;i<=3;i++){
             m_duty[i] = float(m_speed_ref[i])/100.0;
         }       
-        //set all the duty cycles to the actual motor drivers
-        setDuty();
-        //read all the ADC values when not doing something else
-        readADC();       
-        //do SPI communication stuff
-        spiComms();
+        setDuty(); //set all the duty cycles to the motor drivers
+        readADC(); //read all the ADC values when not doing something else
+        spiComms(); //do SPI communication stuff
     }
 }
 
@@ -156,4 +191,60 @@
             rpi.reply(0x00);
         }
     }
-}
\ No newline at end of file
+}
+void mrbEncoderIsr1(){
+    if(MRB_ENC2 == 0) { 
+ //       m_count[0] ++;
+    } else {
+ //       m_count[0] --;
+    }
+}
+void mrbEncoderIsr2(){
+    if(MRB_ENC1 == 1) {
+        m_count[0] ++;
+    } else {
+        m_count[0] --;
+    }
+}
+void mrfEncoderIsr1(){
+    if(MRF_ENC2 == 0) { 
+        m_count[1] ++;
+    } else {
+        m_count[1] --;
+    }
+}
+void mrfEncoderIsr2(){
+    if(MRF_ENC1 == 1) {
+        m_count[1] ++;
+    } else {
+        m_count[1] --;
+    }
+}
+void mlbEncoderIsr1(){
+    if(MLB_ENC2 == 0) {
+        m_count[2] ++;
+    } else {
+        m_count[2] --;
+    }
+}
+void mlbEncoderIsr2(){
+    if(MLB_ENC1 == 1) {
+        m_count[2] ++;
+    } else {
+        m_count[2] --;
+    }
+}
+void mlfEncoderIsr1(){
+    if(MLF_ENC2 == 0) {
+        m_count[3] ++;
+    } else {
+        m_count[3] --;
+    }
+}
+void mlfEncoderIsr2(){
+    if(MLF_ENC1 == 1) {
+        m_count[3] ++;
+    } else {
+        m_count[3] --;
+    }
+}