自動機の手動プログラム

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "Help.h"
00004 
00005 Serial pc(USBTX,USBRX);
00006 QEI arm_enc(PA_6, PA_8, NC, 624);
00007 
00008 int direct,speed;
00009 char wheel_RF = SBREAK;
00010 char wheel_LF = SBREAK;
00011 char wheel_RB = SBREAK;
00012 char wheel_LB = SBREAK;
00013 char arm_moter = SBREAK;
00014 char dp_moter = SBREAK;
00015 char rsheet_moter = SBREAK;
00016 char lsheet_moter = SBREAK;
00017 
00018 int main(void)
00019 {
00020     speed = fast;
00021     direct = SBREAK;
00022     set_gpio();
00023     write_data(Rf_add,SBREAK);
00024     write_data(Lf_add,SBREAK);
00025     write_data(Rb_add,SBREAK);
00026     write_data(Lb_add,SBREAK);
00027     write_data(Arm_add,SBREAK);
00028     write_data(Dp_add,SBREAK);
00029     write_data(Rst_add,SBREAK);
00030     write_data(Lst_add,SBREAK);
00031     write_data(Fan_add,SBREAK);
00032     while(1) {
00033         set_emg();
00034         set_direct(&direct);
00035         set_speed(&speed);
00036         set_arm(&arm_moter, cir,arm_enc.getPulses());
00037         set_drop(&dp_moter, squ, cro);
00038         set_sheet(&rsheet_moter,'R',up,dow);
00039         set_sheet(&lsheet_moter,'L',up,dow);
00040         if(direct != 0) {
00041             set_tire(&wheel_RF, &wheel_RB, direct, 'R', speed);
00042             set_tire(&wheel_LF, &wheel_LB, direct, 'L', speed);
00043         } else {
00044             set_slide(&wheel_RF,&wheel_LF,&wheel_RB,&wheel_LB,r1,l1, speed);
00045         }
00046         arm_moter = (dp_moter != SBREAK)? dp_moter:arm_moter;
00047 
00048         write_data(Rf_add,wheel_RF);
00049         write_data(Lf_add,wheel_LF);
00050         write_data(Rb_add,wheel_RB);
00051         write_data(Lb_add,wheel_LB);
00052         write_data(Arm_add,arm_moter);
00053         write_data(Dp_add,dp_moter);
00054         write_data(Dp_add,dp_moter);
00055         write_data(Dp_add,dp_moter);
00056 
00057         pri("\033[2J");
00058         pri("\033[1;1H 0x%x:%d",arm_moter,arm_enc.getPulses());
00059         pri("\033[2;1H 0x%x",dp_moter);
00060         pri("\033[3;1H 0x%x",rsheet_moter);
00061         pri("\033[4;1H 0x%x",lsheet_moter);
00062         /*
00063         pri("\033[2J");
00064         pri("\033[1;1H wheel_RF:0x%x",wheel_RF);
00065         pri("\033[2;1H wheel_LF:0x%x",wheel_LF);
00066         pri("\033[3;1H wheel_RB:0x%x",wheel_RB);
00067         pri("\033[4;1H wheel_LB:0x%x",wheel_LB);
00068         pri("\033[5;1H arm_moter:0x%x",arm_moter);
00069         pri("\033[6;1H direct:%d",direct);
00070         pri("\033[7;1H speed:%d",speed);
00071         */
00072 
00073     }
00074 }