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@13:126c3f7f9b89, 2018-08-13 (annotated)

Committer:
shimizuta
Date:
Mon Aug 13 09:03:30 2018 +0000
Revision:
13:126c3f7f9b89
Parent:
7:35972aae0647
Child:
18:05f5a3323bda
a

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 2:4515e8cc6b60 3 #include "go.h"
shimizuta 2:4515e8cc6b60 4 #include "check.h"
shimizuta 13:126c3f7f9b89 5 #include "coordinate.h"
shimizuta 2:4515e8cc6b60 6 #include "pinnames.h"
shimizuta 2:4515e8cc6b60 7 #include "cat18_can_info.h"
shimizuta 13:126c3f7f9b89 8 #include "debug.h"
shimizuta 2:4515e8cc6b60 9 #define RIGHT//右用ならRIGHT,左用ならLEFT
shimizuta 2:4515e8cc6b60 10
shimizuta 2:4515e8cc6b60 11 Ticker ticker;
shimizuta 2:4515e8cc6b60 12 CAN can(pin_can_rd,pin_can_td);
shimizuta 2:4515e8cc6b60 13 int counterpart_is_in_common;
shimizuta 2:4515e8cc6b60 14 char can_pub_data[kCanlength_rightleft];
shimizuta 7:35972aae0647 15
shimizuta 5:af5ccfce1b90 16 const double kTicker_s = 0.01;
shimizuta 2:4515e8cc6b60 17 const int kCanCount = kCanTicker_s / kTicker_s;
shimizuta 7:35972aae0647 18 const int kPIDCount = kPIDTicker_s / kTicker_s;
shimizuta 7:35972aae0647 19 const int kServoCount = kServoTicker_s / kTicker_s;
shimizuta 7:35972aae0647 20 const int kCountReset=kPIDCount*kServoCount*kCanCount;
shimizuta 2:4515e8cc6b60 21
shimizuta 2:4515e8cc6b60 22 void TimerInterrupt();
shimizuta 2:4515e8cc6b60 23 void CanRead();
shimizuta 2:4515e8cc6b60 24 void CanSend();
shimizuta 2:4515e8cc6b60 25
shimizuta 2:4515e8cc6b60 26 void InterruptSetup()
shimizuta 2:4515e8cc6b60 27 {
shimizuta 2:4515e8cc6b60 28 can.frequency(kCanFrequency);
shimizuta 2:4515e8cc6b60 29 ticker.attach(TimerInterrupt,kTicker_s);
shimizuta 2:4515e8cc6b60 30 }
shimizuta 2:4515e8cc6b60 31 void TimerInterrupt()
shimizuta 2:4515e8cc6b60 32 {
shimizuta 7:35972aae0647 33 static int count = 0;
shimizuta 2:4515e8cc6b60 34 PIDInterrupt();
shimizuta 7:35972aae0647 35 if(count % kServoCount==0){
shimizuta 7:35972aae0647 36 ServoTargetInterrupt();
shimizuta 7:35972aae0647 37 }
shimizuta 13:126c3f7f9b89 38 /*
shimizuta 7:35972aae0647 39 if(count % kCanCount==0){
shimizuta 2:4515e8cc6b60 40 CanRead();
shimizuta 2:4515e8cc6b60 41 CanSend();
shimizuta 2:4515e8cc6b60 42 }
shimizuta 13:126c3f7f9b89 43 */
shimizuta 7:35972aae0647 44 if(count == kCountReset) count=0;
shimizuta 7:35972aae0647 45 count++;
shimizuta 2:4515e8cc6b60 46 }
shimizuta 2:4515e8cc6b60 47 int GetCounterpartIsInCommon()
shimizuta 2:4515e8cc6b60 48 {
shimizuta 2:4515e8cc6b60 49 return counterpart_is_in_common;
shimizuta 2:4515e8cc6b60 50 }
shimizuta 2:4515e8cc6b60 51
shimizuta 2:4515e8cc6b60 52 void CanRead()
shimizuta 2:4515e8cc6b60 53 {
shimizuta 2:4515e8cc6b60 54 CANMessage msg;
shimizuta 2:4515e8cc6b60 55 if(!can.read(msg)) {
shimizuta 2:4515e8cc6b60 56 return;
shimizuta 2:4515e8cc6b60 57 }
shimizuta 2:4515e8cc6b60 58 switch (msg.id) {
shimizuta 2:4515e8cc6b60 59 #ifdef RIGHT
shimizuta 2:4515e8cc6b60 60 case CANID_LEFT :
shimizuta 2:4515e8cc6b60 61 #elif defined LEFT
shimizuta 2:4515e8cc6b60 62 case CANID_RIGHT :
shimizuta 2:4515e8cc6b60 63 #endif
shimizuta 2:4515e8cc6b60 64 counterpart_is_in_common = msg.data[0];
shimizuta 2:4515e8cc6b60 65 break;
shimizuta 2:4515e8cc6b60 66
shimizuta 2:4515e8cc6b60 67 }
shimizuta 2:4515e8cc6b60 68 }
shimizuta 2:4515e8cc6b60 69 void CanSend()
shimizuta 2:4515e8cc6b60 70 {
shimizuta 2:4515e8cc6b60 71 static int state = 1;
shimizuta 2:4515e8cc6b60 72 switch(state) {
shimizuta 2:4515e8cc6b60 73 #ifdef RIGHT
shimizuta 2:4515e8cc6b60 74 case CANID_RIGHT :
shimizuta 2:4515e8cc6b60 75 #elif defined LEFT
shimizuta 2:4515e8cc6b60 76 case CANID_LEFT:
shimizuta 2:4515e8cc6b60 77 #endif
shimizuta 2:4515e8cc6b60 78 {
shimizuta 13:126c3f7f9b89 79 double now_position_mm[3] = {GetNowTipLocateX(),GetNowTipLocateY(),GetNowTipLocateZ()};
shimizuta 13:126c3f7f9b89 80 can_pub_data[0] = IsInBanArea(0,now_position_mm);
shimizuta 2:4515e8cc6b60 81 CANMessage msg(state,can_pub_data,kCanlength_rightleft);
shimizuta 2:4515e8cc6b60 82 can.write(msg);
shimizuta 2:4515e8cc6b60 83 break;
shimizuta 2:4515e8cc6b60 84 }
shimizuta 2:4515e8cc6b60 85 }
shimizuta 2:4515e8cc6b60 86 state ++;
shimizuta 2:4515e8cc6b60 87 if(state >= CANID_FINISH) {
shimizuta 2:4515e8cc6b60 88 state = 1;
shimizuta 2:4515e8cc6b60 89 }
shimizuta 2:4515e8cc6b60 90 }