2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
main.cpp@78:e272e65f5a0e, 2018-03-08 (annotated)
- Committer:
- tsumagari
- Date:
- Thu Mar 08 08:07:31 2018 +0000
- Branch:
- mpu????????
- Revision:
- 78:e272e65f5a0e
- Parent:
- 77:ca4ab599ba2b
setTrim,setMaxDeg??????servoPeriod=10000(us)?'d'????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
taurin | 0:e052602db102 | 1 | //翼端can program |
tsumagari | 73:05feda5b0f98 | 2 | /***************************************** |
tsumagari | 73:05feda5b0f98 | 3 | * ----- |
tsumagari | 73:05feda5b0f98 | 4 | * | 1 | Is R pin |
tsumagari | 73:05feda5b0f98 | 5 | * | 2 | In set mode pin |
tsumagari | 73:05feda5b0f98 | 6 | * | 3 | on:Elevon, off:Drug |
tsumagari | 73:05feda5b0f98 | 7 | * | 4 | on:Trim, off:Max deg |
tsumagari | 73:05feda5b0f98 | 8 | * ----- |
tsumagari | 73:05feda5b0f98 | 9 | * |
tsumagari | 73:05feda5b0f98 | 10 | ****************************************** |
tsumagari | 73:05feda5b0f98 | 11 | **magic character of debug** |
tsumagari | 73:05feda5b0f98 | 12 | * |
tsumagari | 73:05feda5b0f98 | 13 | *(0)s:sending datas: mpu, servoV |
tsumagari | 73:05feda5b0f98 | 14 | *(1)g:getting datas: eruronfloat, drugInput, servoOff |
tsumagari | 73:05feda5b0f98 | 15 | *(2)c:servo config:eruronTrim,drugTrim,eMD,dMD |
tsumagari | 74:8ccd04302a7f | 16 | *(3)j:data to debug what you want(joker) |
tsumagari | 73:05feda5b0f98 | 17 | ****************************************** |
tsumagari | 73:05feda5b0f98 | 18 | */ |
taurin | 0:e052602db102 | 19 | #include "mbed.h" |
tsumagari | 22:b38bc18ec3a1 | 20 | #include "MPU6050.h" |
taurin | 0:e052602db102 | 21 | #include "INA226.hpp" |
YusukeWakuta | 42:bf98a29e81ac | 22 | #include "rtos.h" |
YusukeWakuta | 24:d416722b4aad | 23 | #include "XBusServo.h" |
YusukeWakuta | 57:d7b709dd1c4f | 24 | #include "math.h" |
taurin | 4:450cafd95ac3 | 25 | |
tsumagari | 78:e272e65f5a0e | 26 | #define INIT_SERVO_PERIOD_US 10000 |
taurin | 4:450cafd95ac3 | 27 | #define WAIT_LOOP_TIME 0.02 |
tsumagari | 74:8ccd04302a7f | 28 | #define YOKUTAN_DATAS_NUM 7 |
tsumagari | 74:8ccd04302a7f | 29 | #define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、操舵コードと数字を合わせる必要あり。 |
tsumagari | 74:8ccd04302a7f | 30 | #define ERURON_DATAS_NUM 3 //送られてくるエルロンインプットの文字数 |
tsumagari | 74:8ccd04302a7f | 31 | #define TO_SEND_CAN_ID 0x701 //0x0>>0x7ff |
taurin | 4:450cafd95ac3 | 32 | #define SEND_DATAS_LOOP_TIME 0.1 |
tsumagari | 33:d075918d4846 | 33 | #define RECEIVE_DATAS_LOOP_TIME 0.1 |
taurin | 12:fd9d241843f4 | 34 | |
YusukeWakuta | 42:bf98a29e81ac | 35 | #define MPU_LOOP_TIME 0.01 |
YusukeWakuta | 42:bf98a29e81ac | 36 | #define MPU_DELT_MIN 250 |
YusukeWakuta | 42:bf98a29e81ac | 37 | |
tsumagari | 78:e272e65f5a0e | 38 | //値を大きくするとサーボに向かって反時計回りになる |
tsumagari | 78:e272e65f5a0e | 39 | //Rエレボンは頭上げ==値大きく |
tsumagari | 78:e272e65f5a0e | 40 | #define ERURON_MOVE_DEG_INI_R 0.3 |
tsumagari | 78:e272e65f5a0e | 41 | #define ERURON_TRIM_INI_R 0.5 |
tsumagari | 78:e272e65f5a0e | 42 | //Rラダーは右回り(時計回り)が開く |
tsumagari | 78:e272e65f5a0e | 43 | #define DRUG_MOVE_DEG_INI_R -1.41297 |
tsumagari | 78:e272e65f5a0e | 44 | #define DRUG_TRIM_INI_R 1.014408 |
tsumagari | 78:e272e65f5a0e | 45 | //Lエレボンは頭上げ==値小さく |
tsumagari | 78:e272e65f5a0e | 46 | #define ERURON_MOVE_DEG_INI_L -0.32 |
tsumagari | 78:e272e65f5a0e | 47 | #define ERURON_TRIM_INI_L 0.51567 |
tsumagari | 78:e272e65f5a0e | 48 | //Lラダーは左回りが開く |
tsumagari | 78:e272e65f5a0e | 49 | #define DRUG_MOVE_DEG_INI_L 0.983027 |
tsumagari | 78:e272e65f5a0e | 50 | #define DRUG_TRIM_INI_L -0.1 |
taurin | 2:7fcb4f970a02 | 51 | |
tsumagari | 74:8ccd04302a7f | 52 | /*ドラッグラダー |
tsumagari | 74:8ccd04302a7f | 53 | 初期値 0.65 |
tsumagari | 74:8ccd04302a7f | 54 | 最大角0.99 |
tsumagari | 74:8ccd04302a7f | 55 | */ |
tsumagari | 74:8ccd04302a7f | 56 | |
tsumagari | 73:05feda5b0f98 | 57 | #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0) |
tsumagari | 73:05feda5b0f98 | 58 | #define SENDING_DATA_DEBUG_FLAG debugflag[0].flag |
tsumagari | 73:05feda5b0f98 | 59 | #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag |
tsumagari | 77:ca4ab599ba2b | 60 | #define SERVO_CONFIG_DEBUG_FLAG debugflag[2].flag |
tsumagari | 74:8ccd04302a7f | 61 | #define DEBUG_FLAG debugflag[3].flag |
tsumagari | 78:e272e65f5a0e | 62 | #define DRUG_OPEN_FLAG debugflag[4].flag |
tsumagari | 67:94fe2180f39a | 63 | |
tsumagari | 73:05feda5b0f98 | 64 | struct flaglist{ |
tsumagari | 73:05feda5b0f98 | 65 | char key; |
tsumagari | 73:05feda5b0f98 | 66 | bool flag; |
tsumagari | 73:05feda5b0f98 | 67 | }; |
tsumagari | 74:8ccd04302a7f | 68 | struct flaglist debugflag[]={ |
tsumagari | 74:8ccd04302a7f | 69 | {'s',0}, |
tsumagari | 74:8ccd04302a7f | 70 | {'g',0}, |
tsumagari | 74:8ccd04302a7f | 71 | {'c',0}, |
tsumagari | 74:8ccd04302a7f | 72 | {'j',0}, |
tsumagari | 78:e272e65f5a0e | 73 | {'d',0}, |
tsumagari | 74:8ccd04302a7f | 74 | {'0',0} |
tsumagari | 74:8ccd04302a7f | 75 | }; |
tsumagari | 67:94fe2180f39a | 76 | |
tsumagari | 74:8ccd04302a7f | 77 | const char *configfilename = "/local/CONFIG.csv"; |
YusukeWakuta | 24:d416722b4aad | 78 | |
taurin | 0:e052602db102 | 79 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 80 | CANMessage recmsg; |
taurin | 0:e052602db102 | 81 | Serial pc(USBTX,USBRX); |
taurin | 0:e052602db102 | 82 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 83 | INA226 VCmonitor(ina226_i2c); |
tsumagari | 70:7da315abec17 | 84 | PwmOut drugServo(p23); |
tsumagari | 71:a00561d455d1 | 85 | PwmOut eruronServo(p25); |
taurin | 1:9cc932a16d17 | 86 | DigitalOut led1(LED1); |
tsumagari | 69:073116d31a56 | 87 | AnalogIn setDeg10(p20); |
tsumagari | 69:073116d31a56 | 88 | AnalogIn setDeg1(p19); |
tsumagari | 77:ca4ab599ba2b | 89 | DigitalIn IsRPin(p16,PullDown); |
tsumagari | 77:ca4ab599ba2b | 90 | DigitalIn InSetModePin(p15,PullDown); |
tsumagari | 77:ca4ab599ba2b | 91 | DigitalIn EDstatePin(p14,PullDown); |
tsumagari | 77:ca4ab599ba2b | 92 | DigitalIn TrimMaxDegPin(p13,PullDown); |
tsumagari | 71:a00561d455d1 | 93 | DigitalOut servoOff(p17); |
YusukeWakuta | 44:624a4469ae21 | 94 | DigitalOut led2(LED2); |
taurin | 4:450cafd95ac3 | 95 | DigitalOut led3(LED3); |
taurin | 4:450cafd95ac3 | 96 | DigitalOut led4(LED4); |
taurin | 4:450cafd95ac3 | 97 | Ticker sendDatasTicker; |
tsumagari | 22:b38bc18ec3a1 | 98 | //Ticker toStringTicker; |
taurin | 16:82310bf7c326 | 99 | Ticker receiveDatasTicker; |
YusukeWakuta | 42:bf98a29e81ac | 100 | MPU6050 mpu6050; |
YusukeWakuta | 42:bf98a29e81ac | 101 | Timer t; |
YusukeWakuta | 42:bf98a29e81ac | 102 | |
tsumagari | 67:94fe2180f39a | 103 | LocalFileSystem local("local"); |
tsumagari | 67:94fe2180f39a | 104 | |
tsumagari | 74:8ccd04302a7f | 105 | char toSendDatas[YOKUTAN_DATAS_NUM]; |
tsumagari | 74:8ccd04302a7f | 106 | char intValues[ERURON_DATAS_NUM]; |
taurin | 12:fd9d241843f4 | 107 | float eruronTrim; |
taurin | 12:fd9d241843f4 | 108 | float drugTrim; |
taurin | 12:fd9d241843f4 | 109 | float eruronMoveDeg; |
taurin | 12:fd9d241843f4 | 110 | float drugMoveDeg; |
YusukeWakuta | 59:f007e543f8c9 | 111 | float eruronfloat = 0.0; |
taurin | 0:e052602db102 | 112 | unsigned short ina_val; |
taurin | 0:e052602db102 | 113 | double V,C; |
taurin | 0:e052602db102 | 114 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 115 | bool INA_FLAG; |
tsumagari | 22:b38bc18ec3a1 | 116 | bool MPU_FLAG; |
tsumagari | 74:8ccd04302a7f | 117 | |
YusukeWakuta | 42:bf98a29e81ac | 118 | int gyroX; |
YusukeWakuta | 42:bf98a29e81ac | 119 | int gyroY; |
YusukeWakuta | 42:bf98a29e81ac | 120 | int gyroZ; |
YusukeWakuta | 42:bf98a29e81ac | 121 | float sum = 0; |
tsumagari | 74:8ccd04302a7f | 122 | int drugInput = 0; |
tsumagari | 74:8ccd04302a7f | 123 | int servoOffVer = 0; |
YusukeWakuta | 42:bf98a29e81ac | 124 | uint32_t sumCount = 0; |
taurin | 0:e052602db102 | 125 | |
tsumagari | 22:b38bc18ec3a1 | 126 | char gyro_c[6] = {0,0,0,0,0,0}; |
taurin | 4:450cafd95ac3 | 127 | |
taurin | 4:450cafd95ac3 | 128 | void toString(); |
taurin | 16:82310bf7c326 | 129 | void receiveDatas(); |
taurin | 16:82310bf7c326 | 130 | void WriteServo(); |
YusukeWakuta | 42:bf98a29e81ac | 131 | void MpuInit(); |
tsumagari | 78:e272e65f5a0e | 132 | int calcPulse_us(float analog); |
YusukeWakuta | 42:bf98a29e81ac | 133 | void mpuProcessing(void const *arg); |
taurin | 0:e052602db102 | 134 | |
YusukeWakuta | 24:d416722b4aad | 135 | Ticker gTimer; |
YusukeWakuta | 24:d416722b4aad | 136 | |
tsumagari | 73:05feda5b0f98 | 137 | |
tsumagari | 73:05feda5b0f98 | 138 | |
tsumagari | 73:05feda5b0f98 | 139 | void receiveFromPc(){ |
tsumagari | 74:8ccd04302a7f | 140 | while(pc.readable()){ |
tsumagari | 73:05feda5b0f98 | 141 | char c = pc.getc(); |
tsumagari | 73:05feda5b0f98 | 142 | for(int i = 0; debugflag[i].key != '0'; i++){ |
tsumagari | 73:05feda5b0f98 | 143 | if(debugflag[i].key == c) |
tsumagari | 73:05feda5b0f98 | 144 | debugflag[i].flag = !(debugflag[i].flag); |
tsumagari | 73:05feda5b0f98 | 145 | } |
tsumagari | 73:05feda5b0f98 | 146 | } |
tsumagari | 73:05feda5b0f98 | 147 | } |
tsumagari | 73:05feda5b0f98 | 148 | |
YusukeWakuta | 26:f14579683f98 | 149 | bool servoInit() |
YusukeWakuta | 26:f14579683f98 | 150 | { |
tsumagari | 78:e272e65f5a0e | 151 | drugServo.period_us(INIT_SERVO_PERIOD_US); |
taurin | 0:e052602db102 | 152 | return true; |
taurin | 0:e052602db102 | 153 | } |
taurin | 0:e052602db102 | 154 | |
YusukeWakuta | 26:f14579683f98 | 155 | void sendDatas() |
YusukeWakuta | 24:d416722b4aad | 156 | { |
tsumagari | 74:8ccd04302a7f | 157 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, YOKUTAN_DATAS_NUM))) { |
taurin | 4:450cafd95ac3 | 158 | } |
taurin | 4:450cafd95ac3 | 159 | } |
taurin | 4:450cafd95ac3 | 160 | |
YusukeWakuta | 42:bf98a29e81ac | 161 | void MpuInit() |
YusukeWakuta | 42:bf98a29e81ac | 162 | { |
tsumagari | 72:aef1ec8c66c7 | 163 | i2c.frequency(400000); // use fast (400 kHz) I2C |
YusukeWakuta | 42:bf98a29e81ac | 164 | t.start(); |
YusukeWakuta | 42:bf98a29e81ac | 165 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 |
YusukeWakuta | 42:bf98a29e81ac | 166 | if (whoami == 0x68) { // WHO_AM_I should always be 0x68 |
YusukeWakuta | 42:bf98a29e81ac | 167 | Thread::wait(100); |
YusukeWakuta | 42:bf98a29e81ac | 168 | mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values |
YusukeWakuta | 42:bf98a29e81ac | 169 | Thread::wait(100); |
YusukeWakuta | 42:bf98a29e81ac | 170 | if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { |
YusukeWakuta | 42:bf98a29e81ac | 171 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
YusukeWakuta | 42:bf98a29e81ac | 172 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
YusukeWakuta | 42:bf98a29e81ac | 173 | mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
YusukeWakuta | 42:bf98a29e81ac | 174 | Thread::wait(200); |
YusukeWakuta | 42:bf98a29e81ac | 175 | } else { |
YusukeWakuta | 42:bf98a29e81ac | 176 | } |
YusukeWakuta | 42:bf98a29e81ac | 177 | } else { |
YusukeWakuta | 42:bf98a29e81ac | 178 | //pc.printf("out\n\r"); // Loop forever if communication doesn't happen |
YusukeWakuta | 42:bf98a29e81ac | 179 | } |
YusukeWakuta | 42:bf98a29e81ac | 180 | } |
YusukeWakuta | 42:bf98a29e81ac | 181 | |
YusukeWakuta | 42:bf98a29e81ac | 182 | |
YusukeWakuta | 42:bf98a29e81ac | 183 | void mpuProcessing(void const *arg) |
YusukeWakuta | 42:bf98a29e81ac | 184 | { |
YusukeWakuta | 42:bf98a29e81ac | 185 | MpuInit(); |
YusukeWakuta | 42:bf98a29e81ac | 186 | while(1) { |
YusukeWakuta | 42:bf98a29e81ac | 187 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
YusukeWakuta | 42:bf98a29e81ac | 188 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
YusukeWakuta | 42:bf98a29e81ac | 189 | mpu6050.getAres(); |
YusukeWakuta | 42:bf98a29e81ac | 190 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
YusukeWakuta | 42:bf98a29e81ac | 191 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
YusukeWakuta | 42:bf98a29e81ac | 192 | az = (float)accelCount[2]*aRes - accelBias[2]; |
YusukeWakuta | 42:bf98a29e81ac | 193 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
YusukeWakuta | 42:bf98a29e81ac | 194 | mpu6050.getGres(); |
YusukeWakuta | 42:bf98a29e81ac | 195 | gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set |
YusukeWakuta | 42:bf98a29e81ac | 196 | gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; |
YusukeWakuta | 42:bf98a29e81ac | 197 | gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; |
YusukeWakuta | 42:bf98a29e81ac | 198 | tempCount = mpu6050.readTempData(); // Read the x/y/z adc values |
YusukeWakuta | 42:bf98a29e81ac | 199 | temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade |
YusukeWakuta | 42:bf98a29e81ac | 200 | } |
YusukeWakuta | 42:bf98a29e81ac | 201 | Now = t.read_us(); |
YusukeWakuta | 42:bf98a29e81ac | 202 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
YusukeWakuta | 42:bf98a29e81ac | 203 | lastUpdate = Now; |
YusukeWakuta | 42:bf98a29e81ac | 204 | sum += deltat; |
YusukeWakuta | 42:bf98a29e81ac | 205 | sumCount++; |
YusukeWakuta | 42:bf98a29e81ac | 206 | if(lastUpdate - firstUpdate > 10000000.0f) { |
YusukeWakuta | 42:bf98a29e81ac | 207 | beta = 0.04; // decrease filter gain after stabilized |
YusukeWakuta | 42:bf98a29e81ac | 208 | zeta = 0.015; // increasey bias drift gain after stabilized |
YusukeWakuta | 42:bf98a29e81ac | 209 | } |
YusukeWakuta | 42:bf98a29e81ac | 210 | mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); |
YusukeWakuta | 42:bf98a29e81ac | 211 | delt_t = t.read_ms() - count; |
YusukeWakuta | 42:bf98a29e81ac | 212 | if (delt_t > MPU_DELT_MIN) { |
YusukeWakuta | 42:bf98a29e81ac | 213 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
YusukeWakuta | 42:bf98a29e81ac | 214 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
YusukeWakuta | 42:bf98a29e81ac | 215 | roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
YusukeWakuta | 42:bf98a29e81ac | 216 | pitch *= 180.0f / PI; |
YusukeWakuta | 42:bf98a29e81ac | 217 | yaw *= 180.0f / PI; |
YusukeWakuta | 42:bf98a29e81ac | 218 | roll *= 180.0f / PI; |
YusukeWakuta | 48:0bd406fa4a7f | 219 | // myled= !myled; |
YusukeWakuta | 42:bf98a29e81ac | 220 | count = t.read_ms(); |
YusukeWakuta | 42:bf98a29e81ac | 221 | sum = 0; |
YusukeWakuta | 42:bf98a29e81ac | 222 | sumCount = 0; |
YusukeWakuta | 42:bf98a29e81ac | 223 | } |
tsumagari | 64:9d7582920f50 | 224 | gyro_c[0]=(char)((int)pitch); |
tsumagari | 64:9d7582920f50 | 225 | gyro_c[1]=(char)(int)((pitch - (int)pitch)*100); |
tsumagari | 64:9d7582920f50 | 226 | gyro_c[2]=(char)((int)roll); |
tsumagari | 64:9d7582920f50 | 227 | gyro_c[3]=(char)(int)((roll - (int)roll)*100); |
tsumagari | 64:9d7582920f50 | 228 | gyro_c[4]=(char)((int)yaw); |
tsumagari | 64:9d7582920f50 | 229 | gyro_c[5]=(char)(int)((yaw - (int)yaw)*100); |
tsumagari | 73:05feda5b0f98 | 230 | |
YusukeWakuta | 42:bf98a29e81ac | 231 | Thread::wait(1); |
YusukeWakuta | 42:bf98a29e81ac | 232 | }//while(1) |
YusukeWakuta | 42:bf98a29e81ac | 233 | } |
YusukeWakuta | 42:bf98a29e81ac | 234 | |
YusukeWakuta | 42:bf98a29e81ac | 235 | |
YusukeWakuta | 26:f14579683f98 | 236 | bool inaInit() |
YusukeWakuta | 26:f14579683f98 | 237 | { |
YusukeWakuta | 26:f14579683f98 | 238 | if(!VCmonitor.isExist()) { |
taurin | 0:e052602db102 | 239 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 240 | return false; |
taurin | 0:e052602db102 | 241 | } |
taurin | 0:e052602db102 | 242 | ina_val = 0; |
YusukeWakuta | 26:f14579683f98 | 243 | if(VCmonitor.rawRead(0x00,&ina_val) != 0) { |
taurin | 0:e052602db102 | 244 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 245 | return false; |
taurin | 0:e052602db102 | 246 | } |
taurin | 0:e052602db102 | 247 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 248 | return true; |
taurin | 0:e052602db102 | 249 | } |
taurin | 0:e052602db102 | 250 | |
YusukeWakuta | 57:d7b709dd1c4f | 251 | //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。 |
YusukeWakuta | 57:d7b709dd1c4f | 252 | double ConvertDeg(double servo) |
YusukeWakuta | 57:d7b709dd1c4f | 253 | { |
YusukeWakuta | 62:1db967d29809 | 254 | return 0.0011 * pow(servo,3) + 0.017 * pow(servo,2) + 2.3019 * servo - 0.2269; |
YusukeWakuta | 62:1db967d29809 | 255 | //return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105; |
YusukeWakuta | 57:d7b709dd1c4f | 256 | } |
YusukeWakuta | 57:d7b709dd1c4f | 257 | |
YusukeWakuta | 57:d7b709dd1c4f | 258 | //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化 |
YusukeWakuta | 57:d7b709dd1c4f | 259 | double GetValueByHorn(double deg) |
YusukeWakuta | 57:d7b709dd1c4f | 260 | { |
YusukeWakuta | 57:d7b709dd1c4f | 261 | return deg*(0.10/25.7); |
YusukeWakuta | 57:d7b709dd1c4f | 262 | } |
YusukeWakuta | 57:d7b709dd1c4f | 263 | |
YusukeWakuta | 57:d7b709dd1c4f | 264 | double GetFloatByErebon(double erebonDeg) |
YusukeWakuta | 57:d7b709dd1c4f | 265 | { |
YusukeWakuta | 57:d7b709dd1c4f | 266 | double servoDeg = ConvertDeg(erebonDeg); |
YusukeWakuta | 61:988e3f4280ac | 267 | return GetValueByHorn(servoDeg); |
YusukeWakuta | 57:d7b709dd1c4f | 268 | } |
tsumagari | 67:94fe2180f39a | 269 | // configファイル作成 |
tsumagari | 67:94fe2180f39a | 270 | int makeConfigFile() { |
tsumagari | 67:94fe2180f39a | 271 | FILE *fp; |
tsumagari | 67:94fe2180f39a | 272 | if((fp = fopen(configfilename, "w")) == NULL) { |
tsumagari | 67:94fe2180f39a | 273 | pc.printf("can't open %s\n", configfilename); |
tsumagari | 67:94fe2180f39a | 274 | return -1; |
tsumagari | 67:94fe2180f39a | 275 | } |
tsumagari | 67:94fe2180f39a | 276 | fprintf(fp, "// This is Yokutan %s.\n", IsRPin ? "R" : "L"); |
tsumagari | 67:94fe2180f39a | 277 | fprintf(fp, "eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg\n"); |
tsumagari | 67:94fe2180f39a | 278 | fclose(fp); |
tsumagari | 67:94fe2180f39a | 279 | return 0; |
tsumagari | 67:94fe2180f39a | 280 | } |
tsumagari | 67:94fe2180f39a | 281 | |
tsumagari | 67:94fe2180f39a | 282 | // config内容をファイルに追記 |
tsumagari | 67:94fe2180f39a | 283 | int writeConfig() { |
tsumagari | 67:94fe2180f39a | 284 | FILE *fp; |
tsumagari | 67:94fe2180f39a | 285 | if((fp = fopen(configfilename, "a")) == NULL) { |
tsumagari | 67:94fe2180f39a | 286 | pc.printf("can't open %s\n", configfilename); |
tsumagari | 67:94fe2180f39a | 287 | return -1; |
tsumagari | 67:94fe2180f39a | 288 | } |
tsumagari | 67:94fe2180f39a | 289 | fprintf(fp, "%f,%f,%f,%f\n", eruronTrim, drugTrim, eruronMoveDeg, drugMoveDeg); |
tsumagari | 67:94fe2180f39a | 290 | fclose(fp); |
tsumagari | 67:94fe2180f39a | 291 | return 0; |
tsumagari | 67:94fe2180f39a | 292 | } |
tsumagari | 67:94fe2180f39a | 293 | // configの最新情報を読み込み |
tsumagari | 67:94fe2180f39a | 294 | int readConfig() { |
tsumagari | 67:94fe2180f39a | 295 | FILE *fp; |
tsumagari | 67:94fe2180f39a | 296 | char s[256]; |
tsumagari | 67:94fe2180f39a | 297 | if((fp = fopen(configfilename, "r")) == NULL) { |
tsumagari | 67:94fe2180f39a | 298 | return -1; |
tsumagari | 67:94fe2180f39a | 299 | } |
tsumagari | 67:94fe2180f39a | 300 | while(fgets(s, 255, fp) != NULL) { |
tsumagari | 67:94fe2180f39a | 301 | sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg); |
tsumagari | 67:94fe2180f39a | 302 | } |
tsumagari | 67:94fe2180f39a | 303 | fclose(fp); |
tsumagari | 73:05feda5b0f98 | 304 | //debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg); |
tsumagari | 67:94fe2180f39a | 305 | return 0; |
tsumagari | 67:94fe2180f39a | 306 | } |
YusukeWakuta | 57:d7b709dd1c4f | 307 | |
YusukeWakuta | 26:f14579683f98 | 308 | void init() |
YusukeWakuta | 26:f14579683f98 | 309 | { |
tsumagari | 67:94fe2180f39a | 310 | if(readConfig() == -1) { |
tsumagari | 67:94fe2180f39a | 311 | makeConfigFile(); |
tsumagari | 67:94fe2180f39a | 312 | if(IsRPin) { |
tsumagari | 67:94fe2180f39a | 313 | eruronTrim = ERURON_TRIM_INI_R; |
tsumagari | 67:94fe2180f39a | 314 | drugTrim = DRUG_TRIM_INI_R; |
tsumagari | 78:e272e65f5a0e | 315 | // eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R); |
tsumagari | 78:e272e65f5a0e | 316 | eruronMoveDeg = ERURON_MOVE_DEG_INI_R; |
tsumagari | 67:94fe2180f39a | 317 | drugMoveDeg =DRUG_MOVE_DEG_INI_R; |
tsumagari | 67:94fe2180f39a | 318 | } else { |
tsumagari | 67:94fe2180f39a | 319 | eruronTrim = ERURON_TRIM_INI_L; |
tsumagari | 67:94fe2180f39a | 320 | drugTrim = DRUG_TRIM_INI_L; |
tsumagari | 78:e272e65f5a0e | 321 | // eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L); |
tsumagari | 78:e272e65f5a0e | 322 | eruronMoveDeg = ERURON_MOVE_DEG_INI_L; |
tsumagari | 67:94fe2180f39a | 323 | drugMoveDeg = DRUG_MOVE_DEG_INI_L; |
tsumagari | 67:94fe2180f39a | 324 | } |
tsumagari | 78:e272e65f5a0e | 325 | writeConfig(); |
taurin | 12:fd9d241843f4 | 326 | } |
taurin | 0:e052602db102 | 327 | SERVO_FLAG = servoInit(); |
taurin | 0:e052602db102 | 328 | INA_FLAG = inaInit(); |
taurin | 4:450cafd95ac3 | 329 | sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); |
taurin | 16:82310bf7c326 | 330 | // toStringTicker.attach(&toString,0.5); |
taurin | 16:82310bf7c326 | 331 | receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); |
taurin | 0:e052602db102 | 332 | } |
taurin | 0:e052602db102 | 333 | |
YusukeWakuta | 26:f14579683f98 | 334 | void updateDatas() |
YusukeWakuta | 26:f14579683f98 | 335 | { |
YusukeWakuta | 26:f14579683f98 | 336 | if(INA_FLAG) { |
taurin | 0:e052602db102 | 337 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 338 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 339 | } |
tsumagari | 73:05feda5b0f98 | 340 | uint8_t r[4]; |
tsumagari | 73:05feda5b0f98 | 341 | uint8_t y[4]; |
tsumagari | 73:05feda5b0f98 | 342 | int rl,yw; |
tsumagari | 73:05feda5b0f98 | 343 | rl = (int)(roll*10000); |
tsumagari | 73:05feda5b0f98 | 344 | yw = (int)(yaw*10000); |
tsumagari | 73:05feda5b0f98 | 345 | r[2] = (rl&0x00ff0000)>>16; y[2] = (yw&0x00ff0000)>>16; |
tsumagari | 73:05feda5b0f98 | 346 | r[1] = (rl&0x0000ff00)>>8; y[1] = (yw&0x0000ff00)>>8; |
tsumagari | 73:05feda5b0f98 | 347 | r[0] = (rl&0x000000ff); y[0] = (yw&0x000000ff); |
tsumagari | 73:05feda5b0f98 | 348 | r[3] = (r[2]>>7 == 0)?0:0xff;y[3] = (y[2]>>7 == 0)?0:0xff;//r[3]can make from r[2] |
tsumagari | 73:05feda5b0f98 | 349 | int i = 0; |
tsumagari | 73:05feda5b0f98 | 350 | for(;i<3;i++) |
tsumagari | 73:05feda5b0f98 | 351 | toSendDatas[i] = r[i]; |
tsumagari | 73:05feda5b0f98 | 352 | for(;i<6;i++) |
tsumagari | 73:05feda5b0f98 | 353 | toSendDatas[i] = y[i-3]; |
tsumagari | 73:05feda5b0f98 | 354 | toSendDatas[i] = (char)V; |
tsumagari | 73:05feda5b0f98 | 355 | print2pc(SENDING_DATA_DEBUG_FLAG,"p:%12f,r:%12f,y:%12f,servoV%12f r[]:%d,y[]:%d\n\r" |
tsumagari | 73:05feda5b0f98 | 356 | ,pitch,roll,yaw,V,*(int*)r,*(int*)y); |
taurin | 0:e052602db102 | 357 | } |
taurin | 0:e052602db102 | 358 | |
YusukeWakuta | 26:f14579683f98 | 359 | void receiveDatas() |
YusukeWakuta | 26:f14579683f98 | 360 | { |
tsumagari | 73:05feda5b0f98 | 361 | if(can.read(recmsg)) { //ここの中でpc.printfすると固まるので注意 |
tsumagari | 74:8ccd04302a7f | 362 | for(int i = 0; i < ERURON_DATAS_NUM; i++) { |
tsumagari | 74:8ccd04302a7f | 363 | intValues[i] = recmsg.data[i]; |
taurin | 0:e052602db102 | 364 | } |
tsumagari | 74:8ccd04302a7f | 365 | drugInput = (recmsg.data[3]-'0')/2; |
tsumagari | 74:8ccd04302a7f | 366 | servoOffVer = ((recmsg.data[3]-'0')%2); |
tsumagari | 74:8ccd04302a7f | 367 | eruronfloat = atoi(intValues)/100.0; |
taurin | 1:9cc932a16d17 | 368 | led1 = !led1; |
taurin | 0:e052602db102 | 369 | } |
tsumagari | 74:8ccd04302a7f | 370 | servoOff = servoOffVer; |
taurin | 0:e052602db102 | 371 | } |
taurin | 0:e052602db102 | 372 | |
tsumagari | 78:e272e65f5a0e | 373 | //double calcPulse(float analog) |
tsumagari | 78:e272e65f5a0e | 374 | //{ |
tsumagari | 78:e272e65f5a0e | 375 | // return (0.0006 + (analog)*(0.00240-0.00060) ); |
tsumagari | 78:e272e65f5a0e | 376 | //} |
tsumagari | 78:e272e65f5a0e | 377 | int calcPulse_us(float analog) |
YusukeWakuta | 26:f14579683f98 | 378 | { |
tsumagari | 78:e272e65f5a0e | 379 | return (1000 + 1000*analog); |
tsumagari | 78:e272e65f5a0e | 380 | } |
tsumagari | 78:e272e65f5a0e | 381 | |
tsumagari | 78:e272e65f5a0e | 382 | float getPinDeg() |
tsumagari | 78:e272e65f5a0e | 383 | { |
tsumagari | 78:e272e65f5a0e | 384 | float setPin10 = -setDeg10.read() + 1.0; |
tsumagari | 78:e272e65f5a0e | 385 | return (int)(setPin10*9)/10.0 + setDeg1.read()/10; |
taurin | 1:9cc932a16d17 | 386 | } |
taurin | 1:9cc932a16d17 | 387 | |
YusukeWakuta | 29:516a5d383488 | 388 | void WriteServo() |
YusukeWakuta | 26:f14579683f98 | 389 | { |
YusukeWakuta | 62:1db967d29809 | 390 | // for(int i = 0; i< 10; i++) { |
YusukeWakuta | 61:988e3f4280ac | 391 | // pc.printf("%c",floatValues[i]); |
YusukeWakuta | 61:988e3f4280ac | 392 | // } |
YusukeWakuta | 61:988e3f4280ac | 393 | // pc.printf(" : %f",eruronfloat); |
YusukeWakuta | 61:988e3f4280ac | 394 | // pc.printf("\n\r"); |
tsumagari | 78:e272e65f5a0e | 395 | eruronServo.pulsewidth_us(calcPulse_us( eruronTrim + eruronMoveDeg * eruronfloat)); |
tsumagari | 78:e272e65f5a0e | 396 | if(DRUG_OPEN_FLAG)drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg)); |
tsumagari | 78:e272e65f5a0e | 397 | else drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg * drugInput)); |
tsumagari | 74:8ccd04302a7f | 398 | print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff)); |
tsumagari | 77:ca4ab599ba2b | 399 | print2pc(SERVO_CONFIG_DEBUG_FLAG,"eTr:%f dTr:%f eMD:%f dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg); |
tsumagari | 74:8ccd04302a7f | 400 | print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer); |
taurin | 4:450cafd95ac3 | 401 | } |
taurin | 4:450cafd95ac3 | 402 | |
tsumagari | 78:e272e65f5a0e | 403 | void setTrim(PwmOut servo, float* servoTr, float initServoTrim, float initPinState) |
YusukeWakuta | 26:f14579683f98 | 404 | { |
tsumagari | 77:ca4ab599ba2b | 405 | led2 = 1; |
tsumagari | 77:ca4ab599ba2b | 406 | led4 = 0; |
tsumagari | 78:e272e65f5a0e | 407 | if(*servoTr<0)led2 = 0; |
tsumagari | 78:e272e65f5a0e | 408 | if(*servoTr>1.1) led1 = 1; |
tsumagari | 78:e272e65f5a0e | 409 | if(int(*servoTr * 10) == 0) led4 = 1; |
tsumagari | 78:e272e65f5a0e | 410 | // if(EDstatePin) { |
tsumagari | 78:e272e65f5a0e | 411 | // eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState; |
tsumagari | 78:e272e65f5a0e | 412 | // eruronServo.pulsewidth_us(calcPusle_us(eruronTrim)); |
tsumagari | 78:e272e65f5a0e | 413 | // } else { |
tsumagari | 78:e272e65f5a0e | 414 | // drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState; |
tsumagari | 78:e272e65f5a0e | 415 | // drugServo.pulsewidth_us(calcPulse_us(drugTrim)); |
tsumagari | 78:e272e65f5a0e | 416 | // } |
tsumagari | 78:e272e65f5a0e | 417 | *servoTr = getPinDeg() - initPinState + initServoTrim; |
tsumagari | 78:e272e65f5a0e | 418 | servo.pulsewidth_us(calcPulse_us(*servoTr)); |
tsumagari | 78:e272e65f5a0e | 419 | |
tsumagari | 77:ca4ab599ba2b | 420 | pc.printf("eTr:%f dTr:%f ",eruronTrim,drugTrim); |
tsumagari | 73:05feda5b0f98 | 421 | pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
taurin | 4:450cafd95ac3 | 422 | } |
taurin | 4:450cafd95ac3 | 423 | |
tsumagari | 78:e272e65f5a0e | 424 | void setMaxDeg(PwmOut servo, float* servoMD, float initServoTr, float initServoMD, float initPinState) |
YusukeWakuta | 26:f14579683f98 | 425 | { |
taurin | 4:450cafd95ac3 | 426 | led4 = 1; |
tsumagari | 77:ca4ab599ba2b | 427 | led2 = 0; |
tsumagari | 78:e272e65f5a0e | 428 | // float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initEruronState; |
tsumagari | 78:e272e65f5a0e | 429 | // float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initDrugState; |
tsumagari | 78:e272e65f5a0e | 430 | float temp = getPinDeg() - initPinState + initServoTr + initServoMD; |
tsumagari | 78:e272e65f5a0e | 431 | |
tsumagari | 78:e272e65f5a0e | 432 | // if(EDstatePin) { |
tsumagari | 78:e272e65f5a0e | 433 | // eruronMoveDeg = eruronTemp-eruronTrim; |
tsumagari | 78:e272e65f5a0e | 434 | // if(eruronTemp < 0)led4 = 0; |
tsumagari | 78:e272e65f5a0e | 435 | // else eruronServo.pulsewidth_us(calcPusle_us(eruronTemp)); |
tsumagari | 78:e272e65f5a0e | 436 | // } else { |
tsumagari | 78:e272e65f5a0e | 437 | // drugMoveDeg = drugTemp-drugTrim; |
tsumagari | 78:e272e65f5a0e | 438 | // if(drugTemp < 0)led4 = 0; |
tsumagari | 78:e272e65f5a0e | 439 | // else drugServo.pulsewidth_us(calcPusle_us(drugTemp)); |
tsumagari | 78:e272e65f5a0e | 440 | // } |
tsumagari | 78:e272e65f5a0e | 441 | *servoMD = temp-initServoTr; |
tsumagari | 78:e272e65f5a0e | 442 | if(temp<0)led4 = 0; |
tsumagari | 78:e272e65f5a0e | 443 | if(temp>1.1) led1 = 1; |
tsumagari | 78:e272e65f5a0e | 444 | if(int(temp * 10) == 0) led2 = 1; |
tsumagari | 78:e272e65f5a0e | 445 | else servo.pulsewidth_us(calcPulse_us(temp)); |
tsumagari | 78:e272e65f5a0e | 446 | |
tsumagari | 77:ca4ab599ba2b | 447 | pc.printf("eTr:%f dTr:%f ",eruronTrim,drugTrim); |
tsumagari | 73:05feda5b0f98 | 448 | pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
tsumagari | 78:e272e65f5a0e | 449 | |
taurin | 4:450cafd95ac3 | 450 | wait_us(10); |
taurin | 1:9cc932a16d17 | 451 | } |
taurin | 0:e052602db102 | 452 | |
YusukeWakuta | 26:f14579683f98 | 453 | int main() |
YusukeWakuta | 26:f14579683f98 | 454 | { |
taurin | 0:e052602db102 | 455 | init(); |
YusukeWakuta | 42:bf98a29e81ac | 456 | Thread mpu_thread(&mpuProcessing); |
YusukeWakuta | 26:f14579683f98 | 457 | |
tsumagari | 74:8ccd04302a7f | 458 | // start motion |
tsumagari | 33:d075918d4846 | 459 | // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 26:f14579683f98 | 460 | |
YusukeWakuta | 26:f14579683f98 | 461 | while(1) { |
tsumagari | 68:ae3c3500074c | 462 | if(InSetModePin){ |
tsumagari | 78:e272e65f5a0e | 463 | float initEruronTrim = eruronTrim; |
tsumagari | 78:e272e65f5a0e | 464 | float initDrugTrim = drugTrim; |
tsumagari | 78:e272e65f5a0e | 465 | float initEruronMD = eruronMoveDeg; |
tsumagari | 78:e272e65f5a0e | 466 | float initDrugMD = drugMoveDeg; |
tsumagari | 78:e272e65f5a0e | 467 | float initPinState = getPinDeg(); |
tsumagari | 68:ae3c3500074c | 468 | do{ |
tsumagari | 78:e272e65f5a0e | 469 | if(TrimMaxDegPin){ |
tsumagari | 78:e272e65f5a0e | 470 | if(EDstatePin) |
tsumagari | 78:e272e65f5a0e | 471 | setTrim(eruronServo, &eruronTrim, initEruronTrim, initPinState); |
tsumagari | 78:e272e65f5a0e | 472 | else |
tsumagari | 78:e272e65f5a0e | 473 | setTrim(drugServo, &drugTrim, initDrugTrim, initPinState); |
tsumagari | 78:e272e65f5a0e | 474 | } |
tsumagari | 78:e272e65f5a0e | 475 | else{ |
tsumagari | 78:e272e65f5a0e | 476 | if(EDstatePin) |
tsumagari | 78:e272e65f5a0e | 477 | setMaxDeg(eruronServo, &eruronMoveDeg, initEruronTrim, initEruronMD, initPinState); |
tsumagari | 78:e272e65f5a0e | 478 | else |
tsumagari | 78:e272e65f5a0e | 479 | setMaxDeg(drugServo, &drugMoveDeg, initDrugTrim, initDrugMD, initPinState); |
tsumagari | 78:e272e65f5a0e | 480 | } |
tsumagari | 78:e272e65f5a0e | 481 | led1 = 0; |
tsumagari | 68:ae3c3500074c | 482 | }while(InSetModePin); |
tsumagari | 67:94fe2180f39a | 483 | writeConfig(); |
taurin | 4:450cafd95ac3 | 484 | } |
taurin | 4:450cafd95ac3 | 485 | led4 = 0; |
YusukeWakuta | 44:624a4469ae21 | 486 | led2 = 0; |
tsumagari | 68:ae3c3500074c | 487 | receiveDatas(); |
tsumagari | 71:a00561d455d1 | 488 | sendDatas(); |
tsumagari | 78:e272e65f5a0e | 489 | receiveFromPc(); |
taurin | 1:9cc932a16d17 | 490 | WriteServo(); |
taurin | 4:450cafd95ac3 | 491 | updateDatas(); |
tsumagari | 22:b38bc18ec3a1 | 492 | led3 = !led3; |
taurin | 2:7fcb4f970a02 | 493 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 494 | } |
taurin | 0:e052602db102 | 495 | } |