Diff: main.cpp

Revision:

4:abf0070897ff

Parent:

3:497ac7d026b5

Child:

5:600c5c9cbb19

--- a/main.cpp	Sun Feb 24 11:32:05 2019 +0000
+++ b/main.cpp	Sun Feb 24 12:00:49 2019 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 
-Serial pc(SERIAL_TX, SERIAL_RX);
+//Serial pc(SERIAL_TX, SERIAL_RX);
 
 
 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
@@ -35,11 +35,11 @@
 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
+//Right = 0; Left = 1
+int m_count [2] = {0,0}; //setup array for 4 encoder counts
+int m_speed_ref [2] = {0,0}; //setup array for 4 motor speeds
+float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
+int m_distance_ref [2] = {0,0}; //setup array for 4 motor distances
 int max_accel = 0; //for storing the maximum acceleration
 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
 
@@ -50,26 +50,19 @@
 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 
+    float right_speed = 0;
+    float left_speed = 1;
     //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);
     
@@ -78,14 +71,36 @@
     setRMotorDir(1);
     // Set PWM period in us
     setPeriod(100);
-    pc.printf("Starting up");
+ //   pc.printf("Starting up");
     
     while (1) {
-        pc.printf("Motor count %i\r\n", m_count[0]);
+ //       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;
-        }       
+        
+        right_speed = float(m_speed_ref[0])/100.0;
+        if(right_speed < 0){
+            setRMotorDir(0); //set the motors backwards
+            right_speed = -1*right_speed; //make the negative value into positive
+        } else {
+            setRMotorDir(1); //set the motors forwards
+        }
+        if(right_speed > 100){ 
+            right_speed = 100; //make sure the speed isn't greater than 100%
+        }
+        m_duty[0] = right_speed;
+        
+        left_speed = float(m_speed_ref[1])/100.0;
+        if(left_speed < 0){
+            setLMotorDir(0); //set the motors backwards
+            left_speed = -1*left_speed; //make the negative value into positive
+        } else {
+            setLMotorDir(1); //set the motors forwards
+        }
+        if(left_speed > 100){ 
+            left_speed = 100; //make sure the speed isn't greater than 100%
+        }
+        m_duty[1] = left_speed;
+               
         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
@@ -105,9 +120,9 @@
 //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]);
+    MRF.write(m_duty[0]);
+    MLB.write(m_duty[1]);
+    MLF.write(m_duty[1]);
 }
 //set left motor direction. 1 is forward, 0 is backwards.
 void setLMotorDir(bool direction){
@@ -154,7 +169,7 @@
         v = rpi.read();   // Read byte from master
         if(v == char('S')){
             rpi.reply(0x01);
-            for (i=0;i<=3;i++){                   
+            for (i=0;i<=1;i++){                   
                 m_speed_ref[i] = rpi.read() - 128;
                 rpi.reply(m_speed_ref[i]);
             }
@@ -163,11 +178,11 @@
         }
         else if(v == char('D')){
             rpi.reply(0x02);
-            for (i=0;i<=3;i++){                   
+            for (i=0;i<=1;i++){                   
                 m_speed_ref[i] = rpi.read() - 128;
                 rpi.reply(m_speed_ref[i]);
             }
-            for (i=0;i<=3;i++){                   
+            for (i=0;i<=1;i++){                   
                 m_distance_ref[i] = rpi.read() - 128;
                 rpi.reply(m_distance_ref[i]);
             }
@@ -192,59 +207,31 @@
         }
     }
 }
-void mrbEncoderIsr1(){
-    if(MRB_ENC2 == 0) { 
- //       m_count[0] ++;
-    } else {
- //       m_count[0] --;
-    }
-}
-void mrbEncoderIsr2(){
-    if(MRB_ENC1 == 1) {
+void mrfEncoderIsr1(){
+    if(MRF_ENC2 == 0) { 
         m_count[0] ++;
     } else {
         m_count[0] --;
     }
 }
-void mrfEncoderIsr1(){
-    if(MRF_ENC2 == 0) { 
-        m_count[1] ++;
+void mrfEncoderIsr2(){
+    if(MRF_ENC1 == 1) {
+        m_count[0] ++;
     } else {
-        m_count[1] --;
+        m_count[0] --;
     }
 }
-void mrfEncoderIsr2(){
-    if(MRF_ENC1 == 1) {
+void mlfEncoderIsr1(){
+    if(MLF_ENC2 == 0) {
         m_count[1] ++;
     } else {
         m_count[1] --;
     }
 }
-void mlbEncoderIsr1(){
-    if(MLB_ENC2 == 0) {
-        m_count[2] ++;
+void mlfEncoderIsr2(){
+    if(MLF_ENC1 == 1) {
+        m_count[1] ++;
     } else {
-        m_count[2] --;
-    }
-}
-void mlbEncoderIsr2(){
-    if(MLB_ENC1 == 1) {
-        m_count[2] ++;
-    } else {
-        m_count[2] --;
+        m_count[1] --;
     }
 }
-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] --;
-    }
-}