2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
main.cpp@37:1f71ca1e5dd1, 2017-03-11 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Sat Mar 11 16:22:16 2017 +0000
- Branch:
- XBus???
- Revision:
- 37:1f71ca1e5dd1
- Parent:
- 36:ddf4aa818e88
- Child:
- 38:b492990e2b56
??????????????
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 | 24:d416722b4aad | 5 | #include "XBusServo.h" |
taurin | 4:450cafd95ac3 | 6 | |
tsumagari | 22:b38bc18ec3a1 | 7 | #define TO_SEND_DATAS_NUM 7 |
taurin | 2:7fcb4f970a02 | 8 | #define INIT_SERVO_PERIOD_MS 20 |
taurin | 4:450cafd95ac3 | 9 | #define WAIT_LOOP_TIME 0.02 |
tsumagari | 23:d551db88df65 | 10 | #define CONTROL_VALUES_NUM sizeof(float) + 1 |
taurin | 0:e052602db102 | 11 | #define TO_SEND_CAN_ID 100 |
taurin | 4:450cafd95ac3 | 12 | #define SEND_DATAS_LOOP_TIME 0.1 |
tsumagari | 33:d075918d4846 | 13 | #define RECEIVE_DATAS_LOOP_TIME 0.1 |
taurin | 12:fd9d241843f4 | 14 | |
YusukeWakuta | 35:707119a675dc | 15 | #define ERURON_MOVE_DEG_INI_R 1.0 |
YusukeWakuta | 37:1f71ca1e5dd1 | 16 | #define DRUG_MOVE_DEG_INI_R 0.3 |
YusukeWakuta | 32:b03557a08efa | 17 | #define ERURON_TRIM_INI_R 0 |
YusukeWakuta | 37:1f71ca1e5dd1 | 18 | #define DRUG_TRIM_INI_R 0.5 |
taurin | 12:fd9d241843f4 | 19 | |
YusukeWakuta | 35:707119a675dc | 20 | #define ERURON_MOVE_DEG_INI_L 1.0 |
YusukeWakuta | 37:1f71ca1e5dd1 | 21 | #define DRUG_MOVE_DEG_INI_L 0.3 |
YusukeWakuta | 32:b03557a08efa | 22 | #define ERURON_TRIM_INI_L 0 |
YusukeWakuta | 37:1f71ca1e5dd1 | 23 | #define DRUG_TRIM_INI_L 0.5 |
taurin | 2:7fcb4f970a02 | 24 | |
YusukeWakuta | 36:ddf4aa818e88 | 25 | /* |
YusukeWakuta | 24:d416722b4aad | 26 | #define kMaxServoNum 1 // 1 - 50 |
YusukeWakuta | 24:d416722b4aad | 27 | #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) |
YusukeWakuta | 24:d416722b4aad | 28 | #define kMotionInterval 10 // flame / sec |
YusukeWakuta | 24:d416722b4aad | 29 | #define kMotionMinMark 0x1249 |
YusukeWakuta | 24:d416722b4aad | 30 | #define kMotionEndMark 0xED86 |
YusukeWakuta | 37:1f71ca1e5dd1 | 31 | |
YusukeWakuta | 37:1f71ca1e5dd1 | 32 | 初期値 0.65 |
YusukeWakuta | 37:1f71ca1e5dd1 | 33 | 最大角0.99 |
YusukeWakuta | 36:ddf4aa818e88 | 34 | */ |
YusukeWakuta | 24:d416722b4aad | 35 | |
taurin | 0:e052602db102 | 36 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 37 | CANMessage recmsg; |
taurin | 0:e052602db102 | 38 | Serial pc(USBTX,USBRX); |
tsumagari | 22:b38bc18ec3a1 | 39 | MPU6050 mpu(p9,p10); |
taurin | 0:e052602db102 | 40 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 41 | INA226 VCmonitor(ina226_i2c); |
taurin | 13:5e3b4120dbbf | 42 | PwmOut drugServo(p22); |
tsumagari | 33:d075918d4846 | 43 | PwmOut eruronServo(p23); |
taurin | 1:9cc932a16d17 | 44 | DigitalOut led1(LED1); |
taurin | 4:450cafd95ac3 | 45 | AnalogIn drugAna(p20); |
taurin | 4:450cafd95ac3 | 46 | AnalogIn eruronAna(p19); |
YusukeWakuta | 28:99686a3f0e86 | 47 | DigitalIn LRstatePin(p11); |
YusukeWakuta | 28:99686a3f0e86 | 48 | DigitalIn setTrimPin(p12); |
YusukeWakuta | 28:99686a3f0e86 | 49 | DigitalIn EDstatePin(p14); |
YusukeWakuta | 28:99686a3f0e86 | 50 | DigitalIn setMaxDegPin(p15); |
taurin | 4:450cafd95ac3 | 51 | DigitalOut debugLED(LED2); |
taurin | 4:450cafd95ac3 | 52 | DigitalOut led3(LED3); |
taurin | 4:450cafd95ac3 | 53 | DigitalOut led4(LED4); |
taurin | 4:450cafd95ac3 | 54 | Ticker sendDatasTicker; |
tsumagari | 22:b38bc18ec3a1 | 55 | //Ticker toStringTicker; |
taurin | 16:82310bf7c326 | 56 | Ticker receiveDatasTicker; |
taurin | 16:82310bf7c326 | 57 | |
taurin | 0:e052602db102 | 58 | char toSendDatas[TO_SEND_DATAS_NUM]; |
tsumagari | 33:d075918d4846 | 59 | char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug |
tsumagari | 23:d551db88df65 | 60 | char floatvalues[sizeof(float)]; |
taurin | 12:fd9d241843f4 | 61 | float eruronTrim; |
taurin | 12:fd9d241843f4 | 62 | float drugTrim; |
taurin | 12:fd9d241843f4 | 63 | float eruronMoveDeg; |
taurin | 12:fd9d241843f4 | 64 | float drugMoveDeg; |
tsumagari | 23:d551db88df65 | 65 | float eruronfloat; |
taurin | 0:e052602db102 | 66 | unsigned short ina_val; |
taurin | 0:e052602db102 | 67 | double V,C; |
taurin | 0:e052602db102 | 68 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 69 | bool INA_FLAG; |
tsumagari | 22:b38bc18ec3a1 | 70 | bool MPU_FLAG; |
YusukeWakuta | 31:5d22ebe5f705 | 71 | uint16_t XbusValue; |
taurin | 0:e052602db102 | 72 | |
tsumagari | 22:b38bc18ec3a1 | 73 | char gyro_c[6] = {0,0,0,0,0,0}; |
taurin | 4:450cafd95ac3 | 74 | |
taurin | 4:450cafd95ac3 | 75 | void toString(); |
taurin | 16:82310bf7c326 | 76 | void receiveDatas(); |
taurin | 16:82310bf7c326 | 77 | void WriteServo(); |
taurin | 0:e052602db102 | 78 | |
YusukeWakuta | 36:ddf4aa818e88 | 79 | //XBusServo gXBus(p13, NC, NC, kMaxServoNum); |
YusukeWakuta | 24:d416722b4aad | 80 | Ticker gTimer; |
YusukeWakuta | 24:d416722b4aad | 81 | |
YusukeWakuta | 26:f14579683f98 | 82 | bool servoInit() |
YusukeWakuta | 26:f14579683f98 | 83 | { |
taurin | 4:450cafd95ac3 | 84 | drugServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 85 | return true; |
taurin | 0:e052602db102 | 86 | } |
taurin | 0:e052602db102 | 87 | |
YusukeWakuta | 26:f14579683f98 | 88 | void sendDatas() |
YusukeWakuta | 24:d416722b4aad | 89 | { |
YusukeWakuta | 26:f14579683f98 | 90 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) { |
taurin | 4:450cafd95ac3 | 91 | } |
taurin | 4:450cafd95ac3 | 92 | } |
taurin | 4:450cafd95ac3 | 93 | |
YusukeWakuta | 26:f14579683f98 | 94 | bool inaInit() |
YusukeWakuta | 26:f14579683f98 | 95 | { |
YusukeWakuta | 26:f14579683f98 | 96 | if(!VCmonitor.isExist()) { |
taurin | 0:e052602db102 | 97 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 98 | return false; |
taurin | 0:e052602db102 | 99 | } |
taurin | 0:e052602db102 | 100 | ina_val = 0; |
YusukeWakuta | 26:f14579683f98 | 101 | if(VCmonitor.rawRead(0x00,&ina_val) != 0) { |
taurin | 0:e052602db102 | 102 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 103 | return false; |
taurin | 0:e052602db102 | 104 | } |
taurin | 0:e052602db102 | 105 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 106 | return true; |
taurin | 0:e052602db102 | 107 | } |
taurin | 0:e052602db102 | 108 | |
YusukeWakuta | 36:ddf4aa818e88 | 109 | /* |
YusukeWakuta | 26:f14579683f98 | 110 | XBusError initXBus() |
YusukeWakuta | 26:f14579683f98 | 111 | { |
YusukeWakuta | 24:d416722b4aad | 112 | XBusError result; |
YusukeWakuta | 26:f14579683f98 | 113 | result = gXBus.start(); |
YusukeWakuta | 26:f14579683f98 | 114 | if (result != kXBusError_NoError) { |
YusukeWakuta | 26:f14579683f98 | 115 | gXBus.stop(); |
YusukeWakuta | 26:f14579683f98 | 116 | return result; |
YusukeWakuta | 26:f14579683f98 | 117 | } |
YusukeWakuta | 26:f14579683f98 | 118 | result = gXBus.addServo(servoChannel, kXbusServoNeutral); |
YusukeWakuta | 26:f14579683f98 | 119 | if (result != kXBusError_NoError) { |
YusukeWakuta | 26:f14579683f98 | 120 | gXBus.stop(); |
YusukeWakuta | 26:f14579683f98 | 121 | return result; |
YusukeWakuta | 26:f14579683f98 | 122 | } |
YusukeWakuta | 26:f14579683f98 | 123 | return kXBusError_NoError; |
YusukeWakuta | 26:f14579683f98 | 124 | } |
YusukeWakuta | 36:ddf4aa818e88 | 125 | */ |
YusukeWakuta | 26:f14579683f98 | 126 | |
YusukeWakuta | 26:f14579683f98 | 127 | void init() |
YusukeWakuta | 26:f14579683f98 | 128 | { |
YusukeWakuta | 26:f14579683f98 | 129 | if(!LRstatePin) { |
taurin | 13:5e3b4120dbbf | 130 | eruronTrim = ERURON_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 131 | drugTrim = DRUG_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 132 | eruronMoveDeg = ERURON_MOVE_DEG_INI_L; |
taurin | 13:5e3b4120dbbf | 133 | drugMoveDeg = DRUG_MOVE_DEG_INI_L; |
YusukeWakuta | 26:f14579683f98 | 134 | } else { |
taurin | 12:fd9d241843f4 | 135 | eruronTrim = ERURON_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 136 | drugTrim = DRUG_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 137 | eruronMoveDeg = ERURON_MOVE_DEG_INI_R; |
taurin | 13:5e3b4120dbbf | 138 | drugMoveDeg =DRUG_MOVE_DEG_INI_R; |
taurin | 12:fd9d241843f4 | 139 | } |
taurin | 0:e052602db102 | 140 | SERVO_FLAG = servoInit(); |
tsumagari | 22:b38bc18ec3a1 | 141 | MPU_FLAG = mpu.testConnection(); |
taurin | 0:e052602db102 | 142 | INA_FLAG = inaInit(); |
taurin | 4:450cafd95ac3 | 143 | sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); |
taurin | 16:82310bf7c326 | 144 | // toStringTicker.attach(&toString,0.5); |
taurin | 16:82310bf7c326 | 145 | receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); |
YusukeWakuta | 24:d416722b4aad | 146 | |
YusukeWakuta | 36:ddf4aa818e88 | 147 | // initXBus(); |
taurin | 0:e052602db102 | 148 | } |
taurin | 0:e052602db102 | 149 | |
YusukeWakuta | 26:f14579683f98 | 150 | void updateDatas() |
YusukeWakuta | 26:f14579683f98 | 151 | { |
YusukeWakuta | 26:f14579683f98 | 152 | if(INA_FLAG) { |
taurin | 0:e052602db102 | 153 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 154 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 155 | } |
YusukeWakuta | 26:f14579683f98 | 156 | if(MPU_FLAG) { |
tsumagari | 22:b38bc18ec3a1 | 157 | mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6); |
taurin | 0:e052602db102 | 158 | } |
YusukeWakuta | 26:f14579683f98 | 159 | for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) { |
tsumagari | 22:b38bc18ec3a1 | 160 | toSendDatas[i] = gyro_c[i]; |
tsumagari | 22:b38bc18ec3a1 | 161 | } |
tsumagari | 22:b38bc18ec3a1 | 162 | // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); |
tsumagari | 22:b38bc18ec3a1 | 163 | toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77; |
taurin | 0:e052602db102 | 164 | } |
taurin | 0:e052602db102 | 165 | |
YusukeWakuta | 26:f14579683f98 | 166 | void receiveDatas() |
YusukeWakuta | 26:f14579683f98 | 167 | { |
YusukeWakuta | 26:f14579683f98 | 168 | if(can.read(recmsg)) { |
YusukeWakuta | 26:f14579683f98 | 169 | for(int i = 0; i < CONTROL_VALUES_NUM; i++) { |
YusukeWakuta | 3:4417217b4f66 | 170 | controlValues[i] = recmsg.data[i]; |
tsumagari | 33:d075918d4846 | 171 | if(i<sizeof(float)) floatvalues[i] = controlValues[i]; |
taurin | 0:e052602db102 | 172 | } |
tsumagari | 23:d551db88df65 | 173 | eruronfloat = *(const float *)floatvalues; |
taurin | 1:9cc932a16d17 | 174 | led1 = !led1; |
taurin | 0:e052602db102 | 175 | } |
taurin | 0:e052602db102 | 176 | } |
taurin | 0:e052602db102 | 177 | |
YusukeWakuta | 35:707119a675dc | 178 | double calcPulse(float deg) |
YusukeWakuta | 26:f14579683f98 | 179 | { |
YusukeWakuta | 35:707119a675dc | 180 | return (0.0006+(deg)*(0.00235-0.00045)); |
YusukeWakuta | 37:1f71ca1e5dd1 | 181 | /* |
YusukeWakuta | 37:1f71ca1e5dd1 | 182 | int start=510, end=2390; |
YusukeWakuta | 37:1f71ca1e5dd1 | 183 | while(1) { |
YusukeWakuta | 37:1f71ca1e5dd1 | 184 | // pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read())); |
YusukeWakuta | 37:1f71ca1e5dd1 | 185 | pc.printf("%f\n\r",analogIn.read()); |
YusukeWakuta | 37:1f71ca1e5dd1 | 186 | pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read()); |
YusukeWakuta | 37:1f71ca1e5dd1 | 187 | */ |
taurin | 1:9cc932a16d17 | 188 | } |
taurin | 1:9cc932a16d17 | 189 | |
YusukeWakuta | 35:707119a675dc | 190 | float SampleFloat(float f) //小数点以下第二位を切り捨て |
YusukeWakuta | 26:f14579683f98 | 191 | { |
YusukeWakuta | 36:ddf4aa818e88 | 192 | int temp = ((f + 0.025) * 100.0) / 5; |
YusukeWakuta | 35:707119a675dc | 193 | float result = temp / 20.0; |
YusukeWakuta | 35:707119a675dc | 194 | return result; |
YusukeWakuta | 26:f14579683f98 | 195 | } |
YusukeWakuta | 26:f14579683f98 | 196 | |
YusukeWakuta | 29:516a5d383488 | 197 | void WriteServo() |
YusukeWakuta | 26:f14579683f98 | 198 | { |
YusukeWakuta | 37:1f71ca1e5dd1 | 199 | drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)])); |
YusukeWakuta | 37:1f71ca1e5dd1 | 200 | eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * SampleFloat((eruronfloat / 2.0)))); |
YusukeWakuta | 37:1f71ca1e5dd1 | 201 | pc.printf("drValue::%f ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0,SampleFloat((eruronfloat / 2.0))); |
YusukeWakuta | 37:1f71ca1e5dd1 | 202 | // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0)); |
taurin | 4:450cafd95ac3 | 203 | } |
taurin | 4:450cafd95ac3 | 204 | |
YusukeWakuta | 26:f14579683f98 | 205 | void setTrim() |
YusukeWakuta | 26:f14579683f98 | 206 | { |
taurin | 4:450cafd95ac3 | 207 | debugLED = 1; |
YusukeWakuta | 26:f14579683f98 | 208 | if(EDstatePin) { |
YusukeWakuta | 26:f14579683f98 | 209 | eruronTrim = eruronAna.read(); |
YusukeWakuta | 26:f14579683f98 | 210 | } else { |
YusukeWakuta | 37:1f71ca1e5dd1 | 211 | drugTrim = drugAna.read(); |
YusukeWakuta | 26:f14579683f98 | 212 | drugServo.pulsewidth(calcPulse(drugTrim)); |
taurin | 12:fd9d241843f4 | 213 | } |
YusukeWakuta | 14:1f6dd929d7de | 214 | //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); |
YusukeWakuta | 26:f14579683f98 | 215 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 26:f14579683f98 | 216 | pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); |
taurin | 4:450cafd95ac3 | 217 | } |
taurin | 4:450cafd95ac3 | 218 | |
YusukeWakuta | 26:f14579683f98 | 219 | void setMaxDeg() |
YusukeWakuta | 26:f14579683f98 | 220 | { |
taurin | 4:450cafd95ac3 | 221 | led4 = 1; |
YusukeWakuta | 26:f14579683f98 | 222 | float eruronTemp = eruronAna.read(); |
YusukeWakuta | 37:1f71ca1e5dd1 | 223 | float drugTemp = drugAna.read(); |
YusukeWakuta | 26:f14579683f98 | 224 | if(EDstatePin) { |
YusukeWakuta | 26:f14579683f98 | 225 | eruronMoveDeg = eruronTemp-eruronTrim; |
YusukeWakuta | 26:f14579683f98 | 226 | } else { |
YusukeWakuta | 26:f14579683f98 | 227 | drugServo.pulsewidth(calcPulse(drugTemp)); |
YusukeWakuta | 26:f14579683f98 | 228 | drugMoveDeg = drugTemp-drugTrim; |
taurin | 12:fd9d241843f4 | 229 | } |
YusukeWakuta | 14:1f6dd929d7de | 230 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
tsumagari | 23:d551db88df65 | 231 | pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); |
taurin | 4:450cafd95ac3 | 232 | wait_us(10); |
taurin | 1:9cc932a16d17 | 233 | } |
taurin | 0:e052602db102 | 234 | |
YusukeWakuta | 26:f14579683f98 | 235 | int main() |
YusukeWakuta | 26:f14579683f98 | 236 | { |
taurin | 0:e052602db102 | 237 | init(); |
YusukeWakuta | 31:5d22ebe5f705 | 238 | |
YusukeWakuta | 30:00041540e23c | 239 | setTrimPin.mode(PullDown); |
YusukeWakuta | 30:00041540e23c | 240 | setMaxDegPin.mode(PullDown); |
YusukeWakuta | 30:00041540e23c | 241 | EDstatePin.mode(PullDown); |
YusukeWakuta | 30:00041540e23c | 242 | LRstatePin.mode(PullDown); |
YusukeWakuta | 26:f14579683f98 | 243 | |
YusukeWakuta | 24:d416722b4aad | 244 | // start motion |
tsumagari | 33:d075918d4846 | 245 | // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 26:f14579683f98 | 246 | |
YusukeWakuta | 26:f14579683f98 | 247 | while(1) { |
YusukeWakuta | 26:f14579683f98 | 248 | while(setTrimPin) { |
taurin | 4:450cafd95ac3 | 249 | setTrim(); |
taurin | 4:450cafd95ac3 | 250 | } |
YusukeWakuta | 26:f14579683f98 | 251 | while (setMaxDegPin) { |
YusukeWakuta | 29:516a5d383488 | 252 | setMaxDeg(); |
taurin | 4:450cafd95ac3 | 253 | } |
YusukeWakuta | 37:1f71ca1e5dd1 | 254 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 37:1f71ca1e5dd1 | 255 | pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg); |
taurin | 4:450cafd95ac3 | 256 | led4 = 0; |
taurin | 4:450cafd95ac3 | 257 | debugLED = 0; |
taurin | 16:82310bf7c326 | 258 | //receiveDatas(); |
tsumagari | 22:b38bc18ec3a1 | 259 | // sendDatas(); |
taurin | 1:9cc932a16d17 | 260 | WriteServo(); |
taurin | 4:450cafd95ac3 | 261 | updateDatas(); |
tsumagari | 22:b38bc18ec3a1 | 262 | led3 = !led3; |
taurin | 2:7fcb4f970a02 | 263 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 264 | } |
taurin | 0:e052602db102 | 265 | } |