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@7:35972aae0647, 2018-08-09 (annotated)

Committer:
shimizuta
Date:
Thu Aug 09 07:47:09 2018 +0000
Revision:
7:35972aae0647
Parent:
5:af5ccfce1b90
Child:
13:126c3f7f9b89
add origin

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