Diff: main.cpp

Revision:

2:00535b62c344

Parent:

1:08315c315df0

Child:

3:497ac7d026b5

diff -r 08315c315df0 -r 00535b62c344 main.cpp
--- a/main.cpp	Sat Feb 23 16:45:39 2019 +0000
+++ b/main.cpp	Sun Feb 24 10:08:15 2019 +0000
@@ -27,94 +27,133 @@
 int max_accel = 0; //for storing the maximum acceleration
 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
 
+void readADC(); //read 6 channels of ADC
+void setDuty(); //set the 4 motor PWM duty cycles
+void setPeriod(int period); //set PWM period for motors in microseconds
+void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
+void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
+void spiComms(); //do spi communications with raspberry pi
+
 
 int main()
 {
-    int i = 0; //temp counter variable
-    int v = 0; //temp SPI variable
-    
-
+    int i = 0; //temp counter variable 
     //setup driver pins
-    LAIN1 = 1;
-    LAIN2 = 0;
-    LSTANDBY = 1;
-    RAIN1 = 1;
-    RAIN2 = 0;
-    RSTANDBY = 1;
-    // Set PWM
-    MRB.period_us(100);
-    MRB.write(0.0);
-    MRF.period_us(100);
-    MRF.write(0.0);
-    MLB.period_us(100);
-    MLB.write(0.0);
-    MLF.period_us(100);
-    MLF.write(0.0);
-
-    //setup Spi
- //   rpi.reply(0x00); //set default reply
-
+    setLMotorDir(1);
+    setRMotorDir(1);
+    // Set PWM period in us
+    setPeriod(100);
     while (1) {
-        
         //write motor speed
         for (i=0;i<=3;i++){
             m_duty[i] = float(m_speed_ref[i])/100.0;
-        }
-        MRB.write(m_duty[0]);
-        MRF.write(m_duty[1]);
-        MLB.write(m_duty[2]);
-        MLF.write(m_duty[3]);
-        
-//read all the ADC values when not doing something else
-        adc_values[0] = int(ANALOG0.read()*255);
-        adc_values[1] = int(ANALOG1.read()*255);
-        adc_values[2] = int(ANALOG2.read()*255);
-        adc_values[3] = int(ANALOG3.read()*255);
-        adc_values[4] = int(ANALOG4.read()*255);
-        adc_values[5] = int(ANALOG5.read()*255);
-        
-        
+        }       
+        //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
-        if(rpi.receive()) {
-            v = rpi.read();   // Read byte from master
-            if(v == char('S')){
-                rpi.reply(0x01);
-                for (i=0;i<=3;i++){                   
-                    m_speed_ref[i] = rpi.read() - 128;
-                    rpi.reply(m_speed_ref[i]);
-                }
-                v = rpi.read(); //last bit setup a blank reply
-                rpi.reply(0x00);
+        spiComms();
+    }
+}
+
+//function to read all 6 ADC channels
+void readADC(){
+    adc_values[0] = int(ANALOG0.read()*255);
+    adc_values[1] = int(ANALOG1.read()*255);
+    adc_values[2] = int(ANALOG2.read()*255);
+    adc_values[3] = int(ANALOG3.read()*255);
+    adc_values[4] = int(ANALOG4.read()*255);
+    adc_values[5] = int(ANALOG5.read()*255);
+}
+
+//function to set all 4 motor PWM duty cycles
+void setDuty(){
+    MRB.write(m_duty[0]);
+    MRF.write(m_duty[1]);
+    MLB.write(m_duty[2]);
+    MLF.write(m_duty[3]);
+}
+//set left motor direction. 1 is forward, 0 is backwards.
+void setLMotorDir(bool direction){
+    LSTANDBY = 1;
+    if(direction == true){
+        LAIN1 = 1;
+        LAIN2 = 0;
+    }
+    else if(direction == false){
+        LAIN1 = 0;
+        LAIN2 = 1;
+    }
+}
+//set right motor direction. 1 is forward, 0 is backwards.
+void setRMotorDir(bool direction){
+    RSTANDBY = 1;
+    if(direction == true){
+        RAIN1 = 0;
+        RAIN2 = 1;
+    }
+    else if(direction == false){
+        RAIN1 = 1;
+        RAIN2 = 0;
+    }
+}
+
+//initialisation function to set motor PWM period and set to 0 duty
+void setPeriod(int period){
+    MRB.period_us(period);
+    MRB.write(0.0);
+    MRF.period_us(period);
+    MRF.write(0.0);
+    MLB.period_us(period);
+    MLB.write(0.0);
+    MLF.period_us(period);
+    MLF.write(0.0);
+}
+
+void spiComms(){
+       //do SPI communication stuff
+    int i = 0; //temp counter variable
+    int v = 0; //temp SPI variable
+    if(rpi.receive()) {
+        v = rpi.read();   // Read byte from master
+        if(v == char('S')){
+            rpi.reply(0x01);
+            for (i=0;i<=3;i++){                   
+                m_speed_ref[i] = rpi.read() - 128;
+                rpi.reply(m_speed_ref[i]);
             }
-            else if(v == char('D')){
-                rpi.reply(0x02);
-                for (i=0;i<=3;i++){                   
-                    m_speed_ref[i] = rpi.read() - 128;
-                    rpi.reply(m_speed_ref[i]);
-                }
-                for (i=0;i<=3;i++){                   
-                    m_distance_ref[i] = rpi.read() - 128;
-                    rpi.reply(m_distance_ref[i]);
-                }
-                v = rpi.read(); //last bit setup a blank reply
-                rpi.reply(0x00);
+            v = rpi.read(); //last bit setup a blank reply
+            rpi.reply(0x00);
+        }
+        else if(v == char('D')){
+            rpi.reply(0x02);
+            for (i=0;i<=3;i++){                   
+                m_speed_ref[i] = rpi.read() - 128;
+                rpi.reply(m_speed_ref[i]);
+            }
+            for (i=0;i<=3;i++){                   
+                m_distance_ref[i] = rpi.read() - 128;
+                rpi.reply(m_distance_ref[i]);
             }
-            else if(v == char('A')){
-                rpi.reply(0x03);                 
-                max_accel = rpi.read();
-                rpi.reply(max_accel);
-                v = rpi.read(); //last bit setup a blank reply
-                rpi.reply(0x00);
+            v = rpi.read(); //last bit setup a blank reply
+            rpi.reply(0x00);
+        }
+        else if(v == char('A')){
+            rpi.reply(0x03);                 
+            max_accel = rpi.read();
+            rpi.reply(max_accel);
+            v = rpi.read(); //last bit setup a blank reply
+            rpi.reply(0x00);
+        }
+        else if(v == char('V')){
+            rpi.reply(0x04);                 
+            v = rpi.read();
+            if(v <= 6){ //check the ADC to be addressed exists
+                rpi.reply(adc_values[v]);
             }
-            else if(v == char('V')){
-                rpi.reply(0x04);                 
-                v = rpi.read();
-                if(v <= 6){ //check the ADC to be addressed exists
-                    rpi.reply(adc_values[v]);
-                }
-                v = rpi.read(); //last bit setup a blank reply
-                rpi.reply(0x00);
-            }
+            v = rpi.read(); //last bit setup a blank reply
+            rpi.reply(0x00);
         }
     }
-}
+}
\ No newline at end of file