Diff: main.cpp

Revision:

0:0ce0b0fb5215

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 21 08:42:39 2017 +0000
@@ -0,0 +1,79 @@
+#include "QEI.h"
+#include "mbed.h"
+
+
+Serial pc(USBTX, USBRX);
+DigitalIn enA(p21);
+DigitalIn enB(p22);
+
+DigitalIn sw(p23);
+DigitalIn sw2(p24);
+
+DigitalIn lim1(p7);
+DigitalIn lim2(p8);
+
+
+#define target 300.0 // :mm
+//#define target2 0 // :mm
+Timer t;
+
+//AnalogIn aIn_1(PTD4);    // Command input 1(0.0 to 1.0).
+//AnalogIn aIn_2(PTD5);    // Command input 2(0.0 to 1.0).
+ // tx, rx for PC terminal.
+Serial sabertooth(p13,p14);  // tx, rx for Sabertooth.
+
+//Use X4 encoding.
+//QEI wheel(PTD5 A, PTD4 B, NC, 200, QEI::X4_ENCODING);
+//Use X2 encoding by default.
+QEI wheel (p21, p22, NC, 1024);//
+
+double len =0;
+
+int main() {
+    sw.mode(PullUp);
+    sw2.mode(PullUp);
+    
+    sabertooth.baud(9600); // Set baudrate of serial port for Sabertooth.
+    //char cmd_1=0 ;
+    sabertooth.putc(0);   // Send "Stop" to Motor 1 and 2.
+    //double j=0;
+    t.start();
+    
+    while(1){
+    
+    len = wheel.getPulses()*(157.0/(1024.0*2.0));//一回転で進む距離
+    pc.printf("lenghth is: %.0f mm\n", len);
+    pc.printf("pulse: %d \n", wheel.getPulses());
+    
+    
+    pc.printf("%.0f seconds\n", t.read());
+    
+    
+    if(sw.read()== 1 ){
+        if(lim1 == 0){
+            //cmd_1 = (char)(80);//full_reverse=1 forward,stop=63,full=127 back
+            sabertooth.putc(207);   // Output cmd_1 to Motor 1(Right).
+            }
+        else{
+            sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
+            }
+        }
+          
+    else if(sw2.read()== 1 ){
+        if(lim2 == 0){
+            //cmd_1 = (char)(80);//full_reverse=1 forward,stop=63,full=127 back
+            sabertooth.putc(172);   // Output cmd_1 to Motor 1(Right).
+            }
+        else{
+            sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
+            }
+          }
+          
+    else{
+            sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
+            }
+        
+        wait_ms(1);
+        //j++;
+    }
+    }