albatross / Mbed 2 deprecated ControlMain2017

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

Dependencies:   mbed mbed-rtos

Fork of Control_Main_Full_20160608 by albatross

main.cpp@59:04c26e2c20bb, 2018-01-19 (annotated)

Committer:
tsumagari
Date:
Fri Jan 19 11:07:39 2018 +0000
Branch:
?????
Revision:
59:04c26e2c20bb
Parent:
58:0b4b842149de 
rsp???input?????normedRoll drR,normedPitch drL?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 13:79eb74eb86c4 1 //中央
tsumagari 58:0b4b842149de 2 /*
tsumagari 58:0b4b842149de 3 ******************************************
tsumagari 58:0b4b842149de 4 **magic character of debug**
tsumagari 58:0b4b842149de 5 *
tsumagari 59:04c26e2c20bb 6 *(0)]:pilot's input datas: inputR,inputL,normedPitch,normedRoll
tsumagari 58:0b4b842149de 7 ,rollIC.shiftedMax,rollIC.shiftedMin,(int)drug_R,(int)drug_L
tsumagari 58:0b4b842149de 8 *(1)g:getting datas: mpu(LR),servoV(LR) <- mbed always send this to pc(rsp)
tsumagari 59:04c26e2c20bb 9 *(2)_:data to debug what you want(joker)
tsumagari 58:0b4b842149de 10 ******************************************
tsumagari 58:0b4b842149de 11 *
tsumagari 58:0b4b842149de 12 */
YusukeWakuta 0:b1290ca6c4a2 13
YusukeWakuta 0:b1290ca6c4a2 14 #include "mbed.h"
YusukeWakuta 49:87af91607b46 15 #include "InputHandler.h"
tsumagari 37:7ce6e87b9c36 16 //#include "rtos.h"
tsumagari 58:0b4b842149de 17
tsumagari 10:b7159feb11fd 18 #define WAIT_LOOP_TIME 0.001
tsumagari 9:c32999b4d2f9 19 #define YOKUTAN_DATAS_NUM 7
tsumagari 58:0b4b842149de 20 #define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
tsumagari 58:0b4b842149de 21 #define SEND_DATAS_CAN_ID 0x700//強い0x0>>0x7ff弱い
tsumagari 8:dbc8c87dac78 22 #define SEND_DATAS_TIME 0.5
tsumagari 8:dbc8c87dac78 23 #define THRESHOLD_OH_VALUE 0.14
YusukeWakuta 15:c5ade23e5de4 24 #define SAMPLE_INTERVAL 0.05
YusukeWakuta 0:b1290ca6c4a2 25
tsumagari 55:1db68795bccf 26 #define PITCH_PER_ROLL 1.0//pitchがrollの何倍影響するかを示す値
YusukeWakuta 52:a9d23be503c6 27 #define AVE_NUM 100
YusukeWakuta 49:87af91607b46 28
tsumagari 58:0b4b842149de 29
YusukeWakuta 19:6387e3f02b37 30 /*
YusukeWakuta 19:6387e3f02b37 31 roll入力とピッチ入力だとピッチの方が1.5倍効くように
YusukeWakuta 19:6387e3f02b37 32 */
tsumagari 58:0b4b842149de 33 #define SIGN(x) (((x)>0)-((x)<0))
YusukeWakuta 18:31722545ecf1 34
tsumagari 58:0b4b842149de 35 #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0)
tsumagari 58:0b4b842149de 36 #define INPUT_DATA_DEBUG_FLAG debugflag[0].flag
tsumagari 58:0b4b842149de 37 #define TO_KEIKI_DATA_DEBUG_FLAG debugflag[1].flag
tsumagari 58:0b4b842149de 38 #define DEBUG_FLAG debugflag[2].flag
tsumagari 58:0b4b842149de 39
tsumagari 58:0b4b842149de 40 struct flaglist{
tsumagari 58:0b4b842149de 41 char key;
tsumagari 58:0b4b842149de 42 bool flag;
tsumagari 58:0b4b842149de 43 };
tsumagari 58:0b4b842149de 44 struct flaglist debugflag[]={
tsumagari 59:04c26e2c20bb 45 {']',0},
tsumagari 58:0b4b842149de 46 {'g',0},
tsumagari 59:04c26e2c20bb 47 {'_',0},
tsumagari 58:0b4b842149de 48 {'0',0}//footer
tsumagari 58:0b4b842149de 49 };
tsumagari 9:c32999b4d2f9 50 //-----------------------------------(resetInterrupt def)
tsumagari 55:1db68795bccf 51 //extern "C" void mbed_reset();
tsumagari 55:1db68795bccf 52 //InterruptIn resetPin(p22);
tsumagari 55:1db68795bccf 53 //Timer resetTimeCount;
tsumagari 55:1db68795bccf 54 //void resetInterrupt()
tsumagari 55:1db68795bccf 55 //{
tsumagari 55:1db68795bccf 56 // while(resetPin) {
tsumagari 55:1db68795bccf 57 // resetTimeCount.start();
tsumagari 55:1db68795bccf 58 // if(resetTimeCount.read()>3) mbed_reset();
tsumagari 55:1db68795bccf 59 // }
tsumagari 55:1db68795bccf 60 // resetTimeCount.reset();
tsumagari 55:1db68795bccf 61 //}
tsumagari 9:c32999b4d2f9 62 //-------------------------------------------------------
tsumagari 9:c32999b4d2f9 63
taurin 6:7484315ba88a 64 CAN can_R(p30,p29);
taurin 6:7484315ba88a 65 CAN can_L(p9,p10);
YusukeWakuta 14:ffe9460922cb 66 Serial toKeikiSerial(p28,p27);
tsumagari 59:04c26e2c20bb 67 AnalogIn rollPin(p15);// right hand
tsumagari 59:04c26e2c20bb 68 DigitalIn drug_R(p14);// right hand
tsumagari 59:04c26e2c20bb 69 AnalogIn pitchPin(p18);// left hand
tsumagari 59:04c26e2c20bb 70 DigitalIn drug_L(p17);// left hand
tsumagari 57:f6226219e93d 71 DigitalIn servoOff(p26);
YusukeWakuta 25:12408563540d 72 DigitalOut led1(LED1);
YusukeWakuta 25:12408563540d 73 DigitalOut led2(LED2);
YusukeWakuta 15:c5ade23e5de4 74 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 75 DigitalOut led3(LED3);
YusukeWakuta 43:2d8bffac06cd 76 Serial pc(USBTX,USBRX);
YusukeWakuta 49:87af91607b46 77 InputCalc pitchIC;
YusukeWakuta 49:87af91607b46 78 InputCalc rollIC;
tsumagari 37:7ce6e87b9c36 79 //DigitalOut fusokuControl(p25);
tsumagari 9:c32999b4d2f9 80 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 81
taurin 4:4a13dd263a7b 82 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 83 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 59:04c26e2c20bb 84 char inputDatas_R[INPUT_DATAS_NUM+1];//for \0 space
tsumagari 59:04c26e2c20bb 85 char inputDatas_L[INPUT_DATAS_NUM+1];
tsumagari 59:04c26e2c20bb 86 char toKeikiBufInput[(3+1)*2];
tsumagari 58:0b4b842149de 87 CANMessage recmsg_R(0x701,yokutanDatas_R,YOKUTAN_DATAS_NUM);
tsumagari 58:0b4b842149de 88 CANMessage recmsg_L(0x701,yokutanDatas_L,YOKUTAN_DATAS_NUM);
tsumagari 58:0b4b842149de 89 int to_keiki_sending_timer = 0;
YusukeWakuta 0:b1290ca6c4a2 90
tsumagari 58:0b4b842149de 91 void receiveFromPc(){
tsumagari 58:0b4b842149de 92 while(pc.readable()){
tsumagari 58:0b4b842149de 93 char c = pc.getc();
tsumagari 58:0b4b842149de 94 for(int i = 0; debugflag[i].key != '0'; i++){
tsumagari 58:0b4b842149de 95 if(debugflag[i].key == c)
tsumagari 58:0b4b842149de 96 debugflag[i].flag = !(debugflag[i].flag);
tsumagari 58:0b4b842149de 97 }
tsumagari 58:0b4b842149de 98 }
tsumagari 58:0b4b842149de 99 }
tsumagari 58:0b4b842149de 100
YusukeWakuta 49:87af91607b46 101 void SyntheRollAndPitch(double pitch,double roll,float* inputR,float* inputL)
YusukeWakuta 49:87af91607b46 102 {
tsumagari 57:f6226219e93d 103 *inputR = (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
tsumagari 57:f6226219e93d 104 *inputL = (-pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
YusukeWakuta 49:87af91607b46 105 }
tsumagari 36:5ad172e48384 106
YusukeWakuta 15:c5ade23e5de4 107 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 108 {
YusukeWakuta 53:947eff37d01d 109 double normedPitch = pitchIC.Processing(pitchPin.read());
YusukeWakuta 49:87af91607b46 110 double normedRoll = rollIC.Processing(rollPin.read());
YusukeWakuta 52:a9d23be503c6 111
YusukeWakuta 51:8a579a19a4ff 112 float inputR = 0.0;
YusukeWakuta 51:8a579a19a4ff 113 float inputL = 0.0;
tsumagari 57:f6226219e93d 114
tsumagari 58:0b4b842149de 115 uint8_t soff = servoOff;
tsumagari 58:0b4b842149de 116 uint8_t drR = drug_R;
tsumagari 58:0b4b842149de 117 uint8_t drL = drug_L;
tsumagari 58:0b4b842149de 118 drR = drR<<1 | (soff&0x00000001);
tsumagari 58:0b4b842149de 119 drL = drL<<1 | (soff&0x00000001);
tsumagari 58:0b4b842149de 120 //dr= 3->drug on and servo off, 2->drug on and servo on, 1->drug off and servo off, 0->drug off and servo on
tsumagari 58:0b4b842149de 121
YusukeWakuta 51:8a579a19a4ff 122 SyntheRollAndPitch(normedPitch,normedRoll,&inputR,&inputL);
tsumagari 58:0b4b842149de 123 // adjust inputR/L (-0.99~0.99)
tsumagari 58:0b4b842149de 124 if(abs(inputR)==1.0){
tsumagari 58:0b4b842149de 125 inputR -= 0.01*SIGN(inputR);
tsumagari 58:0b4b842149de 126 }
tsumagari 58:0b4b842149de 127 if(abs(inputL)==1.0){
tsumagari 58:0b4b842149de 128 inputL -= 0.01*SIGN(inputL);
tsumagari 58:0b4b842149de 129 }
tsumagari 59:04c26e2c20bb 130 sprintf(inputDatas_R,"%03d%1d",(int)(inputR*99),drR);
tsumagari 59:04c26e2c20bb 131 sprintf(inputDatas_L,"%03d%1d",(int)(inputL*99),drL);
tsumagari 59:04c26e2c20bb 132 sprintf(toKeikiBufInput,"%03d%1d,%03d%d",(int)(normedRoll*99),drR,(int)(normedPitch*99),drL);
tsumagari 58:0b4b842149de 133 print2pc(INPUT_DATA_DEBUG_FLAG,"inR:%5.2f inL:%5.2f nrmR:%5.2f nrmL:%5.2f sMax:%5.2f sMin:%5.2f drugR:%d drugL:%d\n"
tsumagari 58:0b4b842149de 134 ,inputR,inputL,normedPitch,normedRoll,rollIC.shiftedMax,rollIC.shiftedMin,(int)drug_R,(int)drug_L);
YusukeWakuta 51:8a579a19a4ff 135
YusukeWakuta 51:8a579a19a4ff 136 //inputDatas_R[sizeof(float)] = (char)drug_R;
YusukeWakuta 51:8a579a19a4ff 137 //inputDatas_L[sizeof(float)] = (char)drug_L;
YusukeWakuta 52:a9d23be503c6 138 // for(int i = 0; i< 7; i++)
YusukeWakuta 52:a9d23be503c6 139 // pc.printf("%c",inputDatas_R[i]);
YusukeWakuta 52:a9d23be503c6 140 // pc.printf("\n\r");
YusukeWakuta 52:a9d23be503c6 141 //pc.printf("%d",(int)drug_R);
YusukeWakuta 51:8a579a19a4ff 142
YusukeWakuta 49:87af91607b46 143 led4 =! led4;
YusukeWakuta 0:b1290ca6c4a2 144 }
YusukeWakuta 0:b1290ca6c4a2 145
YusukeWakuta 15:c5ade23e5de4 146 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 147 {
tsumagari 58:0b4b842149de 148 /*can*/
tsumagari 10:b7159feb11fd 149 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 150 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 58:0b4b842149de 151
tsumagari 58:0b4b842149de 152 /*keiki*/
tsumagari 58:0b4b842149de 153 char *toKeikiBuf = (char*)malloc(sizeof(char)*55);
tsumagari 58:0b4b842149de 154 int ahead = 0;
tsumagari 59:04c26e2c20bb 155 // ahead += sprintf(toKeikiBuf,"i%s,%s",inputDatas_R,inputDatas_L);
tsumagari 59:04c26e2c20bb 156 ahead += sprintf(toKeikiBuf,"i%s",toKeikiBufInput);
tsumagari 58:0b4b842149de 157 if(can_R.read(recmsg_R)){
tsumagari 58:0b4b842149de 158 led3 = !led3;
tsumagari 58:0b4b842149de 159 char mpu1[3]={recmsg_R.data[0],recmsg_R.data[1],recmsg_R.data[2]};
tsumagari 58:0b4b842149de 160 char mpu2[3]={recmsg_R.data[3],recmsg_R.data[4],recmsg_R.data[5]};
tsumagari 58:0b4b842149de 161 char v = recmsg_R.data[6];
tsumagari 58:0b4b842149de 162 ahead += sprintf(toKeikiBuf+ahead,",R%d,%d,%d",*(int*)mpu1,*(int*)mpu2,(int)v);
tsumagari 58:0b4b842149de 163 TO_KEIKI_DATA_DEBUG_FLAG = 1;
YusukeWakuta 0:b1290ca6c4a2 164 }
tsumagari 58:0b4b842149de 165 if(can_L.read(recmsg_L)){
tsumagari 58:0b4b842149de 166 led2 = !led2;
tsumagari 58:0b4b842149de 167 char mpu1[4]={recmsg_L.data[0],recmsg_L.data[1],recmsg_L.data[2],(recmsg_L.data[2]>>7 == 0)?0:0xff};
tsumagari 58:0b4b842149de 168 char mpu2[4]={recmsg_L.data[3],recmsg_L.data[4],recmsg_L.data[5],(recmsg_L.data[5]>>7 == 0)?0:0xff};
tsumagari 58:0b4b842149de 169 char v = recmsg_L.data[6];
tsumagari 58:0b4b842149de 170 ahead += sprintf(toKeikiBuf+ahead,",L%d,%d,%d",*(int32_t*)mpu1,*(int32_t*)mpu2,(int)v);
tsumagari 58:0b4b842149de 171 TO_KEIKI_DATA_DEBUG_FLAG = 1;
YusukeWakuta 33:d54dacb7a0aa 172 }
tsumagari 58:0b4b842149de 173 sprintf(toKeikiBuf+ahead,"\n\r");
tsumagari 58:0b4b842149de 174 pc.puts(toKeikiBuf);
tsumagari 58:0b4b842149de 175
tsumagari 58:0b4b842149de 176 free(toKeikiBuf);
tsumagari 58:0b4b842149de 177
YusukeWakuta 0:b1290ca6c4a2 178 }
tsumagari 8:dbc8c87dac78 179
YusukeWakuta 15:c5ade23e5de4 180 void init()
YusukeWakuta 15:c5ade23e5de4 181 {
tsumagari 57:f6226219e93d 182 servoOff.mode(PullDown);
YusukeWakuta 15:c5ade23e5de4 183 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 184 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 185 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 186 }
YusukeWakuta 0:b1290ca6c4a2 187 }
YusukeWakuta 0:b1290ca6c4a2 188
tsumagari 57:f6226219e93d 189
YusukeWakuta 0:b1290ca6c4a2 190
YusukeWakuta 15:c5ade23e5de4 191 int main()
YusukeWakuta 15:c5ade23e5de4 192 {
tsumagari 37:7ce6e87b9c36 193 // Thread tickFusokuThread(&tickFusoku);
YusukeWakuta 0:b1290ca6c4a2 194 init();
YusukeWakuta 49:87af91607b46 195 pitchIC.setNeutral(pitchPin.read());
YusukeWakuta 49:87af91607b46 196 rollIC.setNeutral(rollPin.read());
YusukeWakuta 15:c5ade23e5de4 197 while(1) {
tsumagari 58:0b4b842149de 198 receiveFromPc();
tsumagari 8:dbc8c87dac78 199 InputControlValues();
tsumagari 10:b7159feb11fd 200 wait_us(5);
tsumagari 9:c32999b4d2f9 201 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 202 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 203 }
YusukeWakuta 0:b1290ca6c4a2 204 }