albatross / Mbed 2 deprecated ControlYokutan2017_2

ControlMainでの変更に対応して、新しくレポジトリを作りました

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@47:5e3a836deaba, 2017-03-23 (annotated)

Committer:
YusukeWakuta
Date:
Thu Mar 23 08:50:40 2017 +0000
Branch:
mpu????????
Revision:
47:5e3a836deaba
Parent:
46:c722841055c7
Child:
48:0bd406fa4a7f
LR??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 0:e052602db102 1 //翼端can program
taurin 0:e052602db102 2 #include "mbed.h"
tsumagari 22:b38bc18ec3a1 3 #include "MPU6050.h"
taurin 0:e052602db102 4 #include "INA226.hpp"
YusukeWakuta 42:bf98a29e81ac 5 #include "rtos.h"
YusukeWakuta 24:d416722b4aad 6 #include "XBusServo.h"
taurin 4:450cafd95ac3 7
tsumagari 22:b38bc18ec3a1 8 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 9 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 10 #define WAIT_LOOP_TIME 0.02
YusukeWakuta 47:5e3a836deaba 11 #define CONTROL_VALUES_NUM 7
taurin 0:e052602db102 12 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 13 #define SEND_DATAS_LOOP_TIME 0.1
tsumagari 33:d075918d4846 14 #define RECEIVE_DATAS_LOOP_TIME 0.1
taurin 12:fd9d241843f4 15
YusukeWakuta 42:bf98a29e81ac 16 #define MPU_LOOP_TIME 0.01
YusukeWakuta 42:bf98a29e81ac 17 #define MPU_DELT_MIN 250
YusukeWakuta 42:bf98a29e81ac 18
YusukeWakuta 40:ad98da5da7bf 19 #define ERURON_MOVE_DEG_INI_R 1.0
YusukeWakuta 47:5e3a836deaba 20 #define DRUG_MOVE_DEG_INI_R -0.32
YusukeWakuta 44:624a4469ae21 21 #define ERURON_TRIM_INI_R 0.35
YusukeWakuta 47:5e3a836deaba 22 #define DRUG_TRIM_INI_R 0.62
taurin 12:fd9d241843f4 23
YusukeWakuta 43:9a57cec43257 24 #define ERURON_MOVE_DEG_INI_L 0.8
YusukeWakuta 47:5e3a836deaba 25 #define DRUG_MOVE_DEG_INI_L -0.43
YusukeWakuta 44:624a4469ae21 26 #define ERURON_TRIM_INI_L 0.4
YusukeWakuta 47:5e3a836deaba 27 #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側
taurin 2:7fcb4f970a02 28
YusukeWakuta 43:9a57cec43257 29 #define ERURON_MOVE_RANGE 0.24
YusukeWakuta 43:9a57cec43257 30
YusukeWakuta 44:624a4469ae21 31 #define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
YusukeWakuta 42:bf98a29e81ac 32
YusukeWakuta 38:b492990e2b56 33 /*ドラッグラダー
YusukeWakuta 37:1f71ca1e5dd1 34 初期値 0.65
YusukeWakuta 37:1f71ca1e5dd1 35 最大角0.99
YusukeWakuta 36:ddf4aa818e88 36 */
YusukeWakuta 24:d416722b4aad 37
taurin 0:e052602db102 38 CAN can(p30,p29);
taurin 0:e052602db102 39 CANMessage recmsg;
taurin 0:e052602db102 40 Serial pc(USBTX,USBRX);
taurin 0:e052602db102 41 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 42 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 43 PwmOut drugServo(p22);
tsumagari 33:d075918d4846 44 PwmOut eruronServo(p23);
taurin 1:9cc932a16d17 45 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 46 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 47 AnalogIn eruronAna(p19);
YusukeWakuta 28:99686a3f0e86 48 DigitalIn LRstatePin(p11);
YusukeWakuta 28:99686a3f0e86 49 DigitalIn setTrimPin(p12);
YusukeWakuta 28:99686a3f0e86 50 DigitalIn EDstatePin(p14);
YusukeWakuta 28:99686a3f0e86 51 DigitalIn setMaxDegPin(p15);
YusukeWakuta 44:624a4469ae21 52 DigitalOut led2(LED2);
taurin 4:450cafd95ac3 53 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 54 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 55 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 56 //Ticker toStringTicker;
taurin 16:82310bf7c326 57 Ticker receiveDatasTicker;
YusukeWakuta 42:bf98a29e81ac 58 MPU6050 mpu6050;
YusukeWakuta 42:bf98a29e81ac 59 Timer t;
YusukeWakuta 42:bf98a29e81ac 60
YusukeWakuta 42:bf98a29e81ac 61 //Set up I2C, (SDA,SCL)
YusukeWakuta 42:bf98a29e81ac 62 I2C i2c_mpu(p9,p10);
taurin 16:82310bf7c326 63
taurin 0:e052602db102 64 char toSendDatas[TO_SEND_DATAS_NUM];
YusukeWakuta 45:e3df49d7b912 65 char controlValues[sizeof(int) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
YusukeWakuta 42:bf98a29e81ac 66 char intvalues[sizeof(int)];
taurin 12:fd9d241843f4 67 float eruronTrim;
taurin 12:fd9d241843f4 68 float drugTrim;
taurin 12:fd9d241843f4 69 float eruronMoveDeg;
taurin 12:fd9d241843f4 70 float drugMoveDeg;
YusukeWakuta 44:624a4469ae21 71 int eruronint = 1;
taurin 0:e052602db102 72 unsigned short ina_val;
taurin 0:e052602db102 73 double V,C;
taurin 0:e052602db102 74 bool SERVO_FLAG;
taurin 0:e052602db102 75 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 76 bool MPU_FLAG;
YusukeWakuta 31:5d22ebe5f705 77 uint16_t XbusValue;
YusukeWakuta 42:bf98a29e81ac 78 int gyroX;
YusukeWakuta 42:bf98a29e81ac 79 int gyroY;
YusukeWakuta 42:bf98a29e81ac 80 int gyroZ;
YusukeWakuta 42:bf98a29e81ac 81 float sum = 0;
YusukeWakuta 42:bf98a29e81ac 82 uint32_t sumCount = 0;
taurin 0:e052602db102 83
tsumagari 22:b38bc18ec3a1 84 char gyro_c[6] = {0,0,0,0,0,0};
taurin 4:450cafd95ac3 85
taurin 4:450cafd95ac3 86 void toString();
taurin 16:82310bf7c326 87 void receiveDatas();
taurin 16:82310bf7c326 88 void WriteServo();
YusukeWakuta 42:bf98a29e81ac 89 void MpuInit();
YusukeWakuta 42:bf98a29e81ac 90 void mpuProcessing(void const *arg);
taurin 0:e052602db102 91
YusukeWakuta 24:d416722b4aad 92 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 93
YusukeWakuta 26:f14579683f98 94 bool servoInit()
YusukeWakuta 26:f14579683f98 95 {
taurin 4:450cafd95ac3 96 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 97 return true;
taurin 0:e052602db102 98 }
taurin 0:e052602db102 99
YusukeWakuta 26:f14579683f98 100 void sendDatas()
YusukeWakuta 24:d416722b4aad 101 {
YusukeWakuta 26:f14579683f98 102 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
taurin 4:450cafd95ac3 103 }
taurin 4:450cafd95ac3 104 }
taurin 4:450cafd95ac3 105
YusukeWakuta 42:bf98a29e81ac 106 void MpuInit()
YusukeWakuta 42:bf98a29e81ac 107 {
YusukeWakuta 42:bf98a29e81ac 108 i2c_mpu.frequency(400000); // use fast (400 kHz) I2C
YusukeWakuta 42:bf98a29e81ac 109 t.start();
YusukeWakuta 42:bf98a29e81ac 110 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
YusukeWakuta 42:bf98a29e81ac 111 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
YusukeWakuta 42:bf98a29e81ac 112 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 113 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
YusukeWakuta 42:bf98a29e81ac 114 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 115 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 116 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
YusukeWakuta 42:bf98a29e81ac 117 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
YusukeWakuta 42:bf98a29e81ac 118 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 119 Thread::wait(200);
YusukeWakuta 42:bf98a29e81ac 120 } else {
YusukeWakuta 42:bf98a29e81ac 121 }
YusukeWakuta 42:bf98a29e81ac 122 } else {
YusukeWakuta 42:bf98a29e81ac 123 //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
YusukeWakuta 42:bf98a29e81ac 124 }
YusukeWakuta 42:bf98a29e81ac 125 }
YusukeWakuta 42:bf98a29e81ac 126
YusukeWakuta 42:bf98a29e81ac 127
YusukeWakuta 42:bf98a29e81ac 128 void mpuProcessing(void const *arg)
YusukeWakuta 42:bf98a29e81ac 129 {
YusukeWakuta 42:bf98a29e81ac 130 MpuInit();
YusukeWakuta 42:bf98a29e81ac 131 while(1) {
YusukeWakuta 42:bf98a29e81ac 132 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
YusukeWakuta 42:bf98a29e81ac 133 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 134 mpu6050.getAres();
YusukeWakuta 42:bf98a29e81ac 135 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 136 ay = (float)accelCount[1]*aRes - accelBias[1];
YusukeWakuta 42:bf98a29e81ac 137 az = (float)accelCount[2]*aRes - accelBias[2];
YusukeWakuta 42:bf98a29e81ac 138 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 139 mpu6050.getGres();
YusukeWakuta 42:bf98a29e81ac 140 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 141 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
YusukeWakuta 42:bf98a29e81ac 142 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
YusukeWakuta 42:bf98a29e81ac 143 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 144 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
YusukeWakuta 42:bf98a29e81ac 145 }
YusukeWakuta 42:bf98a29e81ac 146 Now = t.read_us();
YusukeWakuta 42:bf98a29e81ac 147 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
YusukeWakuta 42:bf98a29e81ac 148 lastUpdate = Now;
YusukeWakuta 42:bf98a29e81ac 149 sum += deltat;
YusukeWakuta 42:bf98a29e81ac 150 sumCount++;
YusukeWakuta 42:bf98a29e81ac 151 if(lastUpdate - firstUpdate > 10000000.0f) {
YusukeWakuta 42:bf98a29e81ac 152 beta = 0.04; // decrease filter gain after stabilized
YusukeWakuta 42:bf98a29e81ac 153 zeta = 0.015; // increasey bias drift gain after stabilized
YusukeWakuta 42:bf98a29e81ac 154 }
YusukeWakuta 42:bf98a29e81ac 155 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
YusukeWakuta 42:bf98a29e81ac 156 delt_t = t.read_ms() - count;
YusukeWakuta 42:bf98a29e81ac 157 if (delt_t > MPU_DELT_MIN) {
YusukeWakuta 42:bf98a29e81ac 158 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 159 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
YusukeWakuta 42:bf98a29e81ac 160 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 161 pitch *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 162 yaw *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 163 roll *= 180.0f / PI;
YusukeWakuta 47:5e3a836deaba 164 // myled= !myled;
YusukeWakuta 42:bf98a29e81ac 165 count = t.read_ms();
YusukeWakuta 42:bf98a29e81ac 166 sum = 0;
YusukeWakuta 42:bf98a29e81ac 167 sumCount = 0;
YusukeWakuta 42:bf98a29e81ac 168 }
YusukeWakuta 42:bf98a29e81ac 169 Thread::wait(1);
YusukeWakuta 42:bf98a29e81ac 170 }//while(1)
YusukeWakuta 42:bf98a29e81ac 171 }
YusukeWakuta 42:bf98a29e81ac 172
YusukeWakuta 42:bf98a29e81ac 173
YusukeWakuta 26:f14579683f98 174 bool inaInit()
YusukeWakuta 26:f14579683f98 175 {
YusukeWakuta 26:f14579683f98 176 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 177 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 178 return false;
taurin 0:e052602db102 179 }
taurin 0:e052602db102 180 ina_val = 0;
YusukeWakuta 26:f14579683f98 181 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 182 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 183 return false;
taurin 0:e052602db102 184 }
taurin 0:e052602db102 185 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 186 return true;
taurin 0:e052602db102 187 }
taurin 0:e052602db102 188
YusukeWakuta 26:f14579683f98 189 void init()
YusukeWakuta 26:f14579683f98 190 {
YusukeWakuta 26:f14579683f98 191 if(!LRstatePin) {
taurin 13:5e3b4120dbbf 192 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 193 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 194 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 195 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
YusukeWakuta 26:f14579683f98 196 } else {
taurin 12:fd9d241843f4 197 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 198 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 199 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 200 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 201 }
taurin 0:e052602db102 202 SERVO_FLAG = servoInit();
taurin 0:e052602db102 203 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 204 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 205 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 206 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
YusukeWakuta 24:d416722b4aad 207
YusukeWakuta 36:ddf4aa818e88 208 // initXBus();
taurin 0:e052602db102 209 }
taurin 0:e052602db102 210
YusukeWakuta 26:f14579683f98 211 void updateDatas()
YusukeWakuta 26:f14579683f98 212 {
YusukeWakuta 26:f14579683f98 213 if(INA_FLAG) {
taurin 0:e052602db102 214 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 215 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 216 }
YusukeWakuta 42:bf98a29e81ac 217
YusukeWakuta 26:f14579683f98 218 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
tsumagari 22:b38bc18ec3a1 219 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 220 }
tsumagari 22:b38bc18ec3a1 221 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
YusukeWakuta 40:ad98da5da7bf 222 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V;
taurin 0:e052602db102 223 }
taurin 0:e052602db102 224
YusukeWakuta 26:f14579683f98 225 void receiveDatas()
YusukeWakuta 26:f14579683f98 226 {
YusukeWakuta 26:f14579683f98 227 if(can.read(recmsg)) {
YusukeWakuta 26:f14579683f98 228 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 3:4417217b4f66 229 controlValues[i] = recmsg.data[i];
YusukeWakuta 42:bf98a29e81ac 230 if(i<sizeof(int)) intvalues[i] = controlValues[i];
taurin 0:e052602db102 231 }
YusukeWakuta 44:624a4469ae21 232 // intvalues[0] = controlValues[0];
YusukeWakuta 42:bf98a29e81ac 233 eruronint = *(const int *)intvalues;
taurin 1:9cc932a16d17 234 led1 = !led1;
taurin 0:e052602db102 235 }
taurin 0:e052602db102 236 }
taurin 0:e052602db102 237
YusukeWakuta 42:bf98a29e81ac 238 double calcPulse(float analog)
YusukeWakuta 26:f14579683f98 239 {
YusukeWakuta 42:bf98a29e81ac 240 return (0.0006+(analog)*(0.00240-0.00060));
YusukeWakuta 37:1f71ca1e5dd1 241 /*
YusukeWakuta 37:1f71ca1e5dd1 242 int start=510, end=2390;
YusukeWakuta 37:1f71ca1e5dd1 243 while(1) {
YusukeWakuta 37:1f71ca1e5dd1 244 // pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read()));
YusukeWakuta 37:1f71ca1e5dd1 245 pc.printf("%f\n\r",analogIn.read());
YusukeWakuta 37:1f71ca1e5dd1 246 pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read());
YusukeWakuta 37:1f71ca1e5dd1 247 */
taurin 1:9cc932a16d17 248 }
taurin 1:9cc932a16d17 249
YusukeWakuta 29:516a5d383488 250 void WriteServo()
YusukeWakuta 26:f14579683f98 251 {
YusukeWakuta 45:e3df49d7b912 252 drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
YusukeWakuta 43:9a57cec43257 253 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * ((1.0/PHASE_NUM) * eruronint) * ERURON_MOVE_RANGE));
YusukeWakuta 44:624a4469ae21 254 pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint* ERURON_MOVE_RANGE));
YusukeWakuta 47:5e3a836deaba 255 pc.printf("drValue::%f ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronint);
YusukeWakuta 37:1f71ca1e5dd1 256 // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
taurin 4:450cafd95ac3 257 }
taurin 4:450cafd95ac3 258
YusukeWakuta 26:f14579683f98 259 void setTrim()
YusukeWakuta 26:f14579683f98 260 {
YusukeWakuta 44:624a4469ae21 261 led2 = 1;
YusukeWakuta 26:f14579683f98 262 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 263 eruronTrim = eruronAna.read();
YusukeWakuta 26:f14579683f98 264 } else {
YusukeWakuta 37:1f71ca1e5dd1 265 drugTrim = drugAna.read();
YusukeWakuta 26:f14579683f98 266 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 267 }
YusukeWakuta 14:1f6dd929d7de 268 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 269 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 42:bf98a29e81ac 270 pc.printf("eMD:%f dMD:%f ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
taurin 4:450cafd95ac3 271 }
taurin 4:450cafd95ac3 272
YusukeWakuta 26:f14579683f98 273 void setMaxDeg()
YusukeWakuta 26:f14579683f98 274 {
taurin 4:450cafd95ac3 275 led4 = 1;
YusukeWakuta 26:f14579683f98 276 float eruronTemp = eruronAna.read();
YusukeWakuta 37:1f71ca1e5dd1 277 float drugTemp = drugAna.read();
YusukeWakuta 26:f14579683f98 278 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 279 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 26:f14579683f98 280 } else {
YusukeWakuta 26:f14579683f98 281 drugServo.pulsewidth(calcPulse(drugTemp));
YusukeWakuta 26:f14579683f98 282 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 283 }
YusukeWakuta 14:1f6dd929d7de 284 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 42:bf98a29e81ac 285 pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
taurin 4:450cafd95ac3 286 wait_us(10);
taurin 1:9cc932a16d17 287 }
taurin 0:e052602db102 288
YusukeWakuta 26:f14579683f98 289 int main()
YusukeWakuta 26:f14579683f98 290 {
taurin 0:e052602db102 291 init();
YusukeWakuta 31:5d22ebe5f705 292
YusukeWakuta 30:00041540e23c 293 setTrimPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 294 setMaxDegPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 295 EDstatePin.mode(PullDown);
YusukeWakuta 30:00041540e23c 296 LRstatePin.mode(PullDown);
YusukeWakuta 42:bf98a29e81ac 297 Thread mpu_thread(&mpuProcessing);
YusukeWakuta 26:f14579683f98 298
YusukeWakuta 24:d416722b4aad 299 // start motion
tsumagari 33:d075918d4846 300 // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 301
YusukeWakuta 26:f14579683f98 302 while(1) {
YusukeWakuta 26:f14579683f98 303 while(setTrimPin) {
taurin 4:450cafd95ac3 304 setTrim();
taurin 4:450cafd95ac3 305 }
YusukeWakuta 26:f14579683f98 306 while (setMaxDegPin) {
YusukeWakuta 29:516a5d383488 307 setMaxDeg();
taurin 4:450cafd95ac3 308 }
YusukeWakuta 42:bf98a29e81ac 309 // pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 42:bf98a29e81ac 310 //// pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg);
YusukeWakuta 42:bf98a29e81ac 311 // pc.printf("pitch:%d roll:%d yaw:%d\n\r",pitch,roll,yaw);
taurin 4:450cafd95ac3 312 led4 = 0;
YusukeWakuta 44:624a4469ae21 313 led2 = 0;
taurin 16:82310bf7c326 314 //receiveDatas();
tsumagari 22:b38bc18ec3a1 315 // sendDatas();
taurin 1:9cc932a16d17 316 WriteServo();
taurin 4:450cafd95ac3 317 updateDatas();
tsumagari 22:b38bc18ec3a1 318 led3 = !led3;
taurin 2:7fcb4f970a02 319 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 320 }
taurin 0:e052602db102 321 }