Diff: main.cpp

Revision:

0:c1476d342c13

Child:

1:86c4c38abe40

--- /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