yuto kawamura
/
MR2_2n_1_saku
sls
main.cpp@0:c1476d342c13, 2019-04-26 (annotated)
- Committer:
- eri
- Date:
- Fri Apr 26 11:34:13 2019 +0000
- Revision:
- 0:c1476d342c13
- Child:
- 1:86c4c38abe40
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:c1476d342c13 | 1 | #include "mbed.h" |
eri | 0:c1476d342c13 | 2 | #include "pin.h" |
eri | 0:c1476d342c13 | 3 | #include "microinfinity.h" |
eri | 0:c1476d342c13 | 4 | |
eri | 0:c1476d342c13 | 5 | |
eri | 0:c1476d342c13 | 6 | |
eri | 0:c1476d342c13 | 7 | ////////////関数 |
eri | 0:c1476d342c13 | 8 | void setup(); |
eri | 0:c1476d342c13 | 9 | void can_send(); |
eri | 0:c1476d342c13 | 10 | |
eri | 0:c1476d342c13 | 11 | |
eri | 0:c1476d342c13 | 12 | |
eri | 0:c1476d342c13 | 13 | ////////////定数 |
eri | 0:c1476d342c13 | 14 | |
eri | 0:c1476d342c13 | 15 | |
eri | 0:c1476d342c13 | 16 | |
eri | 0:c1476d342c13 | 17 | |
eri | 0:c1476d342c13 | 18 | |
eri | 0:c1476d342c13 | 19 | ////////////変数 |
eri | 0:c1476d342c13 | 20 | double target_ro=0,target_ri=0; |
eri | 0:c1476d342c13 | 21 | double target_lo=0,target_li=0; |
eri | 0:c1476d342c13 | 22 | bool hand_mode=0; |
eri | 0:c1476d342c13 | 23 | |
eri | 0:c1476d342c13 | 24 | |
eri | 0:c1476d342c13 | 25 | |
eri | 0:c1476d342c13 | 26 | ///////////////////////////////////////////// |
eri | 0:c1476d342c13 | 27 | int main() { |
eri | 0:c1476d342c13 | 28 | |
eri | 0:c1476d342c13 | 29 | setup(); |
eri | 0:c1476d342c13 | 30 | while(1) { |
eri | 0:c1476d342c13 | 31 | can_send(); |
eri | 0:c1476d342c13 | 32 | wait(0.01); |
eri | 0:c1476d342c13 | 33 | } |
eri | 0:c1476d342c13 | 34 | } |
eri | 0:c1476d342c13 | 35 | |
eri | 0:c1476d342c13 | 36 | |
eri | 0:c1476d342c13 | 37 | void setup(){ |
eri | 0:c1476d342c13 | 38 | can1.frequency(1000000); |
eri | 0:c1476d342c13 | 39 | motor_lo_f.period_us(100); |
eri | 0:c1476d342c13 | 40 | motor_lo_b.period_us(100); |
eri | 0:c1476d342c13 | 41 | motor_li_f.period_us(100); |
eri | 0:c1476d342c13 | 42 | motor_li_b.period_us(100); |
eri | 0:c1476d342c13 | 43 | |
eri | 0:c1476d342c13 | 44 | hand.mode(PullUp); |
eri | 0:c1476d342c13 | 45 | switch2.mode(PullUp); |
eri | 0:c1476d342c13 | 46 | switch3.mode(PullUp); |
eri | 0:c1476d342c13 | 47 | switch4.mode(PullUp); |
eri | 0:c1476d342c13 | 48 | |
eri | 0:c1476d342c13 | 49 | |
eri | 0:c1476d342c13 | 50 | device.baud(115200); |
eri | 0:c1476d342c13 | 51 | device.format(8,Serial::None,1); |
eri | 0:c1476d342c13 | 52 | device.attach(dev_rx, Serial::RxIrq); |
eri | 0:c1476d342c13 | 53 | wait(0.05); |
eri | 0:c1476d342c13 | 54 | theta0=degree0; |
eri | 0:c1476d342c13 | 55 | check_gyro(); |
eri | 0:c1476d342c13 | 56 | } |
eri | 0:c1476d342c13 | 57 | |
eri | 0:c1476d342c13 | 58 | |
eri | 0:c1476d342c13 | 59 | //////////////////////////////////////can |
eri | 0:c1476d342c13 | 60 | void can_send() |
eri | 0:c1476d342c13 | 61 | { |
eri | 0:c1476d342c13 | 62 | char data[4]={0}; |
eri | 0:c1476d342c13 | 63 | int target_ro_send=target_ro+360; |
eri | 0:c1476d342c13 | 64 | int target_ri_send=target_ri+360; |
eri | 0:c1476d342c13 | 65 | data[0]=target_ro_send & 0b11111111; |
eri | 0:c1476d342c13 | 66 | data[1]=target_ri_send & 0b11111111; |
eri | 0:c1476d342c13 | 67 | data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000); |
eri | 0:c1476d342c13 | 68 | data[3]=hand_mode; |
eri | 0:c1476d342c13 | 69 | |
eri | 0:c1476d342c13 | 70 | if(can1.write(CANMessage(0,data,4)))led4=1; |
eri | 0:c1476d342c13 | 71 | else led4=0; |
eri | 0:c1476d342c13 | 72 | } |