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@28:bcfc84b481bc, 2018-09-04 (annotated)

Committer:
shimizuta
Date:
Tue Sep 04 12:00:39 2018 +0000
Revision:
28:bcfc84b481bc
Parent:
23:f45eb02433a5
Child:
29:ea48140fc85e
add manual

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