yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }