albatross / Mbed 2 deprecated ControlMain2017_2

今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

main.cpp@17:e6d7bdf2c5f6, 2017-03-18 (annotated)

Committer:
tsumagari
Date:
Sat Mar 18 14:10:36 2017 +0000
Revision:
17:e6d7bdf2c5f6
Parent:
16:336e8b102555 
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 13:79eb74eb86c4 1 //中央
YusukeWakuta 0:b1290ca6c4a2 2
YusukeWakuta 0:b1290ca6c4a2 3 #include "mbed.h"
tsumagari 10:b7159feb11fd 4 #define WAIT_LOOP_TIME 0.001
tsumagari 9:c32999b4d2f9 5 #define YOKUTAN_DATAS_NUM 7
tsumagari 10:b7159feb11fd 6 #define INPUT_DATAS_NUM 5
YusukeWakuta 0:b1290ca6c4a2 7 #define SEND_DATAS_CAN_ID 100
tsumagari 8:dbc8c87dac78 8 #define SEND_DATAS_TIME 0.5
tsumagari 8:dbc8c87dac78 9 #define THRESHOLD_OH_VALUE 0.14
YusukeWakuta 16:336e8b102555 10 #define SUM_UP_NUM 20
YusukeWakuta 15:c5ade23e5de4 11 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 12
tsumagari 9:c32999b4d2f9 13 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 14 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 15 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 16 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 17 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 18 {
YusukeWakuta 15:c5ade23e5de4 19 while(resetPin) {
tsumagari 9:c32999b4d2f9 20 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 21 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 22 }
tsumagari 9:c32999b4d2f9 23 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 24 }
tsumagari 9:c32999b4d2f9 25 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 26
taurin 6:7484315ba88a 27 CAN can_R(p30,p29);
taurin 6:7484315ba88a 28 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 29 Serial toKeikiSerial(p28,p27);
tsumagari 7:59ddbe239835 30 Serial pc(USBTX,USBRX);
tsumagari 10:b7159feb11fd 31 AnalogIn rollPin(p15);
YusukeWakuta 13:79eb74eb86c4 32 AnalogIn pichPin(p20);
YusukeWakuta 13:79eb74eb86c4 33 DigitalIn drug_R(p14);
taurin 6:7484315ba88a 34 DigitalIn drug_L(p19);
YusukeWakuta 0:b1290ca6c4a2 35 DigitalOut myled1(LED1);
YusukeWakuta 0:b1290ca6c4a2 36 DigitalOut myled2(LED2);
YusukeWakuta 15:c5ade23e5de4 37 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 38 DigitalOut led3(LED3);
tsumagari 9:c32999b4d2f9 39 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 40
taurin 4:4a13dd263a7b 41 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 42 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 43 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 44 char inputDatas_L[INPUT_DATAS_NUM];
taurin 4:4a13dd263a7b 45 bool stateP = true;
taurin 4:4a13dd263a7b 46 bool stateQ = true;
taurin 4:4a13dd263a7b 47 bool stateP_old = true;
taurin 4:4a13dd263a7b 48 bool stateQ_old = true;
YusukeWakuta 0:b1290ca6c4a2 49
YusukeWakuta 3:0e66ce2ab2fb 50 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 51 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 52
YusukeWakuta 15:c5ade23e5de4 53 float get_analogin_ave(AnalogIn pin)
YusukeWakuta 15:c5ade23e5de4 54 {
tsumagari 10:b7159feb11fd 55 float val = 0;
YusukeWakuta 15:c5ade23e5de4 56 for(int i = 0; i<SUM_UP_NUM; i++) {
tsumagari 10:b7159feb11fd 57 val += pin.read();
YusukeWakuta 15:c5ade23e5de4 58 }
YusukeWakuta 15:c5ade23e5de4 59 return val/SUM_UP_NUM;
tsumagari 10:b7159feb11fd 60 }
tsumagari 10:b7159feb11fd 61
YusukeWakuta 15:c5ade23e5de4 62 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 63 {
YusukeWakuta 16:336e8b102555 64 *(float *)inputDatas_R = get_analogin_ave(pichPin) * (2.0/3.0) + get_analogin_ave(rollPin) * (1.0 / 3.0);
YusukeWakuta 16:336e8b102555 65 *(float *)inputDatas_L = get_analogin_ave(pichPin) * (2.0/3.0) - get_analogin_ave(rollPin) * (1.0 / 3.0);
YusukeWakuta 16:336e8b102555 66
YusukeWakuta 16:336e8b102555 67 if(*(float *)inputDatas_R < 0)
YusukeWakuta 16:336e8b102555 68 *(float *)inputDatas_R = 0;
YusukeWakuta 16:336e8b102555 69 else if(*(float *)inputDatas_R > 1)
YusukeWakuta 16:336e8b102555 70 *(float *)inputDatas_R = 1;
YusukeWakuta 16:336e8b102555 71 if(*(float *)inputDatas_L < 0)
YusukeWakuta 16:336e8b102555 72 *(float *)inputDatas_L = 0 ;
YusukeWakuta 16:336e8b102555 73 else if(*(float *)inputDatas_L > 1)
YusukeWakuta 16:336e8b102555 74 *(float *)inputDatas_L =1;
YusukeWakuta 16:336e8b102555 75
YusukeWakuta 16:336e8b102555 76 pc.printf("input_R:%f input_L:%f\n\r",*(float *)inputDatas_R,*(float *)inputDatas_L);
tsumagari 10:b7159feb11fd 77 inputDatas_R[4] = (char)drug_R;
YusukeWakuta 15:c5ade23e5de4 78 led4 =! led4;
YusukeWakuta 15:c5ade23e5de4 79 pc.printf("%c",*(char *)inputDatas_R[4]);
YusukeWakuta 15:c5ade23e5de4 80 //pc.printf("%c",(char)drug_R);
tsumagari 10:b7159feb11fd 81 inputDatas_L[4] = (char)drug_L;
YusukeWakuta 15:c5ade23e5de4 82 // pc.printf("");
tsumagari 17:e6d7bdf2c5f6 83 pc.printf("DR_R:%d DR_L:%d\n\r",inputDatas_R[4],inputDatas_L[4]);
YusukeWakuta 0:b1290ca6c4a2 84 }
YusukeWakuta 0:b1290ca6c4a2 85
YusukeWakuta 15:c5ade23e5de4 86 void toString_R()
YusukeWakuta 15:c5ade23e5de4 87 {
taurin 4:4a13dd263a7b 88 pc.printf("R:");
YusukeWakuta 15:c5ade23e5de4 89 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 90 pc.printf("%d:%i ",i,inputDatas_R[i]);
taurin 4:4a13dd263a7b 91 }
YusukeWakuta 0:b1290ca6c4a2 92 }
YusukeWakuta 0:b1290ca6c4a2 93
YusukeWakuta 15:c5ade23e5de4 94 void toString_L()
YusukeWakuta 15:c5ade23e5de4 95 {
taurin 4:4a13dd263a7b 96 pc.printf("L:");
YusukeWakuta 15:c5ade23e5de4 97 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 98 pc.printf("%d:%i ",i,inputDatas_L[i]);
taurin 4:4a13dd263a7b 99 }
taurin 4:4a13dd263a7b 100 pc.printf("\n\r");
YusukeWakuta 0:b1290ca6c4a2 101 }
YusukeWakuta 0:b1290ca6c4a2 102
YusukeWakuta 15:c5ade23e5de4 103 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 104 {
tsumagari 10:b7159feb11fd 105 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 106 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 107 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 108 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 109 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 110 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 111 }
YusukeWakuta 15:c5ade23e5de4 112 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 113 toKeikiSerial.putc(inputDatas_R[i]);
taurin 4:4a13dd263a7b 114 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 115 }
tsumagari 8:dbc8c87dac78 116 // toKeikiSerial.putc(cadence);
YusukeWakuta 15:c5ade23e5de4 117 // pc.printf("test\n\r");
YusukeWakuta 0:b1290ca6c4a2 118 }
tsumagari 8:dbc8c87dac78 119
YusukeWakuta 15:c5ade23e5de4 120 void init()
YusukeWakuta 15:c5ade23e5de4 121 {
tsumagari 9:c32999b4d2f9 122 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 123 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 124 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 125 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 126 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 127 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 128 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 129 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 130 }
YusukeWakuta 0:b1290ca6c4a2 131 }
YusukeWakuta 0:b1290ca6c4a2 132
YusukeWakuta 15:c5ade23e5de4 133 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 134 {
YusukeWakuta 15:c5ade23e5de4 135 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 136 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 137 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 15:c5ade23e5de4 138 // pc.printf("%c",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 139 }
taurin 4:4a13dd263a7b 140 myled1 = !myled1;
YusukeWakuta 0:b1290ca6c4a2 141 }
YusukeWakuta 15:c5ade23e5de4 142 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 143 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 144 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 145 led3 = !led3;
YusukeWakuta 15:c5ade23e5de4 146 // pc.printf("%c",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 147 }
taurin 4:4a13dd263a7b 148 myled2 = !myled2;
YusukeWakuta 0:b1290ca6c4a2 149 }
YusukeWakuta 0:b1290ca6c4a2 150 }
YusukeWakuta 0:b1290ca6c4a2 151
YusukeWakuta 15:c5ade23e5de4 152 int main()
YusukeWakuta 15:c5ade23e5de4 153 {
YusukeWakuta 0:b1290ca6c4a2 154 init();
YusukeWakuta 15:c5ade23e5de4 155 while(1) {
tsumagari 8:dbc8c87dac78 156 InputControlValues();
tsumagari 10:b7159feb11fd 157 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 158 receiveDatas();
tsumagari 9:c32999b4d2f9 159 SendDatas();
YusukeWakuta 15:c5ade23e5de4 160 // pc.printf("%d",drug_R.read());
tsumagari 10:b7159feb11fd 161 // toString_R();
taurin 4:4a13dd263a7b 162 // toString_L();
YusukeWakuta 0:b1290ca6c4a2 163 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 164 }
YusukeWakuta 0:b1290ca6c4a2 165 }