main.cpp

00001 #include "mbed.h"
00002 #include "FEP_RX22.h"
00003 #include "pinconfig.h"
00004 #include "ikarashiMDC.h"
00005 #include "QEI.h"
00006 #include "can_tr.h"
00007 #include "HOSOKIkikou.h"
00008 //#include "SEKIkikou.h"
00009 #include "R1370.h"
00010 
00011 FEP_RX22 mycon(fepTX, fepRX, fepad);
00012 Serial pc(USBTX, USBRX, 115200);
00013 Serial serial(motorTX, motorRX, 115200);
00014 QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
00015 QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
00016 QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
00017 QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
00018 R1370 r1370(jyro_TX,jyro_RX);
00019 DigitalOut Reset(jyro_RST);
00020 DigitalOut led(LED2);
00021 
00022 ikarashiMDC motor[] = {
00023     ikarashiMDC(0,0,SM,&serial),
00024     ikarashiMDC(0,1,SM,&serial),
00025     ikarashiMDC(0,2,SM,&serial),
00026     ikarashiMDC(0,3,SM,&serial),
00027     ikarashiMDC(1,0,SM,&serial),
00028     ikarashiMDC(1,1,SM,&serial),
00029     ikarashiMDC(1,2,SM,&serial),
00030     ikarashiMDC(1,3,SM,&serial),
00031 };
00032 /*
00033 uint8_t data[128];
00034 
00035 void recieveController(){
00036     mycon.getData(data);
00037     for (int i=7,j=128; j>0; i--,j/=2){
00038         b[i]=data[0]/j;
00039         data[0]%=j;
00040     }
00041     for (int i=0; i<4; i++) stick[i]=128-data[i+1];
00042     if(mycon.getStatus()==false){
00043         for (int i=0; i<8; i++) b[i]=0;
00044         for (int i=0; i<4; i++) stick[i]=0;
00045     }
00046     canTR();
00047     for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
00048     for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
00049     pc.printf(" | ");
00050     if (mycon.getStatus()) pc.printf("received\r\n");
00051     else pc.printf("anything error...\r\n");
00052 }
00053 */
00054 void updateenc(){
00055     encoderValue[0] = (float)enc1.getPulses();
00056     encoderValue[1] = (float)enc2.getPulses();
00057     encoderValue[2] = (float)enc3.getPulses();
00058     encoderValue[3] = (float)enc4.getPulses();
00059 //    for (int i=4; i<8; i++) pc.printf("%d  ", encoderValue[i]);
00060 //    pc.printf("\r\n ");
00061 }
00062 
00063 void updatejyro(){
00064     r1370.update();
00065     jyroValue=r1370.getAngle();
00066 //    pc.printf("%f  ",jyroValue);
00067 }
00068 
00069 int main()
00070 {
00071 //    mycon.StartReceive();
00072     led = 1;
00073     pc.printf("success!\r\n");
00074     
00075     for(int i=0; i < 8; i++){
00076         motor[i].setSpeed(0);
00077     }
00078 
00079     HOSOKIkikou hosoki(&motor[6],&motor[7], &motor[4], &motor[5], &b[4], NULL, NULL, NULL, NULL, NULL);
00080     while(1) {   
00081 //        recieveController();
00082         Reset=0;   
00083         updateenc();
00084         updatejyro();
00085         canTR();
00086         hosoki.runAll(-0.9,0.9,0,0);
00087 
00088         pc.printf("|wheel:  ");
00089         for(int i=8; i < 12; i++){
00090             pc.printf("%.2f  ",motorSpeed[i]);
00091         }
00092         pc.printf("|spin:  %.2f  ",motorSpeed[1]);
00093         pc.printf("|updown:  %.2f  ",motorSpeed[2]);
00094         pc.printf("|enc:  ");
00095         for(int i=0; i < 7; i++){
00096             pc.printf("%d  ",encoderValue[i]);
00097             
00098         }
00099         pc.printf("|  ");
00100         pc.printf("R1370:%.3f",jyroValue);
00101         pc.printf("|  ");
00102         for(int i=0; i < 8; i++){
00103             pc.printf("%d ",b[i]);
00104         }
00105         pc.printf("|  ");
00106         for(int i=0; i < 4; i++){
00107             pc.printf("%d  ",stick[i]);
00108         }
00109         pc.printf("\r\n");
00110         
00111     }
00112 }