a
Dependencies: mbed LidarLitev2
main.cpp
- Committer:
- kazuryu
- Date:
- 2019-09-29
- Revision:
- 0:48dbc1f972f1
File content as of revision 0:48dbc1f972f1:
#include "mbed.h" #include "rori.h" DigitalIn ins(D12); DigitalIn intt(D5); PwmOut outs(D6); PwmOut outt(D3); DigitalOut outs_admin(D9); DigitalOut outt_admin(D4); //RORI roris(PA_0_ALT1,PA_1_ALT1); RORI rorit(PA_4_ALT0,PB_0_ALT0); Serial serial(USBTX,USBRX); /* DigitalIn ins(D13); DigitalIn intt(D11); PwmOut outs(D5); PwmOut outt(D3); DigitalOut outs_admin(D12); DigitalOut outt_admin(D10); RORI roris(PA_0_ALT1,PA_1_ALT1); RORI rorit(PA_4_ALT0,PB_0_ALT0); Serial serial(USBTX,USBRX); */ long roris_l = 0; long rorit_l = 0; int bunkais = 1024; int bunkait = 1024; float angs = 0; float angt = 0; int counts = 1; int countt = 1; bool start_lock = false; long lasts= 0; long lastt = 0; DigitalIn inlock_f(D2); PwmOut lock(D11); /* DigitalIn inlock_f(D15); PwmOut lock(D14); */ DigitalOut leds(D0); DigitalOut ledt(D1); bool flags = false; bool flagt = false; /* DigitalIn discr1(D14); DigitalIn discr2(D15); DigitalOut fieldColor(D13); DigitalOut cloth(D12); */ int main(){ ins.mode(PullDown); intt.mode(PullDown); inlock_f.mode(PullDown); outs.period_ms(1); lock.period_ms(1); while(1){ if(start_lock){ serial.printf("%s","start"); serial.printf("%d",inlock_f.read()); while(!inlock_f.read()){ lock = 0.2; serial.printf("%s\n","while"); } serial.printf("%s\n","out"); lock = 0; start_lock = false; } if(ins.read()){ outs = 0.1f; //roris.read(&roris_l);// if(roris_l == lasts){ flags = !flags; leds = (flags)?1:0; } angs = (float)roris_l/bunkais;//ang angs *= 360;//deg //serial.printf("%s",":"); //serial.printf("%f\n",angs); if(abs(angs) > 90*counts){ counts++; outs = 0; outs_admin = 1; wait(1); outs_admin = 0; } lasts = roris_l; }else{ outs = 0; } if(intt.read()){ outt = 0.1f; rorit.read(&rorit_l); if(rorit_l == lastt){ flagt = !flagt; ledt = (flagt)?:0; } angt = (float)rorit_l/bunkait; angt *= 360; if(abs(angt) > 120*countt){ countt++; outt = 0; outt_admin = 1; wait(0.5f); outt_admin = 0; } lastt = rorit_l; }else{ outt = 0; } } } /* int Discrimination(bool hoge){ bool discColor; bool discMode; discColor=discr1; discMode=discr2; if(hoge){ fieldColor=discr1; cloth=discr2; }else{ return 0; } return 1; } */