albatross / Mbed 2 deprecated ControlYokutan2017_2

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@61:988e3f4280ac, 2017-06-07 (annotated)

Committer:
YusukeWakuta
Date:
Wed Jun 07 15:14:10 2017 +0000
Branch:
mpu????????
Revision:
61:988e3f4280ac
Parent:
60:a45dc19a6001
Child:
62:1db967d29809
???????????????

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