main.cpp

00001 #include "mbed.h"
00002 #include "rori.h"
00003 
00004 DigitalIn ins(D12);
00005 DigitalIn intt(D5);
00006 PwmOut outs(D6);
00007 PwmOut outt(D3);
00008 DigitalOut outs_admin(D9);
00009 DigitalOut outt_admin(D4);
00010 //RORI roris(PA_0_ALT1,PA_1_ALT1);
00011 RORI rorit(PA_4_ALT0,PB_0_ALT0);
00012 Serial serial(USBTX,USBRX);
00013 
00014 /*
00015 DigitalIn ins(D13);
00016 DigitalIn intt(D11);
00017 PwmOut outs(D5);
00018 PwmOut outt(D3);
00019 DigitalOut outs_admin(D12);
00020 DigitalOut outt_admin(D10);
00021 RORI roris(PA_0_ALT1,PA_1_ALT1);
00022 RORI rorit(PA_4_ALT0,PB_0_ALT0);
00023 Serial serial(USBTX,USBRX);
00024 */
00025 long roris_l = 0;
00026 long rorit_l = 0;
00027 int bunkais = 1024;
00028 int bunkait = 1024;
00029 float angs = 0;
00030 float angt = 0;
00031 int counts = 1;
00032 int countt = 1;
00033 bool start_lock = false;
00034 long lasts= 0;
00035 long lastt = 0;
00036 
00037 DigitalIn inlock_f(D2);
00038 PwmOut lock(D11);
00039 /*
00040 DigitalIn inlock_f(D15);
00041 PwmOut lock(D14);
00042 */
00043 DigitalOut leds(D0);
00044 DigitalOut ledt(D1);
00045 bool flags = false;
00046 bool flagt = false;
00047 /*
00048 DigitalIn discr1(D14);
00049 DigitalIn discr2(D15);
00050 DigitalOut fieldColor(D13);
00051 DigitalOut cloth(D12);
00052 */
00053 
00054 
00055 int main(){
00056     ins.mode(PullDown);
00057     intt.mode(PullDown);
00058     inlock_f.mode(PullDown);
00059 
00060     outs.period_ms(1);
00061     lock.period_ms(1);
00062     while(1){
00063         if(start_lock){
00064             serial.printf("%s","start");
00065             serial.printf("%d",inlock_f.read());
00066             while(!inlock_f.read()){
00067                 lock = 0.2;
00068                 serial.printf("%s\n","while");
00069             }
00070             serial.printf("%s\n","out");
00071             lock = 0;
00072             start_lock = false;
00073             }
00074         if(ins.read()){
00075             outs = 0.1f;
00076             //roris.read(&roris_l);//
00077             if(roris_l == lasts){
00078                 flags = !flags;
00079                 leds = (flags)?1:0;
00080             }
00081             angs = (float)roris_l/bunkais;//ang
00082             angs *= 360;//deg
00083             //serial.printf("%s",":");
00084             //serial.printf("%f\n",angs);
00085             if(abs(angs) > 90*counts){
00086                 counts++;
00087                 outs = 0;
00088                 outs_admin = 1;
00089                 wait(1);
00090                 outs_admin = 0;
00091                 }
00092             lasts = roris_l;
00093             
00094         }else{
00095             outs = 0;
00096             }
00097         
00098         if(intt.read()){
00099             outt = 0.1f;
00100             rorit.read(&rorit_l);
00101             if(rorit_l == lastt){
00102                 flagt = !flagt;
00103                 ledt = (flagt)?:0;
00104             }
00105             angt = (float)rorit_l/bunkait;
00106             angt *= 360;
00107             if(abs(angt) > 120*countt){
00108                 countt++;
00109                 outt = 0;
00110                 outt_admin = 1;
00111                 wait(0.5f);
00112                 outt_admin = 0;
00113                 }
00114             lastt = rorit_l;
00115         }else{
00116             outt = 0;
00117             }
00118             
00119         }
00120     }
00121 /*
00122     int Discrimination(bool hoge){
00123         bool discColor;
00124         bool discMode;
00125         discColor=discr1;
00126         discMode=discr2;
00127         if(hoge){
00128             fieldColor=discr1;
00129             cloth=discr2;
00130             }else{
00131                 return 0;
00132                 }
00133                 return 1;
00134         }
00135 */