CRシャトル

Dependencies:   QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "QEI.h"
00002 #include "mbed.h"
00003 
00004 
00005 Serial pc(USBTX, USBRX);
00006 DigitalIn enA(p21);
00007 DigitalIn enB(p22);
00008 
00009 DigitalIn sw(p23);
00010 DigitalIn sw2(p24);
00011 
00012 DigitalIn lim1(p7);
00013 DigitalIn lim2(p8);
00014 
00015 
00016 #define target 300.0 // :mm
00017 //#define target2 0 // :mm
00018 Timer t;
00019 
00020 //AnalogIn aIn_1(PTD4);    // Command input 1(0.0 to 1.0).
00021 //AnalogIn aIn_2(PTD5);    // Command input 2(0.0 to 1.0).
00022  // tx, rx for PC terminal.
00023 Serial sabertooth(p13,p14);  // tx, rx for Sabertooth.
00024 
00025 //Use X4 encoding.
00026 //QEI wheel(PTD5 A, PTD4 B, NC, 200, QEI::X4_ENCODING);
00027 //Use X2 encoding by default.
00028 QEI wheel (p21, p22, NC, 1024);//
00029 
00030 double len =0;
00031 
00032 int main() {
00033     sw.mode(PullUp);
00034     sw2.mode(PullUp);
00035     
00036     sabertooth.baud(9600); // Set baudrate of serial port for Sabertooth.
00037     //char cmd_1=0 ;
00038     sabertooth.putc(0);   // Send "Stop" to Motor 1 and 2.
00039     //double j=0;
00040     t.start();
00041     
00042     while(1){
00043     
00044     len = wheel.getPulses()*(157.0/(1024.0*2.0));//一回転で進む距離
00045     pc.printf("lenghth is: %.0f mm\n", len);
00046     pc.printf("pulse: %d \n", wheel.getPulses());
00047     
00048     
00049     pc.printf("%.0f seconds\n", t.read());
00050     
00051     
00052     if(sw.read()== 1 ){
00053         if(lim1 == 0){
00054             //cmd_1 = (char)(80);//full_reverse=1 forward,stop=63,full=127 back
00055             sabertooth.putc(207);   // Output cmd_1 to Motor 1(Right).
00056             }
00057         else{
00058             sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
00059             }
00060         }
00061           
00062     else if(sw2.read()== 1 ){
00063         if(lim2 == 0){
00064             //cmd_1 = (char)(80);//full_reverse=1 forward,stop=63,full=127 back
00065             sabertooth.putc(172);   // Output cmd_1 to Motor 1(Right).
00066             }
00067         else{
00068             sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
00069             }
00070           }
00071           
00072     else{
00073             sabertooth.putc(191);   // Output cmd_1 to Motor 1(Right).
00074             }
00075         
00076         wait_ms(1);
00077         //j++;
00078     }
00079     }