albatross / Mbed 2 deprecated ControlMain2017

2017年度の製作を開始します

Dependencies:   mbed mbed-rtos

Fork of Control_Main_Full_20160608 by albatross

main.cpp@50:b582ed334d3a, 2017-06-04 (annotated)

Committer:
YusukeWakuta
Date:
Sun Jun 04 16:25:43 2017 +0000
Branch:
?????
Revision:
50:b582ed334d3a
Parent:
49:87af91607b46
Child:
51:8a579a19a4ff
?????float???

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"
YusukeWakuta 49:87af91607b46 4 #include "InputHandler.h"
tsumagari 37:7ce6e87b9c36 5 //#include "rtos.h"
tsumagari 10:b7159feb11fd 6 #define WAIT_LOOP_TIME 0.001
tsumagari 9:c32999b4d2f9 7 #define YOKUTAN_DATAS_NUM 7
YusukeWakuta 32:60af346703d3 8 #define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
YusukeWakuta 0:b1290ca6c4a2 9 #define SEND_DATAS_CAN_ID 100
tsumagari 8:dbc8c87dac78 10 #define SEND_DATAS_TIME 0.5
tsumagari 8:dbc8c87dac78 11 #define THRESHOLD_OH_VALUE 0.14
YusukeWakuta 15:c5ade23e5de4 12 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 13
YusukeWakuta 49:87af91607b46 14 #define PITCH_PER_ROLL 1.5//pitchがrollの何倍影響するかを示す値
YusukeWakuta 49:87af91607b46 15 #define AVE_NUM 1000
YusukeWakuta 49:87af91607b46 16
YusukeWakuta 19:6387e3f02b37 17 /*
YusukeWakuta 19:6387e3f02b37 18 roll入力とピッチ入力だとピッチの方が1.5倍効くように
YusukeWakuta 19:6387e3f02b37 19 */
YusukeWakuta 18:31722545ecf1 20
tsumagari 9:c32999b4d2f9 21 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 22 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 23 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 24 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 25 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 26 {
YusukeWakuta 15:c5ade23e5de4 27 while(resetPin) {
tsumagari 9:c32999b4d2f9 28 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 29 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 30 }
tsumagari 9:c32999b4d2f9 31 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 32 }
tsumagari 9:c32999b4d2f9 33 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 34
taurin 6:7484315ba88a 35 CAN can_R(p30,p29);
taurin 6:7484315ba88a 36 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 37 Serial toKeikiSerial(p28,p27);
tsumagari 10:b7159feb11fd 38 AnalogIn rollPin(p15);
YusukeWakuta 31:3a6be197e98f 39 AnalogIn pitchPin(p18);
YusukeWakuta 13:79eb74eb86c4 40 DigitalIn drug_R(p14);
YusukeWakuta 31:3a6be197e98f 41 DigitalIn drug_L(p17);
YusukeWakuta 25:12408563540d 42 DigitalOut led1(LED1);
YusukeWakuta 25:12408563540d 43 DigitalOut led2(LED2);
YusukeWakuta 15:c5ade23e5de4 44 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 45 DigitalOut led3(LED3);
YusukeWakuta 43:2d8bffac06cd 46 Serial pc(USBTX,USBRX);
YusukeWakuta 49:87af91607b46 47 InputCalc pitchIC;
YusukeWakuta 49:87af91607b46 48 InputCalc rollIC;
tsumagari 37:7ce6e87b9c36 49 //DigitalOut fusokuControl(p25);
tsumagari 9:c32999b4d2f9 50 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 51
taurin 4:4a13dd263a7b 52 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 53 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 54 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 55 char inputDatas_L[INPUT_DATAS_NUM];
YusukeWakuta 24:72f9cb6e9440 56
YusukeWakuta 3:0e66ce2ab2fb 57 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 58 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 59
tsumagari 37:7ce6e87b9c36 60 //void tickFusoku(void const * arg){
tsumagari 37:7ce6e87b9c36 61 // while(1){
tsumagari 37:7ce6e87b9c36 62 // fusokuControl = 1;
tsumagari 37:7ce6e87b9c36 63 // wait_us(500);
tsumagari 37:7ce6e87b9c36 64 // fusokuControl = 0;
tsumagari 37:7ce6e87b9c36 65 // wait_us(4500);
tsumagari 37:7ce6e87b9c36 66 // }
tsumagari 37:7ce6e87b9c36 67 //}
YusukeWakuta 49:87af91607b46 68 void SyntheRollAndPitch(double pitch,double roll,float* inputR,float* inputL)
YusukeWakuta 49:87af91607b46 69 {
YusukeWakuta 49:87af91607b46 70 *inputR = (pitch * PITCH_PER_ROLL + roll)/(PITCH_PER_ROLL + 1.0);
YusukeWakuta 49:87af91607b46 71 *inputL = (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
YusukeWakuta 49:87af91607b46 72 }
YusukeWakuta 49:87af91607b46 73 double GetAve(double input)
YusukeWakuta 49:87af91607b46 74 {
YusukeWakuta 49:87af91607b46 75 float sum = 0.0;
YusukeWakuta 49:87af91607b46 76 for (int i = 0; i < AVE_NUM; i++) {
YusukeWakuta 49:87af91607b46 77 sum += input;
YusukeWakuta 49:87af91607b46 78 }
YusukeWakuta 49:87af91607b46 79 return sum / AVE_NUM;
YusukeWakuta 49:87af91607b46 80 }
tsumagari 36:5ad172e48384 81
YusukeWakuta 15:c5ade23e5de4 82 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 83 {
YusukeWakuta 49:87af91607b46 84 double pitch =GetAve( pitchPin.read());
YusukeWakuta 49:87af91607b46 85 double roll = GetAve(rollPin.read());
YusukeWakuta 49:87af91607b46 86 double normedPitch = pitchIC.Processing(pitch);
YusukeWakuta 49:87af91607b46 87 double normedRoll = rollIC.Processing(rollPin.read());
YusukeWakuta 49:87af91607b46 88 // pc.printf("pitch:%4.3f roll:%4.3f\n\r",normedPitch,normedRoll);
YusukeWakuta 49:87af91607b46 89 SyntheRollAndPitch(normedPitch,normedRoll,(float*)inputDatas_R,(float *)inputDatas_L);
YusukeWakuta 49:87af91607b46 90 pc.printf("R:%8.8f L:%8.8f\n\r",*(float*)inputDatas_R,*(float *)inputDatas_L);
YusukeWakuta 49:87af91607b46 91 // pc.printf("R:%d L:%d show Both Right %d %d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L,drug_R.read(), drug_L.read());
YusukeWakuta 50:b582ed334d3a 92 inputDatas_R[sizeof(float)+ 2] = (char)drug_R;
YusukeWakuta 50:b582ed334d3a 93 inputDatas_L[sizeof(float)+2] = (char)drug_L;
YusukeWakuta 49:87af91607b46 94 led4 =! led4;
YusukeWakuta 0:b1290ca6c4a2 95 }
YusukeWakuta 0:b1290ca6c4a2 96
YusukeWakuta 15:c5ade23e5de4 97 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 98 {
tsumagari 10:b7159feb11fd 99 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 100 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 101 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 102 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 34:0cff1601bc13 103 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 31:3a6be197e98f 104
YusukeWakuta 34:0cff1601bc13 105 }
YusukeWakuta 34:0cff1601bc13 106 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 107 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 108 }
YusukeWakuta 15:c5ade23e5de4 109 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 110 toKeikiSerial.putc(inputDatas_R[i]);
YusukeWakuta 33:d54dacb7a0aa 111 }
YusukeWakuta 33:d54dacb7a0aa 112 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 113 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 114 }
YusukeWakuta 0:b1290ca6c4a2 115 }
tsumagari 8:dbc8c87dac78 116
YusukeWakuta 15:c5ade23e5de4 117 void init()
YusukeWakuta 15:c5ade23e5de4 118 {
tsumagari 9:c32999b4d2f9 119 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 120 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 121 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 122 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 123 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 124 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 125 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 126 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 127 }
YusukeWakuta 0:b1290ca6c4a2 128 }
YusukeWakuta 0:b1290ca6c4a2 129
YusukeWakuta 15:c5ade23e5de4 130 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 131 {
YusukeWakuta 15:c5ade23e5de4 132 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 133 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 134 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 0:b1290ca6c4a2 135 }
YusukeWakuta 25:12408563540d 136 led1 = !led1;
YusukeWakuta 0:b1290ca6c4a2 137 }
YusukeWakuta 15:c5ade23e5de4 138 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 139 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 140 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 141 led3 = !led3;
YusukeWakuta 0:b1290ca6c4a2 142 }
YusukeWakuta 25:12408563540d 143 led2 = !led2;
YusukeWakuta 0:b1290ca6c4a2 144 }
YusukeWakuta 0:b1290ca6c4a2 145 }
YusukeWakuta 0:b1290ca6c4a2 146
YusukeWakuta 15:c5ade23e5de4 147 int main()
YusukeWakuta 15:c5ade23e5de4 148 {
tsumagari 37:7ce6e87b9c36 149 // Thread tickFusokuThread(&tickFusoku);
YusukeWakuta 0:b1290ca6c4a2 150 init();
YusukeWakuta 49:87af91607b46 151 pitchIC.setNeutral(pitchPin.read());
YusukeWakuta 49:87af91607b46 152 rollIC.setNeutral(rollPin.read());
YusukeWakuta 15:c5ade23e5de4 153 while(1) {
YusukeWakuta 49:87af91607b46 154
tsumagari 8:dbc8c87dac78 155 InputControlValues();
tsumagari 10:b7159feb11fd 156 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 157 receiveDatas();
tsumagari 9:c32999b4d2f9 158 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 159 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 160 }
YusukeWakuta 0:b1290ca6c4a2 161 }