main.cpp

00001 #include "mbed.h"           //wheel_test3
00002 #include "controller.h"
00003 #include "ikarashiMDC.h"
00004 #include "R1370.h"
00005 #include "wheel_an.h"
00006 #define PI 3.141593
00007 
00008 Controller pad(PC_10, PC_11, 208);
00009 R1370 R1370(D10,D2);
00010 Serial pc(USBTX, USBRX, 115200);
00011 Serial serial(PC_6, PC_7, 115200);
00012 DigitalOut serialcontrol(PD_2);
00013 
00014 ikarashiMDC ikarashi[]
00015 {
00016     ikarashiMDC(&serialcontrol,2,0,SM,&serial),
00017     ikarashiMDC(&serialcontrol,2,1,SM,&serial),
00018     ikarashiMDC(&serialcontrol,2,2,SM,&serial)
00019 };
00020 
00021 Wheelan wheel[]
00022 {
00023     Wheelan(PI * 1 / 6),
00024     Wheelan(PI * 5 / 6),
00025     Wheelan(PI * 9 / 6)
00026 };
00027 
00028 int main()
00029 {
00030     ikarashi[0].braking = true;
00031     int b[11];
00032     double rad[2],dis[2];
00033     double value[3];
00034     while(1){
00035         if(pad.receiveState()){
00036             for(int i = 0; i < 13; i++){
00037                 b[i] = pad.getButton1(i);
00038             }
00039             for(int i = 0; i < 2; i++){
00040                 rad[i] = pad.getRadian(i);
00041                 dis[i] = pad.getNorm(i);
00042                 rad[i] = PI - rad[i];
00043             }
00044             R1370.update();
00045             pc.printf("degree : %.3lf    ", R1370.getAngle());
00046 
00047             for (int i = 0; i < 3; ++i)
00048             {
00049                 value[i] = wheel[i].an_uc(rad[1], dis[1]);
00050             }
00051 
00052             if(b[7] == 1 && b[9] == 0) {
00053                 value[0] -= 0.3;
00054                 value[1] -= 0.3;
00055                 value[2] -= 0.3;
00056             } else if (b[9] == 1 && b[7] == 0) {
00057                 value[0] += 0.3;
00058                 value[1] += 0.3;
00059                 value[2] += 0.3;
00060             }
00061 
00062             for (int i = 0; i < 3; ++i)
00063             {
00064                 pc.printf("%lf ", value[i]);
00065                 ikarashi[i].setSpeed(value[i]);
00066             }
00067             pc.printf("\n\r");
00068 
00069         }else{
00070             pc.printf("error\n\r");
00071             for (int i = 0; i < 3; i++)
00072             {
00073                 ikarashi[i].setSpeed(0);
00074             }
00075         }
00076     }
00077 }