2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
main.cpp@57:d7b709dd1c4f, 2017-05-13 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Sat May 13 15:20:02 2017 +0000
- Branch:
- mpu????????
- Revision:
- 57:d7b709dd1c4f
- Parent:
- 56:2b947d31d307
- Child:
- 58:f84bd22fd586
5?13???????
Who changed what in which revision?
User | Revision | Line number | New 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すると頭上げ |
tsumagari | 53:3eeaafa49707 | 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 | 57:d7b709dd1c4f | 27 | #define ERURON_TRIM_INI_L 0.402 // 値をお大きいくすると頭上げ |
YusukeWakuta | 47:5e3a836deaba | 28 | #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側 |
taurin | 2:7fcb4f970a02 | 29 | |
YusukeWakuta | 57:d7b709dd1c4f | 30 | #define PHASE_NUM 15 //奇数にしてください。そしてメインコードと必ず同じ値にしてください |
YusukeWakuta | 56:2b947d31d307 | 31 | #define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.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 | 48:0bd406fa4a7f | 48 | DigitalIn IsRPin(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 | 55:eaff0c602c99 | 71 | int eruronint = NEUTRAL_PHASE; |
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 | 48:0bd406fa4a7f | 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 | 57:d7b709dd1c4f | 189 | //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。 |
YusukeWakuta | 57:d7b709dd1c4f | 190 | double ConvertDeg(double servo) |
YusukeWakuta | 57:d7b709dd1c4f | 191 | { |
YusukeWakuta | 57:d7b709dd1c4f | 192 | double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105; |
YusukeWakuta | 57:d7b709dd1c4f | 193 | return result; |
YusukeWakuta | 57:d7b709dd1c4f | 194 | } |
YusukeWakuta | 57:d7b709dd1c4f | 195 | |
YusukeWakuta | 57:d7b709dd1c4f | 196 | //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化 |
YusukeWakuta | 57:d7b709dd1c4f | 197 | double GetValueByHorn(double deg) |
YusukeWakuta | 57:d7b709dd1c4f | 198 | { |
YusukeWakuta | 57:d7b709dd1c4f | 199 | return deg*(0.10/25.7); |
YusukeWakuta | 57:d7b709dd1c4f | 200 | } |
YusukeWakuta | 57:d7b709dd1c4f | 201 | |
YusukeWakuta | 57:d7b709dd1c4f | 202 | double GetFloatByErebon(double erebonDeg) |
YusukeWakuta | 57:d7b709dd1c4f | 203 | { |
YusukeWakuta | 57:d7b709dd1c4f | 204 | double servoDeg = ConvertDeg(erebonDeg); |
YusukeWakuta | 57:d7b709dd1c4f | 205 | double FirstMoveDeg = GetValueByHorn(servoDeg); |
YusukeWakuta | 57:d7b709dd1c4f | 206 | return FirstMoveDeg; |
YusukeWakuta | 57:d7b709dd1c4f | 207 | } |
YusukeWakuta | 57:d7b709dd1c4f | 208 | |
YusukeWakuta | 26:f14579683f98 | 209 | void init() |
YusukeWakuta | 26:f14579683f98 | 210 | { |
YusukeWakuta | 48:0bd406fa4a7f | 211 | if(IsRPin) { |
YusukeWakuta | 48:0bd406fa4a7f | 212 | eruronTrim = ERURON_TRIM_INI_R; |
YusukeWakuta | 48:0bd406fa4a7f | 213 | drugTrim = DRUG_TRIM_INI_R; |
YusukeWakuta | 57:d7b709dd1c4f | 214 | eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R); |
YusukeWakuta | 48:0bd406fa4a7f | 215 | drugMoveDeg =DRUG_MOVE_DEG_INI_R; |
YusukeWakuta | 48:0bd406fa4a7f | 216 | } else { |
taurin | 13:5e3b4120dbbf | 217 | eruronTrim = ERURON_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 218 | drugTrim = DRUG_TRIM_INI_L; |
YusukeWakuta | 57:d7b709dd1c4f | 219 | eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L); |
taurin | 13:5e3b4120dbbf | 220 | drugMoveDeg = DRUG_MOVE_DEG_INI_L; |
taurin | 12:fd9d241843f4 | 221 | } |
taurin | 0:e052602db102 | 222 | SERVO_FLAG = servoInit(); |
taurin | 0:e052602db102 | 223 | INA_FLAG = inaInit(); |
taurin | 4:450cafd95ac3 | 224 | sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); |
taurin | 16:82310bf7c326 | 225 | // toStringTicker.attach(&toString,0.5); |
taurin | 16:82310bf7c326 | 226 | receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); |
YusukeWakuta | 24:d416722b4aad | 227 | |
YusukeWakuta | 36:ddf4aa818e88 | 228 | // initXBus(); |
taurin | 0:e052602db102 | 229 | } |
taurin | 0:e052602db102 | 230 | |
YusukeWakuta | 26:f14579683f98 | 231 | void updateDatas() |
YusukeWakuta | 26:f14579683f98 | 232 | { |
YusukeWakuta | 26:f14579683f98 | 233 | if(INA_FLAG) { |
taurin | 0:e052602db102 | 234 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 235 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 236 | } |
YusukeWakuta | 42:bf98a29e81ac | 237 | |
YusukeWakuta | 26:f14579683f98 | 238 | for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) { |
tsumagari | 22:b38bc18ec3a1 | 239 | toSendDatas[i] = gyro_c[i]; |
tsumagari | 22:b38bc18ec3a1 | 240 | } |
tsumagari | 22:b38bc18ec3a1 | 241 | // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); |
YusukeWakuta | 40:ad98da5da7bf | 242 | toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V; |
taurin | 0:e052602db102 | 243 | } |
taurin | 0:e052602db102 | 244 | |
YusukeWakuta | 26:f14579683f98 | 245 | void receiveDatas() |
YusukeWakuta | 26:f14579683f98 | 246 | { |
YusukeWakuta | 26:f14579683f98 | 247 | if(can.read(recmsg)) { |
YusukeWakuta | 26:f14579683f98 | 248 | for(int i = 0; i < CONTROL_VALUES_NUM; i++) { |
YusukeWakuta | 3:4417217b4f66 | 249 | controlValues[i] = recmsg.data[i]; |
YusukeWakuta | 42:bf98a29e81ac | 250 | if(i<sizeof(int)) intvalues[i] = controlValues[i]; |
taurin | 0:e052602db102 | 251 | } |
YusukeWakuta | 44:624a4469ae21 | 252 | // intvalues[0] = controlValues[0]; |
YusukeWakuta | 42:bf98a29e81ac | 253 | eruronint = *(const int *)intvalues; |
taurin | 1:9cc932a16d17 | 254 | led1 = !led1; |
taurin | 0:e052602db102 | 255 | } |
taurin | 0:e052602db102 | 256 | } |
taurin | 0:e052602db102 | 257 | |
YusukeWakuta | 42:bf98a29e81ac | 258 | double calcPulse(float analog) |
YusukeWakuta | 26:f14579683f98 | 259 | { |
tsumagari | 53:3eeaafa49707 | 260 | return (0.0006 + (analog)*(0.00240-0.00060) ); |
tsumagari | 53:3eeaafa49707 | 261 | // double min = 0.0006; |
tsumagari | 53:3eeaafa49707 | 262 | // double max = 0.00240; |
tsumagari | 53:3eeaafa49707 | 263 | // if(analog >= max) |
tsumagari | 53:3eeaafa49707 | 264 | // analog = max; |
tsumagari | 53:3eeaafa49707 | 265 | // else if(analog <= min) |
tsumagari | 53:3eeaafa49707 | 266 | // analog = min; |
YusukeWakuta | 56:2b947d31d307 | 267 | // |
tsumagari | 53:3eeaafa49707 | 268 | // return (min+(analog)*(max-min)); |
YusukeWakuta | 37:1f71ca1e5dd1 | 269 | /* |
YusukeWakuta | 37:1f71ca1e5dd1 | 270 | int start=510, end=2390; |
YusukeWakuta | 37:1f71ca1e5dd1 | 271 | while(1) { |
YusukeWakuta | 37:1f71ca1e5dd1 | 272 | // pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read())); |
YusukeWakuta | 37:1f71ca1e5dd1 | 273 | pc.printf("%f\n\r",analogIn.read()); |
YusukeWakuta | 37:1f71ca1e5dd1 | 274 | pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read()); |
YusukeWakuta | 37:1f71ca1e5dd1 | 275 | */ |
taurin | 1:9cc932a16d17 | 276 | } |
taurin | 1:9cc932a16d17 | 277 | |
YusukeWakuta | 29:516a5d383488 | 278 | void WriteServo() |
YusukeWakuta | 26:f14579683f98 | 279 | { |
YusukeWakuta | 45:e3df49d7b912 | 280 | drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2])); |
YusukeWakuta | 57:d7b709dd1c4f | 281 | eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE))); |
YusukeWakuta | 48:0bd406fa4a7f | 282 | pc.printf("WriteNum:%f ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint)); |
YusukeWakuta | 55:eaff0c602c99 | 283 | pc.printf("drValue::%f ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE)); |
YusukeWakuta | 37:1f71ca1e5dd1 | 284 | // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); |
taurin | 4:450cafd95ac3 | 285 | } |
taurin | 4:450cafd95ac3 | 286 | |
YusukeWakuta | 26:f14579683f98 | 287 | void setTrim() |
YusukeWakuta | 26:f14579683f98 | 288 | { |
YusukeWakuta | 44:624a4469ae21 | 289 | led2 = 1; |
YusukeWakuta | 26:f14579683f98 | 290 | if(EDstatePin) { |
YusukeWakuta | 26:f14579683f98 | 291 | eruronTrim = eruronAna.read(); |
YusukeWakuta | 50:b3a8f8e88c50 | 292 | eruronServo.pulsewidth(calcPulse(eruronTrim)); |
YusukeWakuta | 26:f14579683f98 | 293 | } else { |
YusukeWakuta | 37:1f71ca1e5dd1 | 294 | drugTrim = drugAna.read(); |
YusukeWakuta | 26:f14579683f98 | 295 | drugServo.pulsewidth(calcPulse(drugTrim)); |
taurin | 12:fd9d241843f4 | 296 | } |
YusukeWakuta | 14:1f6dd929d7de | 297 | //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); |
YusukeWakuta | 26:f14579683f98 | 298 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 42:bf98a29e81ac | 299 | pc.printf("eMD:%f dMD:%f ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint); |
taurin | 4:450cafd95ac3 | 300 | } |
taurin | 4:450cafd95ac3 | 301 | |
YusukeWakuta | 26:f14579683f98 | 302 | void setMaxDeg() |
YusukeWakuta | 26:f14579683f98 | 303 | { |
taurin | 4:450cafd95ac3 | 304 | led4 = 1; |
YusukeWakuta | 26:f14579683f98 | 305 | float eruronTemp = eruronAna.read(); |
YusukeWakuta | 37:1f71ca1e5dd1 | 306 | float drugTemp = drugAna.read(); |
YusukeWakuta | 26:f14579683f98 | 307 | if(EDstatePin) { |
YusukeWakuta | 26:f14579683f98 | 308 | eruronMoveDeg = eruronTemp-eruronTrim; |
YusukeWakuta | 49:8522856fe0cd | 309 | eruronServo.pulsewidth(calcPulse(eruronTemp)); |
YusukeWakuta | 26:f14579683f98 | 310 | } else { |
YusukeWakuta | 26:f14579683f98 | 311 | drugServo.pulsewidth(calcPulse(drugTemp)); |
YusukeWakuta | 26:f14579683f98 | 312 | drugMoveDeg = drugTemp-drugTrim; |
taurin | 12:fd9d241843f4 | 313 | } |
YusukeWakuta | 14:1f6dd929d7de | 314 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 42:bf98a29e81ac | 315 | pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint); |
taurin | 4:450cafd95ac3 | 316 | wait_us(10); |
taurin | 1:9cc932a16d17 | 317 | } |
taurin | 0:e052602db102 | 318 | |
YusukeWakuta | 26:f14579683f98 | 319 | int main() |
YusukeWakuta | 26:f14579683f98 | 320 | { |
taurin | 0:e052602db102 | 321 | init(); |
YusukeWakuta | 31:5d22ebe5f705 | 322 | |
YusukeWakuta | 30:00041540e23c | 323 | setTrimPin.mode(PullDown); |
YusukeWakuta | 30:00041540e23c | 324 | setMaxDegPin.mode(PullDown); |
YusukeWakuta | 30:00041540e23c | 325 | EDstatePin.mode(PullDown); |
YusukeWakuta | 48:0bd406fa4a7f | 326 | IsRPin.mode(PullDown); |
YusukeWakuta | 42:bf98a29e81ac | 327 | Thread mpu_thread(&mpuProcessing); |
YusukeWakuta | 26:f14579683f98 | 328 | |
YusukeWakuta | 24:d416722b4aad | 329 | // start motion |
tsumagari | 33:d075918d4846 | 330 | // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 26:f14579683f98 | 331 | |
YusukeWakuta | 26:f14579683f98 | 332 | while(1) { |
YusukeWakuta | 26:f14579683f98 | 333 | while(setTrimPin) { |
taurin | 4:450cafd95ac3 | 334 | setTrim(); |
taurin | 4:450cafd95ac3 | 335 | } |
YusukeWakuta | 26:f14579683f98 | 336 | while (setMaxDegPin) { |
YusukeWakuta | 29:516a5d383488 | 337 | setMaxDeg(); |
taurin | 4:450cafd95ac3 | 338 | } |
taurin | 4:450cafd95ac3 | 339 | led4 = 0; |
YusukeWakuta | 44:624a4469ae21 | 340 | led2 = 0; |
taurin | 16:82310bf7c326 | 341 | //receiveDatas(); |
tsumagari | 22:b38bc18ec3a1 | 342 | // sendDatas(); |
taurin | 1:9cc932a16d17 | 343 | WriteServo(); |
taurin | 4:450cafd95ac3 | 344 | updateDatas(); |
tsumagari | 22:b38bc18ec3a1 | 345 | led3 = !led3; |
taurin | 2:7fcb4f970a02 | 346 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 347 | } |
taurin | 0:e052602db102 | 348 | } |