albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

main.cpp@41:d2d123d7a6fa, 2017-04-23 (annotated)

Committer:
YusukeWakuta
Date:
Sun Apr 23 15:12:59 2017 +0000
Branch:
?????
Revision:
41:d2d123d7a6fa
Parent:
37:7ce6e87b9c36
Child:
42:6cc53727acc2
????

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 37:7ce6e87b9c36 4 //#include "rtos.h"
tsumagari 10:b7159feb11fd 5 #define WAIT_LOOP_TIME 0.001
tsumagari 9:c32999b4d2f9 6 #define YOKUTAN_DATAS_NUM 7
YusukeWakuta 32:60af346703d3 7 #define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
YusukeWakuta 0:b1290ca6c4a2 8 #define SEND_DATAS_CAN_ID 100
tsumagari 8:dbc8c87dac78 9 #define SEND_DATAS_TIME 0.5
tsumagari 8:dbc8c87dac78 10 #define THRESHOLD_OH_VALUE 0.14
YusukeWakuta 15:c5ade23e5de4 11 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 12
YusukeWakuta 35:adad361dbb53 13 #define PHASE_NUM 15 //奇数にしてください
YusukeWakuta 24:72f9cb6e9440 14 #define SUM_UP_NUM 10.0
YusukeWakuta 29:8bff47ab602d 15 #define PITCHPERROLL 1.5
YusukeWakuta 19:6387e3f02b37 16 /*
YusukeWakuta 19:6387e3f02b37 17 roll入力とピッチ入力だとピッチの方が1.5倍効くように
YusukeWakuta 19:6387e3f02b37 18 */
YusukeWakuta 18:31722545ecf1 19
tsumagari 9:c32999b4d2f9 20 //-----------------------------------(resetInterrupt def)
tsumagari 9:c32999b4d2f9 21 extern "C" void mbed_reset();
YusukeWakuta 13:79eb74eb86c4 22 InterruptIn resetPin(p22);
tsumagari 9:c32999b4d2f9 23 Timer resetTimeCount;
YusukeWakuta 15:c5ade23e5de4 24 void resetInterrupt()
YusukeWakuta 15:c5ade23e5de4 25 {
YusukeWakuta 15:c5ade23e5de4 26 while(resetPin) {
tsumagari 9:c32999b4d2f9 27 resetTimeCount.start();
tsumagari 9:c32999b4d2f9 28 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 9:c32999b4d2f9 29 }
tsumagari 9:c32999b4d2f9 30 resetTimeCount.reset();
tsumagari 9:c32999b4d2f9 31 }
tsumagari 9:c32999b4d2f9 32 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 33
taurin 6:7484315ba88a 34 CAN can_R(p30,p29);
taurin 6:7484315ba88a 35 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 36 Serial toKeikiSerial(p28,p27);
tsumagari 7:59ddbe239835 37 Serial pc(USBTX,USBRX);
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);
tsumagari 37:7ce6e87b9c36 46 //DigitalOut fusokuControl(p25);
tsumagari 9:c32999b4d2f9 47 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 48
taurin 4:4a13dd263a7b 49 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 50 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 51 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 52 char inputDatas_L[INPUT_DATAS_NUM];
YusukeWakuta 24:72f9cb6e9440 53
YusukeWakuta 25:12408563540d 54 float rollNeutral = 0.739;
YusukeWakuta 25:12408563540d 55 float rollUpperDiff = 0;
YusukeWakuta 25:12408563540d 56 float rollLowerDiff = 0;
YusukeWakuta 24:72f9cb6e9440 57
YusukeWakuta 25:12408563540d 58 float pitchNeutral = 0.468 ;//1って書いた方
YusukeWakuta 25:12408563540d 59 float pitchUpperDiff = 0;
YusukeWakuta 25:12408563540d 60 float pitchLowerDiff = 0;
YusukeWakuta 24:72f9cb6e9440 61 float neutralDiff;
YusukeWakuta 0:b1290ca6c4a2 62
YusukeWakuta 3:0e66ce2ab2fb 63 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 64 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 65
tsumagari 37:7ce6e87b9c36 66 //void tickFusoku(void const * arg){
tsumagari 37:7ce6e87b9c36 67 // while(1){
tsumagari 37:7ce6e87b9c36 68 // fusokuControl = 1;
tsumagari 37:7ce6e87b9c36 69 // wait_us(500);
tsumagari 37:7ce6e87b9c36 70 // fusokuControl = 0;
tsumagari 37:7ce6e87b9c36 71 // wait_us(4500);
tsumagari 37:7ce6e87b9c36 72 // }
tsumagari 37:7ce6e87b9c36 73 //}
tsumagari 36:5ad172e48384 74
YusukeWakuta 25:12408563540d 75
YusukeWakuta 24:72f9cb6e9440 76
YusukeWakuta 25:12408563540d 77
YusukeWakuta 18:31722545ecf1 78
YusukeWakuta 21:9b1f5123f4a8 79
YusukeWakuta 29:8bff47ab602d 80
YusukeWakuta 15:c5ade23e5de4 81 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 82 {
YusukeWakuta 26:745735b0479d 83 setMaxAndMin(enumRoll, rollPin.read());
YusukeWakuta 26:745735b0479d 84 setMaxAndMin(enumPitch, pitchPin.read());
YusukeWakuta 26:745735b0479d 85
YusukeWakuta 30:eb7b7af1070a 86 // pc.printf("rollN:%f rollMax:%f rollMin:%f pitchN:%f pitchMax:%f pitchUpper:%f pitchMin:%f pitchLo:%f raw:%f\r\n",rollNeutral,rollNeutral+rollUpperDiff,rollNeutral+rollLowerDiff,pitchNeutral,pitchNeutral+pitchUpperDiff,pitchUpperDiff,pitchNeutral+pitchLowerDiff,pitchLowerDiff,pitchPin.read());
YusukeWakuta 32:60af346703d3 87 float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
YusukeWakuta 26:745735b0479d 88 float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
YusukeWakuta 32:60af346703d3 89 //pc.printf("Rollmax:%f value:%f Min:%f" ,);
YusukeWakuta 32:60af346703d3 90 float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 29:8bff47ab602d 91 *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 24:72f9cb6e9440 92
YusukeWakuta 29:8bff47ab602d 93 float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff));
YusukeWakuta 27:1af92a603532 94 //pc.printf("matched:%f max:%f min:%f\n\r",(MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0);
YusukeWakuta 29:8bff47ab602d 95 *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
YusukeWakuta 27:1af92a603532 96 //pc.printf("Format:%f max:%f min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
YusukeWakuta 28:056287cb001c 97 if(*(int *)inputDatas_R < 1)
YusukeWakuta 28:056287cb001c 98 *(int *)inputDatas_R = 1;
YusukeWakuta 28:056287cb001c 99 else if(*(int *)inputDatas_R > PHASE_NUM)
YusukeWakuta 28:056287cb001c 100 *(int *)inputDatas_R = PHASE_NUM;
YusukeWakuta 28:056287cb001c 101 if(*(int *)inputDatas_L < 1)
YusukeWakuta 28:056287cb001c 102 *(int *)inputDatas_L = 1 ;
YusukeWakuta 28:056287cb001c 103 else if(*(int *)inputDatas_L > PHASE_NUM)
YusukeWakuta 28:056287cb001c 104 *(int *)inputDatas_L =PHASE_NUM;
YusukeWakuta 16:336e8b102555 105
YusukeWakuta 32:60af346703d3 106 pc.printf("DR :%d DL:%d input_R:%d input_L:%d\n\r",drug_R.read(),drug_L.read(),*(int *)inputDatas_R,*(int *)inputDatas_L);
YusukeWakuta 31:3a6be197e98f 107 inputDatas_R[sizeof(int)+ 2] = (char)drug_R;
YusukeWakuta 15:c5ade23e5de4 108 led4 =! led4;
YusukeWakuta 26:745735b0479d 109 // pc.printf("%c",*(char *)inputDatas_R[4]);
YusukeWakuta 15:c5ade23e5de4 110 //pc.printf("%c",(char)drug_R);
YusukeWakuta 31:3a6be197e98f 111 inputDatas_L[sizeof(int)+2] = (char)drug_L;
YusukeWakuta 33:d54dacb7a0aa 112 // pc.printf("eruron:%f drug:%f\n\r,",);
YusukeWakuta 0:b1290ca6c4a2 113 }
YusukeWakuta 0:b1290ca6c4a2 114
YusukeWakuta 15:c5ade23e5de4 115 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 116 {
tsumagari 10:b7159feb11fd 117 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 118 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 119 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 120 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 34:0cff1601bc13 121 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 31:3a6be197e98f 122
YusukeWakuta 34:0cff1601bc13 123 }
YusukeWakuta 34:0cff1601bc13 124 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 125 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 126 }
YusukeWakuta 15:c5ade23e5de4 127 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 128 toKeikiSerial.putc(inputDatas_R[i]);
YusukeWakuta 33:d54dacb7a0aa 129 }
YusukeWakuta 33:d54dacb7a0aa 130 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 131 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 132 }
YusukeWakuta 0:b1290ca6c4a2 133 }
tsumagari 8:dbc8c87dac78 134
YusukeWakuta 15:c5ade23e5de4 135 void init()
YusukeWakuta 15:c5ade23e5de4 136 {
tsumagari 9:c32999b4d2f9 137 //--------------------------------------(resetInterrupt init)
tsumagari 9:c32999b4d2f9 138 resetPin.rise(resetInterrupt);
tsumagari 9:c32999b4d2f9 139 resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 140 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 141 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
YusukeWakuta 15:c5ade23e5de4 142 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 143 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 144 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 145 }
YusukeWakuta 0:b1290ca6c4a2 146 }
YusukeWakuta 0:b1290ca6c4a2 147
YusukeWakuta 15:c5ade23e5de4 148 void receiveDatas()
YusukeWakuta 15:c5ade23e5de4 149 {
YusukeWakuta 15:c5ade23e5de4 150 if(can_R.read(recmsg_R)) {
YusukeWakuta 15:c5ade23e5de4 151 for(int i = 0; i < recmsg_R.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 152 yokutanDatas_R[i] = recmsg_R.data[i];
YusukeWakuta 15:c5ade23e5de4 153 // pc.printf("%c",yokutanDatas_R[i]);
YusukeWakuta 0:b1290ca6c4a2 154 }
YusukeWakuta 25:12408563540d 155 led1 = !led1;
YusukeWakuta 0:b1290ca6c4a2 156 }
YusukeWakuta 15:c5ade23e5de4 157 if(can_L.read(recmsg_L)) {
YusukeWakuta 15:c5ade23e5de4 158 for(int i = 0; i < recmsg_L.len; i++) {
YusukeWakuta 0:b1290ca6c4a2 159 yokutanDatas_L[i] = recmsg_L.data[i];
YusukeWakuta 15:c5ade23e5de4 160 led3 = !led3;
YusukeWakuta 15:c5ade23e5de4 161 // pc.printf("%c",yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 162 }
YusukeWakuta 25:12408563540d 163 led2 = !led2;
YusukeWakuta 0:b1290ca6c4a2 164 }
YusukeWakuta 0:b1290ca6c4a2 165 }
YusukeWakuta 0:b1290ca6c4a2 166
YusukeWakuta 15:c5ade23e5de4 167 int main()
YusukeWakuta 15:c5ade23e5de4 168 {
tsumagari 37:7ce6e87b9c36 169 // Thread tickFusokuThread(&tickFusoku);
YusukeWakuta 0:b1290ca6c4a2 170 init();
YusukeWakuta 24:72f9cb6e9440 171 setNeutral();
YusukeWakuta 15:c5ade23e5de4 172 while(1) {
tsumagari 8:dbc8c87dac78 173 InputControlValues();
tsumagari 10:b7159feb11fd 174 wait_us(5);
YusukeWakuta 0:b1290ca6c4a2 175 receiveDatas();
tsumagari 9:c32999b4d2f9 176 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 177 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 178 }
YusukeWakuta 0:b1290ca6c4a2 179 }