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@23:f45eb02433a5, 2018-09-02 (annotated)

Committer:
shimizuta
Date:
Sun Sep 02 07:53:06 2018 +0000
Revision:
23:f45eb02433a5
Parent:
20:13934809e117
Child:
28:bcfc84b481bc
can move with 3 servo

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 23:f45eb02433a5 15 int counterpart_is_in_common = 0;
shimizuta 2:4515e8cc6b60 16 char can_pub_data[kCanlength_rightleft];
shimizuta 20:13934809e117 17 //tickerタイミング計算関係
shimizuta 23:f45eb02433a5 18 const double kTicker_s = 0.005;
shimizuta 2:4515e8cc6b60 19 const int kCanCount = kCanTicker_s / kTicker_s;
shimizuta 7:35972aae0647 20 const int kPIDCount = kPIDTicker_s / kTicker_s;
shimizuta 23:f45eb02433a5 21 const int kServoCount = kServoMoveTicker_s / kTicker_s;
shimizuta 20:13934809e117 22 const int kCalTargetCount = kCalTargetTicker_s / kTicker_s;
shimizuta 23:f45eb02433a5 23 const int kCountReset=kPIDCount*kCalTargetCount*kCanCount * kServoCount;
shimizuta 2:4515e8cc6b60 24
shimizuta 20:13934809e117 25 //tickerで呼び出す
shimizuta 2:4515e8cc6b60 26 void TimerInterrupt();
shimizuta 20:13934809e117 27 //CAN通信
shimizuta 2:4515e8cc6b60 28 void CanRead();
shimizuta 2:4515e8cc6b60 29 void CanSend();
shimizuta 2:4515e8cc6b60 30
shimizuta 2:4515e8cc6b60 31 void InterruptSetup()
shimizuta 2:4515e8cc6b60 32 {
shimizuta 20:13934809e117 33 DEBUG("InterruptSetup() start\r\n");
shimizuta 2:4515e8cc6b60 34 can.frequency(kCanFrequency);
shimizuta 2:4515e8cc6b60 35 ticker.attach(TimerInterrupt,kTicker_s);
shimizuta 20:13934809e117 36 DEBUG("InterruptSetup() finish\r\n");
shimizuta 2:4515e8cc6b60 37 }
shimizuta 2:4515e8cc6b60 38 void TimerInterrupt()
shimizuta 2:4515e8cc6b60 39 {
shimizuta 7:35972aae0647 40 static int count = 0;
shimizuta 19:48c3af917932 41 if(count % kPIDCount == 0) PIDInterrupt();
shimizuta 23:f45eb02433a5 42 if(count % kCalTargetCount == 0) CalTargetInterrupt();
shimizuta 23:f45eb02433a5 43 if(count % kServoCount == 0) ServoMoveInterrupt();
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 }