albatross / Mbed 2 deprecated ControlMain2017

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

Dependencies:   mbed mbed-rtos

Fork of Control_Main_Full_20160608 by albatross

main.cpp@57:f6226219e93d, 2017-10-21 (annotated)

Committer:
tsumagari
Date:
Sat Oct 21 06:52:43 2017 +0000
Branch:
?????
Revision:
57:f6226219e93d
Parent:
56:b39ffd94aa54
Child:
58:0b4b842149de
servoOff????????can?????????

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
tsumagari 57:f6226219e93d 8 #define INPUT_DATAS_NUM 8 //ここは8バイトまでしか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
tsumagari 55:1db68795bccf 14 #define PITCH_PER_ROLL 1.0//pitchがrollの何倍影響するかを示す値
YusukeWakuta 52:a9d23be503c6 15 #define AVE_NUM 100
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 55:1db68795bccf 22 //extern "C" void mbed_reset();
tsumagari 55:1db68795bccf 23 //InterruptIn resetPin(p22);
tsumagari 55:1db68795bccf 24 //Timer resetTimeCount;
tsumagari 55:1db68795bccf 25 //void resetInterrupt()
tsumagari 55:1db68795bccf 26 //{
tsumagari 55:1db68795bccf 27 // while(resetPin) {
tsumagari 55:1db68795bccf 28 // resetTimeCount.start();
tsumagari 55:1db68795bccf 29 // if(resetTimeCount.read()>3) mbed_reset();
tsumagari 55:1db68795bccf 30 // }
tsumagari 55:1db68795bccf 31 // resetTimeCount.reset();
tsumagari 55:1db68795bccf 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);
tsumagari 57:f6226219e93d 42 DigitalIn servoOff(p26);
YusukeWakuta 25:12408563540d 43 DigitalOut led1(LED1);
YusukeWakuta 25:12408563540d 44 DigitalOut led2(LED2);
YusukeWakuta 15:c5ade23e5de4 45 DigitalOut led4(LED4);
YusukeWakuta 15:c5ade23e5de4 46 DigitalOut led3(LED3);
YusukeWakuta 43:2d8bffac06cd 47 Serial pc(USBTX,USBRX);
YusukeWakuta 49:87af91607b46 48 InputCalc pitchIC;
YusukeWakuta 49:87af91607b46 49 InputCalc rollIC;
tsumagari 37:7ce6e87b9c36 50 //DigitalOut fusokuControl(p25);
tsumagari 9:c32999b4d2f9 51 //Ticker sendDatasTicker;
YusukeWakuta 0:b1290ca6c4a2 52
taurin 4:4a13dd263a7b 53 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
taurin 4:4a13dd263a7b 54 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
tsumagari 10:b7159feb11fd 55 char inputDatas_R[INPUT_DATAS_NUM];
tsumagari 10:b7159feb11fd 56 char inputDatas_L[INPUT_DATAS_NUM];
YusukeWakuta 24:72f9cb6e9440 57
YusukeWakuta 3:0e66ce2ab2fb 58 CANMessage recmsg_R;
YusukeWakuta 3:0e66ce2ab2fb 59 CANMessage recmsg_L;
YusukeWakuta 0:b1290ca6c4a2 60
tsumagari 37:7ce6e87b9c36 61 //void tickFusoku(void const * arg){
tsumagari 37:7ce6e87b9c36 62 // while(1){
tsumagari 37:7ce6e87b9c36 63 // fusokuControl = 1;
tsumagari 37:7ce6e87b9c36 64 // wait_us(500);
tsumagari 37:7ce6e87b9c36 65 // fusokuControl = 0;
tsumagari 37:7ce6e87b9c36 66 // wait_us(4500);
tsumagari 37:7ce6e87b9c36 67 // }
tsumagari 37:7ce6e87b9c36 68 //}
YusukeWakuta 49:87af91607b46 69 void SyntheRollAndPitch(double pitch,double roll,float* inputR,float* inputL)
YusukeWakuta 49:87af91607b46 70 {
tsumagari 57:f6226219e93d 71 *inputR = (pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
tsumagari 57:f6226219e93d 72 *inputL = (-pitch * PITCH_PER_ROLL - roll)/(PITCH_PER_ROLL + 1.0);
YusukeWakuta 49:87af91607b46 73 }
tsumagari 36:5ad172e48384 74
YusukeWakuta 15:c5ade23e5de4 75 void InputControlValues()
YusukeWakuta 15:c5ade23e5de4 76 {
YusukeWakuta 53:947eff37d01d 77 double normedPitch = pitchIC.Processing(pitchPin.read());
YusukeWakuta 49:87af91607b46 78 double normedRoll = rollIC.Processing(rollPin.read());
YusukeWakuta 52:a9d23be503c6 79
YusukeWakuta 51:8a579a19a4ff 80 float inputR = 0.0;
YusukeWakuta 51:8a579a19a4ff 81 float inputL = 0.0;
tsumagari 57:f6226219e93d 82
tsumagari 57:f6226219e93d 83 int servoV = servoOff;
YusukeWakuta 51:8a579a19a4ff 84 SyntheRollAndPitch(normedPitch,normedRoll,&inputR,&inputL);
tsumagari 57:f6226219e93d 85 sprintf(inputDatas_R,"%5.2f%d%d",inputR,(int)drug_R,servoV);
tsumagari 57:f6226219e93d 86 sprintf(inputDatas_L,"%5.2f%d%d",inputL,(int)drug_L,servoV);
tsumagari 56:b39ffd94aa54 87 pc.printf("inR:%5.2f inL:%5.2f nrmR:%5.2f nrmL:%5.2f sMax:%5.2f sMin:%5.2f\n\r",inputR,inputL,normedPitch,normedRoll,rollIC.shiftedMax,rollIC.shiftedMin);
YusukeWakuta 51:8a579a19a4ff 88
YusukeWakuta 51:8a579a19a4ff 89 //inputDatas_R[sizeof(float)] = (char)drug_R;
YusukeWakuta 51:8a579a19a4ff 90 //inputDatas_L[sizeof(float)] = (char)drug_L;
YusukeWakuta 52:a9d23be503c6 91 // for(int i = 0; i< 7; i++)
YusukeWakuta 52:a9d23be503c6 92 // pc.printf("%c",inputDatas_R[i]);
YusukeWakuta 52:a9d23be503c6 93 // pc.printf("\n\r");
YusukeWakuta 52:a9d23be503c6 94 //pc.printf("%d",(int)drug_R);
YusukeWakuta 51:8a579a19a4ff 95
YusukeWakuta 49:87af91607b46 96 led4 =! led4;
YusukeWakuta 0:b1290ca6c4a2 97 }
YusukeWakuta 0:b1290ca6c4a2 98
YusukeWakuta 15:c5ade23e5de4 99 void SendDatas()
YusukeWakuta 15:c5ade23e5de4 100 {
tsumagari 10:b7159feb11fd 101 can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
tsumagari 10:b7159feb11fd 102 can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
tsumagari 8:dbc8c87dac78 103 toKeikiSerial.putc(';');
YusukeWakuta 15:c5ade23e5de4 104 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 34:0cff1601bc13 105 toKeikiSerial.putc(yokutanDatas_R[i]);
YusukeWakuta 34:0cff1601bc13 106 }
YusukeWakuta 34:0cff1601bc13 107 for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
YusukeWakuta 0:b1290ca6c4a2 108 toKeikiSerial.putc(yokutanDatas_L[i]);
YusukeWakuta 0:b1290ca6c4a2 109 }
YusukeWakuta 15:c5ade23e5de4 110 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 111 toKeikiSerial.putc(inputDatas_R[i]);
YusukeWakuta 33:d54dacb7a0aa 112 }
YusukeWakuta 33:d54dacb7a0aa 113 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 4:4a13dd263a7b 114 toKeikiSerial.putc(inputDatas_L[i]);
taurin 4:4a13dd263a7b 115 }
YusukeWakuta 0:b1290ca6c4a2 116 }
tsumagari 8:dbc8c87dac78 117
YusukeWakuta 15:c5ade23e5de4 118 void init()
YusukeWakuta 15:c5ade23e5de4 119 {
tsumagari 9:c32999b4d2f9 120 //--------------------------------------(resetInterrupt init)
tsumagari 55:1db68795bccf 121 //resetPin.rise(resetInterrupt);
tsumagari 55:1db68795bccf 122 // resetPin.mode(PullDown);
tsumagari 9:c32999b4d2f9 123 //-----------------------------------------------------------
tsumagari 9:c32999b4d2f9 124 // sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
tsumagari 57:f6226219e93d 125 servoOff.mode(PullDown);
YusukeWakuta 15:c5ade23e5de4 126 for(int i = 0; i < INPUT_DATAS_NUM; i++) {
taurin 2:9dc7d5f1e910 127 inputDatas_R[i] = 0;
taurin 2:9dc7d5f1e910 128 inputDatas_L[i] = 0;
taurin 2:9dc7d5f1e910 129 }
YusukeWakuta 0:b1290ca6c4a2 130 }
YusukeWakuta 0:b1290ca6c4a2 131
tsumagari 57:f6226219e93d 132
YusukeWakuta 0:b1290ca6c4a2 133
YusukeWakuta 15:c5ade23e5de4 134 int main()
YusukeWakuta 15:c5ade23e5de4 135 {
tsumagari 37:7ce6e87b9c36 136 // Thread tickFusokuThread(&tickFusoku);
YusukeWakuta 0:b1290ca6c4a2 137 init();
YusukeWakuta 49:87af91607b46 138 pitchIC.setNeutral(pitchPin.read());
YusukeWakuta 49:87af91607b46 139 rollIC.setNeutral(rollPin.read());
YusukeWakuta 15:c5ade23e5de4 140 while(1) {
YusukeWakuta 49:87af91607b46 141
tsumagari 8:dbc8c87dac78 142 InputControlValues();
tsumagari 10:b7159feb11fd 143 wait_us(5);
tsumagari 57:f6226219e93d 144 // receiveDatas();
tsumagari 9:c32999b4d2f9 145 SendDatas();
YusukeWakuta 0:b1290ca6c4a2 146 wait(WAIT_LOOP_TIME);
YusukeWakuta 0:b1290ca6c4a2 147 }
YusukeWakuta 0:b1290ca6c4a2 148 }