yuto kawamura
/
MR2_2n_1_saku
sls
Diff: main.cpp
- Revision:
- 0:c1476d342c13
- Child:
- 1:86c4c38abe40
diff -r 000000000000 -r c1476d342c13 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Apr 26 11:34:13 2019 +0000 @@ -0,0 +1,72 @@ +#include "mbed.h" +#include "pin.h" +#include "microinfinity.h" + + + +////////////関数 +void setup(); +void can_send(); + + + +////////////定数 + + + + + +////////////変数 +double target_ro=0,target_ri=0; +double target_lo=0,target_li=0; +bool hand_mode=0; + + + +///////////////////////////////////////////// +int main() { + + setup(); + while(1) { + can_send(); + wait(0.01); + } +} + + +void setup(){ + can1.frequency(1000000); + motor_lo_f.period_us(100); + motor_lo_b.period_us(100); + motor_li_f.period_us(100); + motor_li_b.period_us(100); + + hand.mode(PullUp); + switch2.mode(PullUp); + switch3.mode(PullUp); + switch4.mode(PullUp); + + + device.baud(115200); + device.format(8,Serial::None,1); + device.attach(dev_rx, Serial::RxIrq); + wait(0.05); + theta0=degree0; + check_gyro(); +} + + +//////////////////////////////////////can +void can_send() +{ + char data[4]={0}; + int target_ro_send=target_ro+360; + int target_ri_send=target_ri+360; + data[0]=target_ro_send & 0b11111111; + data[1]=target_ri_send & 0b11111111; + data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000); + data[3]=hand_mode; + + if(can1.write(CANMessage(0,data,4)))led4=1; + else led4=0; +} \ No newline at end of file