albatross / Mbed 2 deprecated ControlYokutan2018

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@73:05feda5b0f98, 2017-12-16 (annotated)

Committer:
tsumagari
Date:
Sat Dec 16 12:06:41 2017 +0000
Branch:
mpu????????
Revision:
73:05feda5b0f98
Parent:
72:aef1ec8c66c7
Child:
74:8ccd04302a7f
can?????ver(roll:3B,yaw:3B,servoV:1B),print2pc?????

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 73:05feda5b0f98 16 ******************************************
tsumagari 73:05feda5b0f98 17 */
taurin 0:e052602db102 18 #include "mbed.h"
tsumagari 22:b38bc18ec3a1 19 #include "MPU6050.h"
taurin 0:e052602db102 20 #include "INA226.hpp"
YusukeWakuta 42:bf98a29e81ac 21 #include "rtos.h"
YusukeWakuta 24:d416722b4aad 22 #include "XBusServo.h"
YusukeWakuta 57:d7b709dd1c4f 23 #include "math.h"
taurin 4:450cafd95ac3 24
tsumagari 22:b38bc18ec3a1 25 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 26 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 27 #define WAIT_LOOP_TIME 0.02
YusukeWakuta 47:5e3a836deaba 28 #define CONTROL_VALUES_NUM 7
tsumagari 73:05feda5b0f98 29 #define TO_SEND_CAN_ID 0x100 //0x0>>0x7ff
taurin 4:450cafd95ac3 30 #define SEND_DATAS_LOOP_TIME 0.1
tsumagari 33:d075918d4846 31 #define RECEIVE_DATAS_LOOP_TIME 0.1
taurin 12:fd9d241843f4 32
YusukeWakuta 42:bf98a29e81ac 33 #define MPU_LOOP_TIME 0.01
YusukeWakuta 42:bf98a29e81ac 34 #define MPU_DELT_MIN 250
YusukeWakuta 42:bf98a29e81ac 35
tsumagari 67:94fe2180f39a 36 #define ERURON_MOVE_DEG_INI_R 14.4*0.8//18.0 //degree
tsumagari 67:94fe2180f39a 37 #define DRUG_MOVE_DEG_INI_R 0.49
tsumagari 66:2d1a7f270e27 38 #define ERURON_TRIM_INI_R 0.39633 //値lowerすると頭上げ 0.37で後縁一致
YusukeWakuta 58:f84bd22fd586 39 #define DRUG_TRIM_INI_R 0.37
taurin 12:fd9d241843f4 40
tsumagari 64:9d7582920f50 41 #define ERURON_MOVE_DEG_INI_L -15.52//-19.4 //degree
YusukeWakuta 63:52b882a5968c 42 #define DRUG_MOVE_DEG_INI_L -0.52
tsumagari 67:94fe2180f39a 43 #define ERURON_TRIM_INI_L 0.41567 // 値を大きくすると頭上げ
YusukeWakuta 58:f84bd22fd586 44 #define DRUG_TRIM_INI_L 0.73//値を小さくすると開く側
taurin 2:7fcb4f970a02 45
tsumagari 73:05feda5b0f98 46 #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0)
tsumagari 73:05feda5b0f98 47 #define SENDING_DATA_DEBUG_FLAG debugflag[0].flag
tsumagari 73:05feda5b0f98 48 #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag
tsumagari 73:05feda5b0f98 49 #define SERVO_DONFIG_DEBUG_FLAG debugflag[2].flag
tsumagari 67:94fe2180f39a 50
tsumagari 67:94fe2180f39a 51 const char *configfilename = "/local/CONFIG.csv";
tsumagari 73:05feda5b0f98 52 struct flaglist{
tsumagari 73:05feda5b0f98 53 char key;
tsumagari 73:05feda5b0f98 54 bool flag;
tsumagari 73:05feda5b0f98 55 };
tsumagari 67:94fe2180f39a 56
YusukeWakuta 38:b492990e2b56 57 /*ドラッグラダー
YusukeWakuta 37:1f71ca1e5dd1 58 初期値 0.65
YusukeWakuta 37:1f71ca1e5dd1 59 最大角0.99
YusukeWakuta 36:ddf4aa818e88 60 */
YusukeWakuta 24:d416722b4aad 61
taurin 0:e052602db102 62 CAN can(p30,p29);
taurin 0:e052602db102 63 CANMessage recmsg;
taurin 0:e052602db102 64 Serial pc(USBTX,USBRX);
taurin 0:e052602db102 65 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 66 INA226 VCmonitor(ina226_i2c);
tsumagari 70:7da315abec17 67 PwmOut drugServo(p23);
tsumagari 71:a00561d455d1 68 PwmOut eruronServo(p25);
taurin 1:9cc932a16d17 69 DigitalOut led1(LED1);
tsumagari 69:073116d31a56 70 AnalogIn setDeg10(p20);
tsumagari 69:073116d31a56 71 AnalogIn setDeg1(p19);
tsumagari 71:a00561d455d1 72 DigitalIn IsRPin(p13,PullDown);
tsumagari 71:a00561d455d1 73 DigitalIn InSetModePin(p14,PullDown);
tsumagari 71:a00561d455d1 74 DigitalIn EDstatePin(p15,PullDown);
tsumagari 71:a00561d455d1 75 DigitalIn TrimMaxDegPin(p16,PullDown);
tsumagari 71:a00561d455d1 76 DigitalOut servoOff(p17);
YusukeWakuta 44:624a4469ae21 77 DigitalOut led2(LED2);
taurin 4:450cafd95ac3 78 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 79 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 80 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 81 //Ticker toStringTicker;
taurin 16:82310bf7c326 82 Ticker receiveDatasTicker;
YusukeWakuta 42:bf98a29e81ac 83 MPU6050 mpu6050;
YusukeWakuta 42:bf98a29e81ac 84 Timer t;
YusukeWakuta 42:bf98a29e81ac 85
tsumagari 67:94fe2180f39a 86 LocalFileSystem local("local");
tsumagari 67:94fe2180f39a 87
taurin 0:e052602db102 88 char toSendDatas[TO_SEND_DATAS_NUM];
YusukeWakuta 60:a45dc19a6001 89 char floatValues[10];
taurin 12:fd9d241843f4 90 float eruronTrim;
taurin 12:fd9d241843f4 91 float drugTrim;
taurin 12:fd9d241843f4 92 float eruronMoveDeg;
taurin 12:fd9d241843f4 93 float drugMoveDeg;
YusukeWakuta 59:f007e543f8c9 94 float eruronfloat = 0.0;
taurin 0:e052602db102 95 unsigned short ina_val;
taurin 0:e052602db102 96 double V,C;
taurin 0:e052602db102 97 bool SERVO_FLAG;
taurin 0:e052602db102 98 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 99 bool MPU_FLAG;
tsumagari 73:05feda5b0f98 100 struct flaglist debugflag[]={
tsumagari 73:05feda5b0f98 101 {'s',0},
tsumagari 73:05feda5b0f98 102 {'g',0},
tsumagari 73:05feda5b0f98 103 {'c',0},
tsumagari 73:05feda5b0f98 104 {'0',0}
tsumagari 73:05feda5b0f98 105 };
YusukeWakuta 42:bf98a29e81ac 106 int gyroX;
YusukeWakuta 42:bf98a29e81ac 107 int gyroY;
YusukeWakuta 42:bf98a29e81ac 108 int gyroZ;
YusukeWakuta 42:bf98a29e81ac 109 float sum = 0;
YusukeWakuta 61:988e3f4280ac 110 float drugInput = 0.0;
YusukeWakuta 42:bf98a29e81ac 111 uint32_t sumCount = 0;
taurin 0:e052602db102 112
tsumagari 22:b38bc18ec3a1 113 char gyro_c[6] = {0,0,0,0,0,0};
taurin 4:450cafd95ac3 114
taurin 4:450cafd95ac3 115 void toString();
taurin 16:82310bf7c326 116 void receiveDatas();
taurin 16:82310bf7c326 117 void WriteServo();
YusukeWakuta 42:bf98a29e81ac 118 void MpuInit();
YusukeWakuta 42:bf98a29e81ac 119 void mpuProcessing(void const *arg);
taurin 0:e052602db102 120
YusukeWakuta 24:d416722b4aad 121 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 122
tsumagari 73:05feda5b0f98 123
tsumagari 73:05feda5b0f98 124
tsumagari 73:05feda5b0f98 125 void receiveFromPc(){
tsumagari 73:05feda5b0f98 126 for(;pc.readable();){
tsumagari 73:05feda5b0f98 127 char c = pc.getc();
tsumagari 73:05feda5b0f98 128 for(int i = 0; debugflag[i].key != '0'; i++){
tsumagari 73:05feda5b0f98 129 if(debugflag[i].key == c)
tsumagari 73:05feda5b0f98 130 debugflag[i].flag = !(debugflag[i].flag);
tsumagari 73:05feda5b0f98 131 }
tsumagari 73:05feda5b0f98 132 }
tsumagari 73:05feda5b0f98 133 }
tsumagari 73:05feda5b0f98 134
YusukeWakuta 26:f14579683f98 135 bool servoInit()
YusukeWakuta 26:f14579683f98 136 {
taurin 4:450cafd95ac3 137 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 138 return true;
taurin 0:e052602db102 139 }
taurin 0:e052602db102 140
YusukeWakuta 26:f14579683f98 141 void sendDatas()
YusukeWakuta 24:d416722b4aad 142 {
YusukeWakuta 26:f14579683f98 143 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
taurin 4:450cafd95ac3 144 }
taurin 4:450cafd95ac3 145 }
taurin 4:450cafd95ac3 146
YusukeWakuta 42:bf98a29e81ac 147 void MpuInit()
YusukeWakuta 42:bf98a29e81ac 148 {
tsumagari 72:aef1ec8c66c7 149 i2c.frequency(400000); // use fast (400 kHz) I2C
YusukeWakuta 42:bf98a29e81ac 150 t.start();
YusukeWakuta 42:bf98a29e81ac 151 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
YusukeWakuta 42:bf98a29e81ac 152 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
YusukeWakuta 42:bf98a29e81ac 153 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 154 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
YusukeWakuta 42:bf98a29e81ac 155 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 156 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 157 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
YusukeWakuta 42:bf98a29e81ac 158 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
YusukeWakuta 42:bf98a29e81ac 159 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 160 Thread::wait(200);
YusukeWakuta 42:bf98a29e81ac 161 } else {
YusukeWakuta 42:bf98a29e81ac 162 }
YusukeWakuta 42:bf98a29e81ac 163 } else {
YusukeWakuta 42:bf98a29e81ac 164 //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
YusukeWakuta 42:bf98a29e81ac 165 }
YusukeWakuta 42:bf98a29e81ac 166 }
YusukeWakuta 42:bf98a29e81ac 167
YusukeWakuta 42:bf98a29e81ac 168
YusukeWakuta 42:bf98a29e81ac 169 void mpuProcessing(void const *arg)
YusukeWakuta 42:bf98a29e81ac 170 {
YusukeWakuta 42:bf98a29e81ac 171 MpuInit();
YusukeWakuta 42:bf98a29e81ac 172 while(1) {
YusukeWakuta 42:bf98a29e81ac 173 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
YusukeWakuta 42:bf98a29e81ac 174 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 175 mpu6050.getAres();
YusukeWakuta 42:bf98a29e81ac 176 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 177 ay = (float)accelCount[1]*aRes - accelBias[1];
YusukeWakuta 42:bf98a29e81ac 178 az = (float)accelCount[2]*aRes - accelBias[2];
YusukeWakuta 42:bf98a29e81ac 179 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 180 mpu6050.getGres();
YusukeWakuta 42:bf98a29e81ac 181 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 182 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
YusukeWakuta 42:bf98a29e81ac 183 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
YusukeWakuta 42:bf98a29e81ac 184 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 185 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
YusukeWakuta 42:bf98a29e81ac 186 }
YusukeWakuta 42:bf98a29e81ac 187 Now = t.read_us();
YusukeWakuta 42:bf98a29e81ac 188 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
YusukeWakuta 42:bf98a29e81ac 189 lastUpdate = Now;
YusukeWakuta 42:bf98a29e81ac 190 sum += deltat;
YusukeWakuta 42:bf98a29e81ac 191 sumCount++;
YusukeWakuta 42:bf98a29e81ac 192 if(lastUpdate - firstUpdate > 10000000.0f) {
YusukeWakuta 42:bf98a29e81ac 193 beta = 0.04; // decrease filter gain after stabilized
YusukeWakuta 42:bf98a29e81ac 194 zeta = 0.015; // increasey bias drift gain after stabilized
YusukeWakuta 42:bf98a29e81ac 195 }
YusukeWakuta 42:bf98a29e81ac 196 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
YusukeWakuta 42:bf98a29e81ac 197 delt_t = t.read_ms() - count;
YusukeWakuta 42:bf98a29e81ac 198 if (delt_t > MPU_DELT_MIN) {
YusukeWakuta 42:bf98a29e81ac 199 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 200 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
YusukeWakuta 42:bf98a29e81ac 201 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 202 pitch *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 203 yaw *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 204 roll *= 180.0f / PI;
YusukeWakuta 48:0bd406fa4a7f 205 // myled= !myled;
YusukeWakuta 42:bf98a29e81ac 206 count = t.read_ms();
YusukeWakuta 42:bf98a29e81ac 207 sum = 0;
YusukeWakuta 42:bf98a29e81ac 208 sumCount = 0;
YusukeWakuta 42:bf98a29e81ac 209 }
tsumagari 64:9d7582920f50 210 gyro_c[0]=(char)((int)pitch);
tsumagari 64:9d7582920f50 211 gyro_c[1]=(char)(int)((pitch - (int)pitch)*100);
tsumagari 64:9d7582920f50 212 gyro_c[2]=(char)((int)roll);
tsumagari 64:9d7582920f50 213 gyro_c[3]=(char)(int)((roll - (int)roll)*100);
tsumagari 64:9d7582920f50 214 gyro_c[4]=(char)((int)yaw);
tsumagari 64:9d7582920f50 215 gyro_c[5]=(char)(int)((yaw - (int)yaw)*100);
tsumagari 73:05feda5b0f98 216
YusukeWakuta 42:bf98a29e81ac 217 Thread::wait(1);
YusukeWakuta 42:bf98a29e81ac 218 }//while(1)
YusukeWakuta 42:bf98a29e81ac 219 }
YusukeWakuta 42:bf98a29e81ac 220
YusukeWakuta 42:bf98a29e81ac 221
YusukeWakuta 26:f14579683f98 222 bool inaInit()
YusukeWakuta 26:f14579683f98 223 {
YusukeWakuta 26:f14579683f98 224 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 225 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 226 return false;
taurin 0:e052602db102 227 }
taurin 0:e052602db102 228 ina_val = 0;
YusukeWakuta 26:f14579683f98 229 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 230 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 231 return false;
taurin 0:e052602db102 232 }
taurin 0:e052602db102 233 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 234 return true;
taurin 0:e052602db102 235 }
taurin 0:e052602db102 236
YusukeWakuta 57:d7b709dd1c4f 237 //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
YusukeWakuta 57:d7b709dd1c4f 238 double ConvertDeg(double servo)
YusukeWakuta 57:d7b709dd1c4f 239 {
YusukeWakuta 62:1db967d29809 240 return 0.0011 * pow(servo,3) + 0.017 * pow(servo,2) + 2.3019 * servo - 0.2269;
YusukeWakuta 62:1db967d29809 241 //return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
YusukeWakuta 57:d7b709dd1c4f 242 }
YusukeWakuta 57:d7b709dd1c4f 243
YusukeWakuta 57:d7b709dd1c4f 244 //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
YusukeWakuta 57:d7b709dd1c4f 245 double GetValueByHorn(double deg)
YusukeWakuta 57:d7b709dd1c4f 246 {
YusukeWakuta 57:d7b709dd1c4f 247 return deg*(0.10/25.7);
YusukeWakuta 57:d7b709dd1c4f 248 }
YusukeWakuta 57:d7b709dd1c4f 249
YusukeWakuta 57:d7b709dd1c4f 250 double GetFloatByErebon(double erebonDeg)
YusukeWakuta 57:d7b709dd1c4f 251 {
YusukeWakuta 57:d7b709dd1c4f 252 double servoDeg = ConvertDeg(erebonDeg);
YusukeWakuta 61:988e3f4280ac 253 return GetValueByHorn(servoDeg);
YusukeWakuta 57:d7b709dd1c4f 254 }
tsumagari 67:94fe2180f39a 255 // configファイル作成
tsumagari 67:94fe2180f39a 256 int makeConfigFile() {
tsumagari 67:94fe2180f39a 257 FILE *fp;
tsumagari 67:94fe2180f39a 258 if((fp = fopen(configfilename, "w")) == NULL) {
tsumagari 67:94fe2180f39a 259 pc.printf("can't open %s\n", configfilename);
tsumagari 67:94fe2180f39a 260 return -1;
tsumagari 67:94fe2180f39a 261 }
tsumagari 67:94fe2180f39a 262 fprintf(fp, "// This is Yokutan %s.\n", IsRPin ? "R" : "L");
tsumagari 67:94fe2180f39a 263 fprintf(fp, "eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg\n");
tsumagari 67:94fe2180f39a 264 fclose(fp);
tsumagari 67:94fe2180f39a 265 return 0;
tsumagari 67:94fe2180f39a 266 }
tsumagari 67:94fe2180f39a 267
tsumagari 67:94fe2180f39a 268 // config内容をファイルに追記
tsumagari 67:94fe2180f39a 269 int writeConfig() {
tsumagari 67:94fe2180f39a 270 FILE *fp;
tsumagari 67:94fe2180f39a 271 if((fp = fopen(configfilename, "a")) == NULL) {
tsumagari 67:94fe2180f39a 272 pc.printf("can't open %s\n", configfilename);
tsumagari 67:94fe2180f39a 273 return -1;
tsumagari 67:94fe2180f39a 274 }
tsumagari 67:94fe2180f39a 275 fprintf(fp, "%f,%f,%f,%f\n", eruronTrim, drugTrim, eruronMoveDeg, drugMoveDeg);
tsumagari 67:94fe2180f39a 276 fclose(fp);
tsumagari 67:94fe2180f39a 277 return 0;
tsumagari 67:94fe2180f39a 278 }
tsumagari 67:94fe2180f39a 279 // configの最新情報を読み込み
tsumagari 67:94fe2180f39a 280 int readConfig() {
tsumagari 67:94fe2180f39a 281 FILE *fp;
tsumagari 67:94fe2180f39a 282 char s[256];
tsumagari 67:94fe2180f39a 283 if((fp = fopen(configfilename, "r")) == NULL) {
tsumagari 67:94fe2180f39a 284 return -1;
tsumagari 67:94fe2180f39a 285 }
tsumagari 67:94fe2180f39a 286 while(fgets(s, 255, fp) != NULL) {
tsumagari 67:94fe2180f39a 287 sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg);
tsumagari 67:94fe2180f39a 288 }
tsumagari 67:94fe2180f39a 289 fclose(fp);
tsumagari 73:05feda5b0f98 290 //debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
tsumagari 67:94fe2180f39a 291 return 0;
tsumagari 67:94fe2180f39a 292 }
YusukeWakuta 57:d7b709dd1c4f 293
YusukeWakuta 26:f14579683f98 294 void init()
YusukeWakuta 26:f14579683f98 295 {
tsumagari 67:94fe2180f39a 296 if(readConfig() == -1) {
tsumagari 67:94fe2180f39a 297 makeConfigFile();
tsumagari 67:94fe2180f39a 298 if(IsRPin) {
tsumagari 67:94fe2180f39a 299 eruronTrim = ERURON_TRIM_INI_R;
tsumagari 67:94fe2180f39a 300 drugTrim = DRUG_TRIM_INI_R;
tsumagari 67:94fe2180f39a 301 eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
tsumagari 67:94fe2180f39a 302 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
tsumagari 67:94fe2180f39a 303 } else {
tsumagari 67:94fe2180f39a 304 eruronTrim = ERURON_TRIM_INI_L;
tsumagari 67:94fe2180f39a 305 drugTrim = DRUG_TRIM_INI_L;
tsumagari 67:94fe2180f39a 306 eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
tsumagari 67:94fe2180f39a 307 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
tsumagari 67:94fe2180f39a 308 }
taurin 12:fd9d241843f4 309 }
taurin 0:e052602db102 310 SERVO_FLAG = servoInit();
taurin 0:e052602db102 311 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 312 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 313 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 314 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
taurin 0:e052602db102 315 }
taurin 0:e052602db102 316
YusukeWakuta 26:f14579683f98 317 void updateDatas()
YusukeWakuta 26:f14579683f98 318 {
YusukeWakuta 26:f14579683f98 319 if(INA_FLAG) {
taurin 0:e052602db102 320 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 321 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 322 }
tsumagari 73:05feda5b0f98 323 uint8_t r[4];
tsumagari 73:05feda5b0f98 324 uint8_t y[4];
tsumagari 73:05feda5b0f98 325 int rl,yw;
tsumagari 73:05feda5b0f98 326 rl = (int)(roll*10000);
tsumagari 73:05feda5b0f98 327 yw = (int)(yaw*10000);
tsumagari 73:05feda5b0f98 328 // r[3] = (rl&0xff000000)>>24; y[3] = (yw&0xff000000)>>24;
tsumagari 73:05feda5b0f98 329 r[2] = (rl&0x00ff0000)>>16; y[2] = (yw&0x00ff0000)>>16;
tsumagari 73:05feda5b0f98 330 r[1] = (rl&0x0000ff00)>>8; y[1] = (yw&0x0000ff00)>>8;
tsumagari 73:05feda5b0f98 331 r[0] = (rl&0x000000ff); y[0] = (yw&0x000000ff);
tsumagari 73:05feda5b0f98 332 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 333 int i = 0;
tsumagari 73:05feda5b0f98 334 for(;i<3;i++)
tsumagari 73:05feda5b0f98 335 toSendDatas[i] = r[i];
tsumagari 73:05feda5b0f98 336 for(;i<6;i++)
tsumagari 73:05feda5b0f98 337 toSendDatas[i] = y[i-3];
tsumagari 73:05feda5b0f98 338 toSendDatas[i] = (char)V;
tsumagari 73:05feda5b0f98 339 print2pc(SENDING_DATA_DEBUG_FLAG,"p:%12f,r:%12f,y:%12f,servoV%12f r[]:%d,y[]:%d\n\r"
tsumagari 73:05feda5b0f98 340 ,pitch,roll,yaw,V,*(int*)r,*(int*)y);
taurin 0:e052602db102 341 }
taurin 0:e052602db102 342
YusukeWakuta 26:f14579683f98 343 void receiveDatas()
YusukeWakuta 26:f14579683f98 344 {
tsumagari 73:05feda5b0f98 345 if(can.read(recmsg)) { //ここの中でpc.printfすると固まるので注意
tsumagari 71:a00561d455d1 346 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 60:a45dc19a6001 347 floatValues[i] = recmsg.data[i];
taurin 0:e052602db102 348 }
YusukeWakuta 61:988e3f4280ac 349 drugInput = recmsg.data[5]- '0';
tsumagari 71:a00561d455d1 350 servoOff = recmsg.data[6]-'0';
YusukeWakuta 60:a45dc19a6001 351 eruronfloat = atof(floatValues);
taurin 1:9cc932a16d17 352 led1 = !led1;
taurin 0:e052602db102 353 }
taurin 0:e052602db102 354 }
taurin 0:e052602db102 355
YusukeWakuta 42:bf98a29e81ac 356 double calcPulse(float analog)
YusukeWakuta 26:f14579683f98 357 {
tsumagari 53:3eeaafa49707 358 return (0.0006 + (analog)*(0.00240-0.00060) );
taurin 1:9cc932a16d17 359 }
taurin 1:9cc932a16d17 360
YusukeWakuta 29:516a5d383488 361 void WriteServo()
YusukeWakuta 26:f14579683f98 362 {
YusukeWakuta 62:1db967d29809 363 // for(int i = 0; i< 10; i++) {
YusukeWakuta 61:988e3f4280ac 364 // pc.printf("%c",floatValues[i]);
YusukeWakuta 61:988e3f4280ac 365 // }
YusukeWakuta 61:988e3f4280ac 366 // pc.printf(" : %f",eruronfloat);
YusukeWakuta 61:988e3f4280ac 367 // pc.printf("\n\r");
YusukeWakuta 61:988e3f4280ac 368 drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
YusukeWakuta 59:f007e543f8c9 369 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
tsumagari 73:05feda5b0f98 370 print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%10f di:%5f so:%d\n\r",eruronfloat,drugInput,servoOff);
taurin 4:450cafd95ac3 371 }
taurin 4:450cafd95ac3 372
YusukeWakuta 26:f14579683f98 373 void setTrim()
YusukeWakuta 26:f14579683f98 374 {
YusukeWakuta 44:624a4469ae21 375 led2 = 1;
YusukeWakuta 26:f14579683f98 376 if(EDstatePin) {
tsumagari 69:073116d31a56 377 eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 50:b3a8f8e88c50 378 eruronServo.pulsewidth(calcPulse(eruronTrim));
YusukeWakuta 26:f14579683f98 379 } else {
tsumagari 69:073116d31a56 380 drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 26:f14579683f98 381 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 382 }
YusukeWakuta 14:1f6dd929d7de 383 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 384 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 73:05feda5b0f98 385 pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin 4:450cafd95ac3 386 }
taurin 4:450cafd95ac3 387
YusukeWakuta 26:f14579683f98 388 void setMaxDeg()
YusukeWakuta 26:f14579683f98 389 {
taurin 4:450cafd95ac3 390 led4 = 1;
tsumagari 69:073116d31a56 391 float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
tsumagari 69:073116d31a56 392 float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 26:f14579683f98 393 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 394 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 49:8522856fe0cd 395 eruronServo.pulsewidth(calcPulse(eruronTemp));
YusukeWakuta 26:f14579683f98 396 } else {
tsumagari 68:ae3c3500074c 397 drugMoveDeg = drugTemp-drugTrim;
YusukeWakuta 26:f14579683f98 398 drugServo.pulsewidth(calcPulse(drugTemp));
taurin 12:fd9d241843f4 399 }
YusukeWakuta 14:1f6dd929d7de 400 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 73:05feda5b0f98 401 pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin 4:450cafd95ac3 402 wait_us(10);
taurin 1:9cc932a16d17 403 }
taurin 0:e052602db102 404
YusukeWakuta 26:f14579683f98 405 int main()
YusukeWakuta 26:f14579683f98 406 {
taurin 0:e052602db102 407 init();
YusukeWakuta 42:bf98a29e81ac 408 Thread mpu_thread(&mpuProcessing);
YusukeWakuta 26:f14579683f98 409
YusukeWakuta 24:d416722b4aad 410 // start motion
tsumagari 33:d075918d4846 411 // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 412
YusukeWakuta 26:f14579683f98 413 while(1) {
tsumagari 68:ae3c3500074c 414 if(InSetModePin){
tsumagari 68:ae3c3500074c 415 do{
tsumagari 68:ae3c3500074c 416 if(TrimMaxDegPin)
tsumagari 68:ae3c3500074c 417 setTrim();
tsumagari 68:ae3c3500074c 418 else
tsumagari 68:ae3c3500074c 419 setMaxDeg();
tsumagari 68:ae3c3500074c 420 }while(InSetModePin);
tsumagari 67:94fe2180f39a 421 writeConfig();
taurin 4:450cafd95ac3 422 }
taurin 4:450cafd95ac3 423 led4 = 0;
YusukeWakuta 44:624a4469ae21 424 led2 = 0;
tsumagari 68:ae3c3500074c 425 receiveDatas();
tsumagari 71:a00561d455d1 426 sendDatas();
taurin 1:9cc932a16d17 427 WriteServo();
taurin 4:450cafd95ac3 428 updateDatas();
tsumagari 73:05feda5b0f98 429 receiveFromPc();
tsumagari 22:b38bc18ec3a1 430 led3 = !led3;
taurin 2:7fcb4f970a02 431 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 432 }
taurin 0:e052602db102 433 }