2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
main.cpp@25:e8bfb629e1b1, 2017-02-15 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Feb 15 13:29:20 2017 +0000
- Branch:
- XBus???
- Revision:
- 25:e8bfb629e1b1
- Parent:
- 24:d416722b4aad
- Parent:
- 23:d551db88df65
- Child:
- 26:f14579683f98
merge
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 "ADXL345_I2C.h" |
tsumagari | 22:b38bc18ec3a1 | 4 | #include "MPU6050.h" |
taurin | 0:e052602db102 | 5 | #include "INA226.hpp" |
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 |
tsumagari | 23:d551db88df65 | 11 | #define CONTROL_VALUES_NUM sizeof(float) + 1 |
taurin | 0:e052602db102 | 12 | #define TO_SEND_CAN_ID 100 |
taurin | 4:450cafd95ac3 | 13 | #define ADXL_MEAN_NUM 10 |
taurin | 4:450cafd95ac3 | 14 | #define SEND_DATAS_LOOP_TIME 0.1 |
taurin | 16:82310bf7c326 | 15 | #define RECEIVE_DATAS_LOOP_TIME 0.05 |
taurin | 12:fd9d241843f4 | 16 | |
tsumagari | 23:d551db88df65 | 17 | #define ERURON_MOVE_DEG_INI_R 30 // もともと10 |
YusukeWakuta | 18:253f0526b8b3 | 18 | #define DRUG_MOVE_DEG_INI_R 76 |
YusukeWakuta | 20:a5a85df4de13 | 19 | #define ERURON_TRIM_INI_R 97 //元々94 |
YusukeWakuta | 18:253f0526b8b3 | 20 | #define DRUG_TRIM_INI_R 33 |
taurin | 12:fd9d241843f4 | 21 | |
tsumagari | 23:d551db88df65 | 22 | #define ERURON_MOVE_DEG_INI_L -30 //もともと-7 |
YusukeWakuta | 17:d91ce54dd09a | 23 | #define DRUG_MOVE_DEG_INI_L -80 |
YusukeWakuta | 20:a5a85df4de13 | 24 | #define ERURON_TRIM_INI_L 113 //元々95 |
YusukeWakuta | 17:d91ce54dd09a | 25 | #define DRUG_TRIM_INI_L 110 |
taurin | 2:7fcb4f970a02 | 26 | |
YusukeWakuta | 24:d416722b4aad | 27 | #define kXBusTx p9 |
YusukeWakuta | 24:d416722b4aad | 28 | #define kMaxServoNum 1 // 1 - 50 |
YusukeWakuta | 24:d416722b4aad | 29 | #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) |
YusukeWakuta | 24:d416722b4aad | 30 | #define kMotionInterval 10 // flame / sec |
YusukeWakuta | 24:d416722b4aad | 31 | #define kMotionMinMark 0x1249 |
YusukeWakuta | 24:d416722b4aad | 32 | #define kMotionEndMark 0xED86 |
YusukeWakuta | 24:d416722b4aad | 33 | |
taurin | 0:e052602db102 | 34 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 35 | CANMessage recmsg; |
taurin | 0:e052602db102 | 36 | Serial pc(USBTX,USBRX); |
tsumagari | 22:b38bc18ec3a1 | 37 | //ADXL345_I2C accelerometer(p9, p10); |
tsumagari | 22:b38bc18ec3a1 | 38 | MPU6050 mpu(p9,p10); |
taurin | 0:e052602db102 | 39 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 40 | INA226 VCmonitor(ina226_i2c); |
taurin | 13:5e3b4120dbbf | 41 | PwmOut drugServo(p22); |
taurin | 13:5e3b4120dbbf | 42 | PwmOut eruronServo(p23); |
taurin | 1:9cc932a16d17 | 43 | DigitalOut led1(LED1); |
taurin | 4:450cafd95ac3 | 44 | AnalogIn drugAna(p20); |
taurin | 4:450cafd95ac3 | 45 | AnalogIn eruronAna(p19); |
taurin | 12:fd9d241843f4 | 46 | DigitalIn LRstatePin(p14); |
taurin | 4:450cafd95ac3 | 47 | DigitalIn setTrimPin(p15); |
taurin | 12:fd9d241843f4 | 48 | DigitalIn EDstatePin(p16); |
taurin | 4:450cafd95ac3 | 49 | DigitalIn checkMaxDegPin(p17); |
taurin | 4:450cafd95ac3 | 50 | DigitalOut debugLED(LED2); |
taurin | 4:450cafd95ac3 | 51 | DigitalOut led3(LED3); |
taurin | 4:450cafd95ac3 | 52 | DigitalOut led4(LED4); |
taurin | 4:450cafd95ac3 | 53 | Ticker sendDatasTicker; |
tsumagari | 22:b38bc18ec3a1 | 54 | //Ticker toStringTicker; |
taurin | 16:82310bf7c326 | 55 | Ticker receiveDatasTicker; |
taurin | 16:82310bf7c326 | 56 | |
taurin | 0:e052602db102 | 57 | char toSendDatas[TO_SEND_DATAS_NUM]; |
tsumagari | 23:d551db88df65 | 58 | char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug |
tsumagari | 23:d551db88df65 | 59 | char floatvalues[sizeof(float)]; |
taurin | 12:fd9d241843f4 | 60 | float eruronTrim; |
taurin | 12:fd9d241843f4 | 61 | float drugTrim; |
taurin | 12:fd9d241843f4 | 62 | float eruronMoveDeg; |
taurin | 12:fd9d241843f4 | 63 | float drugMoveDeg; |
tsumagari | 23:d551db88df65 | 64 | float eruronfloat; |
taurin | 0:e052602db102 | 65 | unsigned short ina_val; |
taurin | 0:e052602db102 | 66 | double V,C; |
taurin | 0:e052602db102 | 67 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 68 | bool ADXL_FLAG; |
taurin | 0:e052602db102 | 69 | bool INA_FLAG; |
tsumagari | 22:b38bc18ec3a1 | 70 | bool MPU_FLAG; |
taurin | 0:e052602db102 | 71 | |
tsumagari | 22:b38bc18ec3a1 | 72 | //int16_t acc[3] = {0,0,0}; |
tsumagari | 22:b38bc18ec3a1 | 73 | char gyro_c[6] = {0,0,0,0,0,0}; |
tsumagari | 22:b38bc18ec3a1 | 74 | //char acc_mean[3][ADXL_MEAN_NUM]; |
tsumagari | 22:b38bc18ec3a1 | 75 | //int adxl_mean_counter = 0; |
taurin | 4:450cafd95ac3 | 76 | |
taurin | 4:450cafd95ac3 | 77 | void toString(); |
taurin | 16:82310bf7c326 | 78 | void receiveDatas(); |
taurin | 16:82310bf7c326 | 79 | void WriteServo(); |
taurin | 0:e052602db102 | 80 | |
YusukeWakuta | 24:d416722b4aad | 81 | static const uint8_t servoChannel = 0x01; |
YusukeWakuta | 24:d416722b4aad | 82 | |
YusukeWakuta | 24:d416722b4aad | 83 | XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum); |
YusukeWakuta | 24:d416722b4aad | 84 | Ticker gTimer; |
YusukeWakuta | 24:d416722b4aad | 85 | |
taurin | 0:e052602db102 | 86 | bool servoInit(){ |
taurin | 4:450cafd95ac3 | 87 | drugServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 4:450cafd95ac3 | 88 | eruronServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 89 | return true; |
taurin | 0:e052602db102 | 90 | } |
taurin | 0:e052602db102 | 91 | |
YusukeWakuta | 24:d416722b4aad | 92 | //============================================================= |
YusukeWakuta | 24:d416722b4aad | 93 | // XbusIntervalHandler() |
YusukeWakuta | 24:d416722b4aad | 94 | // play motion ! |
YusukeWakuta | 24:d416722b4aad | 95 | //============================================================= |
YusukeWakuta | 24:d416722b4aad | 96 | void XbusIntervalHandler() |
YusukeWakuta | 24:d416722b4aad | 97 | { |
YusukeWakuta | 24:d416722b4aad | 98 | uint16_t value; |
YusukeWakuta | 24:d416722b4aad | 99 | uint16_t diff = kMotionEndMark - kMotionMinMark; |
YusukeWakuta | 24:d416722b4aad | 100 | value = (uint16_t)(diff * analog.read()) + kMotionMinMark; |
YusukeWakuta | 24:d416722b4aad | 101 | gXBus.setServo(servoChannel, value); |
YusukeWakuta | 24:d416722b4aad | 102 | gXBus.sendChannelDataPacket(); |
YusukeWakuta | 24:d416722b4aad | 103 | } |
YusukeWakuta | 24:d416722b4aad | 104 | |
tsumagari | 22:b38bc18ec3a1 | 105 | //bool adxlInit(){ |
tsumagari | 22:b38bc18ec3a1 | 106 | // accelerometer.setPowerControl(0x00); |
tsumagari | 22:b38bc18ec3a1 | 107 | // accelerometer.setDataFormatControl(0x0B); |
tsumagari | 22:b38bc18ec3a1 | 108 | // accelerometer.setDataRate(ADXL345_3200HZ); |
tsumagari | 22:b38bc18ec3a1 | 109 | // accelerometer.setPowerControl(0x08); |
tsumagari | 22:b38bc18ec3a1 | 110 | // return true; |
tsumagari | 22:b38bc18ec3a1 | 111 | //} |
taurin | 0:e052602db102 | 112 | |
taurin | 4:450cafd95ac3 | 113 | void sendDatas(){ |
taurin | 4:450cafd95ac3 | 114 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){ |
taurin | 4:450cafd95ac3 | 115 | } |
taurin | 4:450cafd95ac3 | 116 | } |
taurin | 4:450cafd95ac3 | 117 | |
taurin | 0:e052602db102 | 118 | bool inaInit(){ |
taurin | 0:e052602db102 | 119 | if(!VCmonitor.isExist()){ |
taurin | 0:e052602db102 | 120 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 121 | return false; |
taurin | 0:e052602db102 | 122 | } |
taurin | 0:e052602db102 | 123 | ina_val = 0; |
taurin | 0:e052602db102 | 124 | if(VCmonitor.rawRead(0x00,&ina_val) != 0){ |
taurin | 0:e052602db102 | 125 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 126 | return false; |
taurin | 0:e052602db102 | 127 | } |
taurin | 0:e052602db102 | 128 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 129 | return true; |
taurin | 0:e052602db102 | 130 | } |
taurin | 0:e052602db102 | 131 | |
YusukeWakuta | 24:d416722b4aad | 132 | XBusError init(){ |
YusukeWakuta | 24:d416722b4aad | 133 | |
YusukeWakuta | 24:d416722b4aad | 134 | XBusError result; |
YusukeWakuta | 24:d416722b4aad | 135 | |
taurin | 15:1db5ee4fe7ce | 136 | if(!LRstatePin){ |
taurin | 13:5e3b4120dbbf | 137 | eruronTrim = ERURON_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 138 | drugTrim = DRUG_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 139 | eruronMoveDeg = ERURON_MOVE_DEG_INI_L; |
taurin | 13:5e3b4120dbbf | 140 | drugMoveDeg = DRUG_MOVE_DEG_INI_L; |
taurin | 13:5e3b4120dbbf | 141 | } |
taurin | 13:5e3b4120dbbf | 142 | else{ |
taurin | 12:fd9d241843f4 | 143 | eruronTrim = ERURON_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 144 | drugTrim = DRUG_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 145 | eruronMoveDeg = ERURON_MOVE_DEG_INI_R; |
taurin | 13:5e3b4120dbbf | 146 | drugMoveDeg =DRUG_MOVE_DEG_INI_R; |
taurin | 12:fd9d241843f4 | 147 | } |
taurin | 0:e052602db102 | 148 | SERVO_FLAG = servoInit(); |
tsumagari | 22:b38bc18ec3a1 | 149 | // ADXL_FLAG = adxlInit(); |
tsumagari | 22:b38bc18ec3a1 | 150 | MPU_FLAG = mpu.testConnection(); |
taurin | 0:e052602db102 | 151 | INA_FLAG = inaInit(); |
taurin | 4:450cafd95ac3 | 152 | sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); |
taurin | 16:82310bf7c326 | 153 | // toStringTicker.attach(&toString,0.5); |
taurin | 16:82310bf7c326 | 154 | receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); |
YusukeWakuta | 24:d416722b4aad | 155 | |
YusukeWakuta | 24:d416722b4aad | 156 | //XBus |
YusukeWakuta | 24:d416722b4aad | 157 | // initialize XBus |
YusukeWakuta | 24:d416722b4aad | 158 | result = gXBus.start(); |
YusukeWakuta | 24:d416722b4aad | 159 | if (result != kXBusError_NoError) { |
YusukeWakuta | 24:d416722b4aad | 160 | gXBus.stop(); |
YusukeWakuta | 24:d416722b4aad | 161 | return result; |
YusukeWakuta | 24:d416722b4aad | 162 | } |
YusukeWakuta | 24:d416722b4aad | 163 | // initialize XBus Servos |
YusukeWakuta | 24:d416722b4aad | 164 | result = gXBus.addServo(servoChannel, kXbusServoNeutral); |
YusukeWakuta | 24:d416722b4aad | 165 | if (result != kXBusError_NoError) { |
YusukeWakuta | 24:d416722b4aad | 166 | gXBus.stop(); |
YusukeWakuta | 24:d416722b4aad | 167 | return result; |
YusukeWakuta | 24:d416722b4aad | 168 | } |
YusukeWakuta | 24:d416722b4aad | 169 | |
YusukeWakuta | 24:d416722b4aad | 170 | return kXBusError_NoError; |
taurin | 0:e052602db102 | 171 | } |
taurin | 0:e052602db102 | 172 | |
taurin | 0:e052602db102 | 173 | void updateDatas(){ |
tsumagari | 22:b38bc18ec3a1 | 174 | // if(ADXL_FLAG){ |
tsumagari | 22:b38bc18ec3a1 | 175 | //// accelerometer.getOutput(acc); |
tsumagari | 22:b38bc18ec3a1 | 176 | // } |
taurin | 0:e052602db102 | 177 | if(INA_FLAG){ |
taurin | 0:e052602db102 | 178 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 179 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 180 | } |
tsumagari | 22:b38bc18ec3a1 | 181 | if(MPU_FLAG){ |
tsumagari | 22:b38bc18ec3a1 | 182 | mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6); |
taurin | 0:e052602db102 | 183 | } |
tsumagari | 22:b38bc18ec3a1 | 184 | for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){ |
tsumagari | 22:b38bc18ec3a1 | 185 | // toSendDatas[i] = acc[i]; |
tsumagari | 22:b38bc18ec3a1 | 186 | toSendDatas[i] = gyro_c[i]; |
tsumagari | 22:b38bc18ec3a1 | 187 | } |
tsumagari | 22:b38bc18ec3a1 | 188 | // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); |
tsumagari | 22:b38bc18ec3a1 | 189 | toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77; |
taurin | 0:e052602db102 | 190 | } |
taurin | 0:e052602db102 | 191 | |
taurin | 0:e052602db102 | 192 | void receiveDatas(){ |
taurin | 0:e052602db102 | 193 | if(can.read(recmsg)){ |
taurin | 2:7fcb4f970a02 | 194 | for(int i = 0; i < CONTROL_VALUES_NUM; i++){ |
YusukeWakuta | 3:4417217b4f66 | 195 | controlValues[i] = recmsg.data[i]; |
tsumagari | 23:d551db88df65 | 196 | if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i]; |
taurin | 0:e052602db102 | 197 | } |
tsumagari | 23:d551db88df65 | 198 | eruronfloat = *(const float *)floatvalues; |
taurin | 1:9cc932a16d17 | 199 | led1 = !led1; |
taurin | 16:82310bf7c326 | 200 | //WriteServo(); |
taurin | 0:e052602db102 | 201 | } |
taurin | 0:e052602db102 | 202 | } |
taurin | 0:e052602db102 | 203 | |
taurin | 0:e052602db102 | 204 | void toString(){ |
taurin | 4:450cafd95ac3 | 205 | for(int i = 0; i <CONTROL_VALUES_NUM; i++){ |
taurin | 4:450cafd95ac3 | 206 | pc.printf("%d, ",controlValues[i]); |
taurin | 4:450cafd95ac3 | 207 | } |
taurin | 4:450cafd95ac3 | 208 | pc.printf("\n\r"); |
taurin | 0:e052602db102 | 209 | } |
taurin | 0:e052602db102 | 210 | |
taurin | 1:9cc932a16d17 | 211 | double calcPulse(int deg){ |
taurin | 4:450cafd95ac3 | 212 | return (0.0006+(deg/180.0)*(0.00235-0.00045)); |
taurin | 4:450cafd95ac3 | 213 | |
taurin | 1:9cc932a16d17 | 214 | } |
taurin | 1:9cc932a16d17 | 215 | |
taurin | 1:9cc932a16d17 | 216 | void WriteServo(){ |
taurin | 12:fd9d241843f4 | 217 | //if(debugServoPin){ |
taurin | 12:fd9d241843f4 | 218 | // led3 = 1; |
taurin | 12:fd9d241843f4 | 219 | // float a = eruronAna.read()*180; |
taurin | 12:fd9d241843f4 | 220 | // float b = drugAna.read()*180; |
taurin | 12:fd9d241843f4 | 221 | // eruronServo.pulsewidth(calcPulse(eruronAna.read()*180)); |
taurin | 12:fd9d241843f4 | 222 | // drugServo.pulsewidth(calcPulse(drugAna.read()*180)); |
taurin | 12:fd9d241843f4 | 223 | // pc.printf("eruron:%f drug:%f\n\r",a,b); |
taurin | 12:fd9d241843f4 | 224 | // } |
taurin | 12:fd9d241843f4 | 225 | // else{ |
taurin | 12:fd9d241843f4 | 226 | // led3 = 0; |
tsumagari | 23:d551db88df65 | 227 | // eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) )); |
tsumagari | 23:d551db88df65 | 228 | drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4])); |
tsumagari | 23:d551db88df65 | 229 | eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) )); |
taurin | 12:fd9d241843f4 | 230 | //} |
taurin | 4:450cafd95ac3 | 231 | } |
taurin | 4:450cafd95ac3 | 232 | |
taurin | 4:450cafd95ac3 | 233 | void setTrim(){ |
taurin | 4:450cafd95ac3 | 234 | debugLED = 1; |
taurin | 12:fd9d241843f4 | 235 | if(EDstatePin){ |
taurin | 4:450cafd95ac3 | 236 | eruronTrim = eruronAna.read()*180; |
taurin | 12:fd9d241843f4 | 237 | eruronServo.pulsewidth(calcPulse(eruronTrim)); |
taurin | 12:fd9d241843f4 | 238 | } |
taurin | 12:fd9d241843f4 | 239 | else{ |
taurin | 4:450cafd95ac3 | 240 | drugTrim = drugAna.read()*180; |
taurin | 4:450cafd95ac3 | 241 | drugServo.pulsewidth(calcPulse(drugTrim)); |
taurin | 12:fd9d241843f4 | 242 | } |
YusukeWakuta | 14:1f6dd929d7de | 243 | //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); |
YusukeWakuta | 14:1f6dd929d7de | 244 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
tsumagari | 23:d551db88df65 | 245 | pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); |
taurin | 4:450cafd95ac3 | 246 | } |
taurin | 4:450cafd95ac3 | 247 | |
taurin | 4:450cafd95ac3 | 248 | void checkMaxDeg(){ |
taurin | 4:450cafd95ac3 | 249 | led4 = 1; |
taurin | 4:450cafd95ac3 | 250 | float eruronTemp = eruronAna.read()*180; |
taurin | 4:450cafd95ac3 | 251 | float drugTemp = drugAna.read()*180; |
taurin | 12:fd9d241843f4 | 252 | if(EDstatePin){ |
taurin | 4:450cafd95ac3 | 253 | eruronServo.pulsewidth(calcPulse(eruronTemp)); |
taurin | 12:fd9d241843f4 | 254 | eruronMoveDeg = eruronTemp-eruronTrim; |
taurin | 12:fd9d241843f4 | 255 | } |
taurin | 12:fd9d241843f4 | 256 | else{ |
taurin | 4:450cafd95ac3 | 257 | drugServo.pulsewidth(calcPulse(drugTemp)); |
taurin | 4:450cafd95ac3 | 258 | drugMoveDeg = drugTemp-drugTrim; |
taurin | 12:fd9d241843f4 | 259 | } |
YusukeWakuta | 14:1f6dd929d7de | 260 | // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
YusukeWakuta | 14:1f6dd929d7de | 261 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
tsumagari | 23:d551db88df65 | 262 | pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); |
taurin | 4:450cafd95ac3 | 263 | wait_us(10); |
taurin | 1:9cc932a16d17 | 264 | } |
taurin | 0:e052602db102 | 265 | |
taurin | 0:e052602db102 | 266 | int main(){ |
taurin | 0:e052602db102 | 267 | init(); |
YusukeWakuta | 24:d416722b4aad | 268 | XBusError result; |
YusukeWakuta | 24:d416722b4aad | 269 | |
YusukeWakuta | 24:d416722b4aad | 270 | // start motion |
YusukeWakuta | 24:d416722b4aad | 271 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 24:d416722b4aad | 272 | |
taurin | 0:e052602db102 | 273 | while(1){ |
taurin | 4:450cafd95ac3 | 274 | while(setTrimPin){ |
taurin | 4:450cafd95ac3 | 275 | setTrim(); |
taurin | 4:450cafd95ac3 | 276 | } |
taurin | 4:450cafd95ac3 | 277 | while(checkMaxDegPin){ |
taurin | 4:450cafd95ac3 | 278 | checkMaxDeg(); |
taurin | 4:450cafd95ac3 | 279 | } |
YusukeWakuta | 14:1f6dd929d7de | 280 | // pc.printf("eT:%f\n\r",eruronTrim); |
YusukeWakuta | 14:1f6dd929d7de | 281 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
tsumagari | 23:d551db88df65 | 282 | pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat); |
taurin | 4:450cafd95ac3 | 283 | led4 = 0; |
YusukeWakuta | 14:1f6dd929d7de | 284 | |
taurin | 4:450cafd95ac3 | 285 | debugLED = 0; |
taurin | 16:82310bf7c326 | 286 | //receiveDatas(); |
tsumagari | 22:b38bc18ec3a1 | 287 | // sendDatas(); |
taurin | 1:9cc932a16d17 | 288 | WriteServo(); |
taurin | 4:450cafd95ac3 | 289 | updateDatas(); |
tsumagari | 22:b38bc18ec3a1 | 290 | // pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]); |
tsumagari | 22:b38bc18ec3a1 | 291 | led3 = !led3; |
taurin | 2:7fcb4f970a02 | 292 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 293 | } |
taurin | 0:e052602db102 | 294 | } |