albatross / Mbed 2 deprecated ControlYokutan2018

2018年度用翼端mbedプログラム

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

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?

UserRevisionLine numberNew 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 }