yuto kawamura / Mbed 2 deprecated cat18_operate_last

yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

interrupt/interrupt.cpp@20:13934809e117, 2018-08-27 (annotated)

Committer:
shimizuta
Date:
Mon Aug 27 03:47:21 2018 +0000
Revision:
20:13934809e117
Parent:
19:48c3af917932
Child:
23:f45eb02433a5
There are a lot of change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 2:4515e8cc6b60 1 #include "interrupt.h"
shimizuta 2:4515e8cc6b60 2 #include "mbed.h"
shimizuta 20:13934809e117 3 #include "debug.h"
shimizuta 2:4515e8cc6b60 4 #include "go.h"
shimizuta 2:4515e8cc6b60 5 #include "check.h"
shimizuta 13:126c3f7f9b89 6 #include "coordinate.h"
shimizuta 2:4515e8cc6b60 7 #include "pinnames.h"
shimizuta 2:4515e8cc6b60 8 #include "cat18_can_info.h"
shimizuta 20:13934809e117 9 #include "workinfo.h"
shimizuta 20:13934809e117 10 #include "define_right_or_left.h"
shimizuta 20:13934809e117 11 #include "calplace.h"
shimizuta 20:13934809e117 12 //#define CAN_ON //CAN通信するならdefine
shimizuta 2:4515e8cc6b60 13 Ticker ticker;
shimizuta 2:4515e8cc6b60 14 CAN can(pin_can_rd,pin_can_td);
shimizuta 2:4515e8cc6b60 15 int counterpart_is_in_common;
shimizuta 2:4515e8cc6b60 16 char can_pub_data[kCanlength_rightleft];
shimizuta 20:13934809e117 17 //tickerタイミング計算関係
shimizuta 19:48c3af917932 18 const double kTicker_s = 0.001;
shimizuta 2:4515e8cc6b60 19 const int kCanCount = kCanTicker_s / kTicker_s;
shimizuta 7:35972aae0647 20 const int kPIDCount = kPIDTicker_s / kTicker_s;
shimizuta 20:13934809e117 21 const int kCalTargetCount = kCalTargetTicker_s / kTicker_s;
shimizuta 20:13934809e117 22 const int kCountReset=kPIDCount*kCalTargetCount*kCanCount;
shimizuta 2:4515e8cc6b60 23
shimizuta 20:13934809e117 24 //tickerで呼び出す
shimizuta 2:4515e8cc6b60 25 void TimerInterrupt();
shimizuta 20:13934809e117 26 //CAN通信
shimizuta 2:4515e8cc6b60 27 void CanRead();
shimizuta 2:4515e8cc6b60 28 void CanSend();
shimizuta 2:4515e8cc6b60 29
shimizuta 2:4515e8cc6b60 30 void InterruptSetup()
shimizuta 2:4515e8cc6b60 31 {
shimizuta 20:13934809e117 32 DEBUG("InterruptSetup() start\r\n");
shimizuta 2:4515e8cc6b60 33 can.frequency(kCanFrequency);
shimizuta 2:4515e8cc6b60 34 ticker.attach(TimerInterrupt,kTicker_s);
shimizuta 20:13934809e117 35 DEBUG("InterruptSetup() finish\r\n");
shimizuta 2:4515e8cc6b60 36 }
shimizuta 2:4515e8cc6b60 37 void TimerInterrupt()
shimizuta 2:4515e8cc6b60 38 {
shimizuta 7:35972aae0647 39 static int count = 0;
shimizuta 19:48c3af917932 40 if(count % kPIDCount == 0) PIDInterrupt();
shimizuta 20:13934809e117 41 if(count % kCalTargetCount==0) {
shimizuta 20:13934809e117 42 CalTargetInterrupt();
shimizuta 7:35972aae0647 43 }
shimizuta 20:13934809e117 44 #ifdef CAN_ON
shimizuta 20:13934809e117 45 if(count % kCanCount==0) {
shimizuta 2:4515e8cc6b60 46 CanRead();
shimizuta 2:4515e8cc6b60 47 CanSend();
shimizuta 2:4515e8cc6b60 48 }
shimizuta 20:13934809e117 49 #endif
shimizuta 7:35972aae0647 50 if(count == kCountReset) count=0;
shimizuta 7:35972aae0647 51 count++;
shimizuta 2:4515e8cc6b60 52 }
shimizuta 2:4515e8cc6b60 53 int GetCounterpartIsInCommon()
shimizuta 2:4515e8cc6b60 54 {
shimizuta 20:13934809e117 55 //一つもワークを置いていなければ入っていはいけない＝１
shimizuta 20:13934809e117 56 if(shootingbox[kFirstPutPlace].is_exist == 0) return 1;
shimizuta 2:4515e8cc6b60 57 return counterpart_is_in_common;
shimizuta 2:4515e8cc6b60 58 }
shimizuta 2:4515e8cc6b60 59 void CanRead()
shimizuta 2:4515e8cc6b60 60 {
shimizuta 2:4515e8cc6b60 61 CANMessage msg;
shimizuta 2:4515e8cc6b60 62 if(!can.read(msg)) {
shimizuta 2:4515e8cc6b60 63 return;
shimizuta 2:4515e8cc6b60 64 }
shimizuta 2:4515e8cc6b60 65 switch (msg.id) {
shimizuta 2:4515e8cc6b60 66 #ifdef RIGHT
shimizuta 2:4515e8cc6b60 67 case CANID_LEFT :
shimizuta 2:4515e8cc6b60 68 #elif defined LEFT
shimizuta 2:4515e8cc6b60 69 case CANID_RIGHT :
shimizuta 2:4515e8cc6b60 70 #endif
shimizuta 2:4515e8cc6b60 71 counterpart_is_in_common = msg.data[0];
shimizuta 2:4515e8cc6b60 72 break;
shimizuta 2:4515e8cc6b60 73 }
shimizuta 2:4515e8cc6b60 74 }
shimizuta 2:4515e8cc6b60 75 void CanSend()
shimizuta 2:4515e8cc6b60 76 {
shimizuta 2:4515e8cc6b60 77 static int state = 1;
shimizuta 2:4515e8cc6b60 78 switch(state) {
shimizuta 2:4515e8cc6b60 79 #ifdef RIGHT
shimizuta 2:4515e8cc6b60 80 case CANID_RIGHT :
shimizuta 2:4515e8cc6b60 81 #elif defined LEFT
shimizuta 2:4515e8cc6b60 82 case CANID_LEFT:
shimizuta 2:4515e8cc6b60 83 #endif
shimizuta 2:4515e8cc6b60 84 {
shimizuta 13:126c3f7f9b89 85 double now_position_mm[3] = {GetNowTipLocateX(),GetNowTipLocateY(),GetNowTipLocateZ()};
shimizuta 18:05f5a3323bda 86 if(CheckBanArea(0, now_position_mm[0], now_position_mm) == 0) can_pub_data[0] = 1;
shimizuta 18:05f5a3323bda 87 else can_pub_data[0] = 0;
shimizuta 2:4515e8cc6b60 88 CANMessage msg(state,can_pub_data,kCanlength_rightleft);
shimizuta 2:4515e8cc6b60 89 can.write(msg);
shimizuta 2:4515e8cc6b60 90 break;
shimizuta 2:4515e8cc6b60 91 }
shimizuta 2:4515e8cc6b60 92 }
shimizuta 2:4515e8cc6b60 93 state ++;
shimizuta 2:4515e8cc6b60 94 if(state >= CANID_FINISH) {
shimizuta 2:4515e8cc6b60 95 state = 1;
shimizuta 2:4515e8cc6b60 96 }
shimizuta 2:4515e8cc6b60 97 }