![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
+3度のやつ
Dependencies: ADXL345_I2C Control_Yokutan_CANver1 mbed
Fork of Souda_Yokutan_ver4 by
main.cpp@18:253f0526b8b3, 2016-05-25 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed May 25 13:27:51 2016 +0000
- Revision:
- 18:253f0526b8b3
- Parent:
- 17:d91ce54dd09a
+3??????
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" |
taurin | 0:e052602db102 | 3 | #include "ADXL345_I2C.h" |
taurin | 0:e052602db102 | 4 | #include "INA226.hpp" |
taurin | 4:450cafd95ac3 | 5 | |
taurin | 4:450cafd95ac3 | 6 | #define TO_SEND_DATAS_NUM 4 |
taurin | 2:7fcb4f970a02 | 7 | #define INIT_SERVO_PERIOD_MS 20 |
taurin | 4:450cafd95ac3 | 8 | #define WAIT_LOOP_TIME 0.02 |
taurin | 0:e052602db102 | 9 | #define CONTROL_VALUES_NUM 2 |
taurin | 0:e052602db102 | 10 | #define TO_SEND_CAN_ID 100 |
taurin | 4:450cafd95ac3 | 11 | #define ADXL_MEAN_NUM 10 |
taurin | 4:450cafd95ac3 | 12 | #define SEND_DATAS_LOOP_TIME 0.1 |
taurin | 16:82310bf7c326 | 13 | #define RECEIVE_DATAS_LOOP_TIME 0.05 |
taurin | 12:fd9d241843f4 | 14 | |
YusukeWakuta | 18:253f0526b8b3 | 15 | #define ERURON_MOVE_DEG_INI_R 10 |
YusukeWakuta | 18:253f0526b8b3 | 16 | #define DRUG_MOVE_DEG_INI_R 76 |
YusukeWakuta | 18:253f0526b8b3 | 17 | #define ERURON_TRIM_INI_R 90 //元々94 |
YusukeWakuta | 18:253f0526b8b3 | 18 | #define DRUG_TRIM_INI_R 33 |
taurin | 12:fd9d241843f4 | 19 | |
YusukeWakuta | 18:253f0526b8b3 | 20 | #define ERURON_MOVE_DEG_INI_L -7 |
YusukeWakuta | 17:d91ce54dd09a | 21 | #define DRUG_MOVE_DEG_INI_L -80 |
YusukeWakuta | 18:253f0526b8b3 | 22 | #define ERURON_TRIM_INI_L 92 //元々95 |
YusukeWakuta | 17:d91ce54dd09a | 23 | #define DRUG_TRIM_INI_L 110 |
taurin | 2:7fcb4f970a02 | 24 | |
taurin | 0:e052602db102 | 25 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 26 | CANMessage recmsg; |
taurin | 0:e052602db102 | 27 | Serial pc(USBTX,USBRX); |
taurin | 0:e052602db102 | 28 | ADXL345_I2C accelerometer(p9, p10); |
taurin | 0:e052602db102 | 29 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 30 | INA226 VCmonitor(ina226_i2c); |
taurin | 13:5e3b4120dbbf | 31 | PwmOut drugServo(p22); |
taurin | 13:5e3b4120dbbf | 32 | PwmOut eruronServo(p23); |
taurin | 1:9cc932a16d17 | 33 | DigitalOut led1(LED1); |
taurin | 4:450cafd95ac3 | 34 | AnalogIn drugAna(p20); |
taurin | 4:450cafd95ac3 | 35 | AnalogIn eruronAna(p19); |
taurin | 12:fd9d241843f4 | 36 | DigitalIn LRstatePin(p14); |
taurin | 4:450cafd95ac3 | 37 | DigitalIn setTrimPin(p15); |
taurin | 12:fd9d241843f4 | 38 | DigitalIn EDstatePin(p16); |
taurin | 4:450cafd95ac3 | 39 | DigitalIn checkMaxDegPin(p17); |
taurin | 4:450cafd95ac3 | 40 | DigitalOut debugLED(LED2); |
taurin | 4:450cafd95ac3 | 41 | DigitalOut led3(LED3); |
taurin | 4:450cafd95ac3 | 42 | DigitalOut led4(LED4); |
taurin | 4:450cafd95ac3 | 43 | Ticker sendDatasTicker; |
taurin | 4:450cafd95ac3 | 44 | Ticker toStringTicker; |
taurin | 16:82310bf7c326 | 45 | Ticker receiveDatasTicker; |
taurin | 16:82310bf7c326 | 46 | |
taurin | 0:e052602db102 | 47 | char toSendDatas[TO_SEND_DATAS_NUM]; |
taurin | 1:9cc932a16d17 | 48 | char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug |
taurin | 0:e052602db102 | 49 | |
taurin | 12:fd9d241843f4 | 50 | float eruronTrim; |
taurin | 12:fd9d241843f4 | 51 | float drugTrim; |
taurin | 12:fd9d241843f4 | 52 | float eruronMoveDeg; |
taurin | 12:fd9d241843f4 | 53 | float drugMoveDeg; |
taurin | 0:e052602db102 | 54 | unsigned short ina_val; |
taurin | 0:e052602db102 | 55 | double V,C; |
taurin | 0:e052602db102 | 56 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 57 | bool ADXL_FLAG; |
taurin | 0:e052602db102 | 58 | bool INA_FLAG; |
taurin | 0:e052602db102 | 59 | |
taurin | 0:e052602db102 | 60 | int acc[3] = {0,0,0}; |
taurin | 4:450cafd95ac3 | 61 | char acc_mean[3][ADXL_MEAN_NUM]; |
taurin | 4:450cafd95ac3 | 62 | int adxl_mean_counter = 0; |
taurin | 4:450cafd95ac3 | 63 | |
taurin | 4:450cafd95ac3 | 64 | void toString(); |
taurin | 16:82310bf7c326 | 65 | void receiveDatas(); |
taurin | 16:82310bf7c326 | 66 | void WriteServo(); |
taurin | 0:e052602db102 | 67 | |
taurin | 0:e052602db102 | 68 | bool servoInit(){ |
taurin | 4:450cafd95ac3 | 69 | drugServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 4:450cafd95ac3 | 70 | eruronServo.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 71 | return true; |
taurin | 0:e052602db102 | 72 | } |
taurin | 0:e052602db102 | 73 | |
taurin | 0:e052602db102 | 74 | bool adxlInit(){ |
taurin | 0:e052602db102 | 75 | accelerometer.setPowerControl(0x00); |
taurin | 0:e052602db102 | 76 | accelerometer.setDataFormatControl(0x0B); |
taurin | 0:e052602db102 | 77 | accelerometer.setDataRate(ADXL345_3200HZ); |
taurin | 0:e052602db102 | 78 | accelerometer.setPowerControl(0x08); |
taurin | 0:e052602db102 | 79 | return true; |
taurin | 0:e052602db102 | 80 | } |
taurin | 0:e052602db102 | 81 | |
taurin | 4:450cafd95ac3 | 82 | void sendDatas(){ |
taurin | 4:450cafd95ac3 | 83 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){ |
taurin | 4:450cafd95ac3 | 84 | } |
taurin | 4:450cafd95ac3 | 85 | } |
taurin | 4:450cafd95ac3 | 86 | |
taurin | 0:e052602db102 | 87 | bool inaInit(){ |
taurin | 0:e052602db102 | 88 | if(!VCmonitor.isExist()){ |
taurin | 0:e052602db102 | 89 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 90 | return false; |
taurin | 0:e052602db102 | 91 | } |
taurin | 0:e052602db102 | 92 | ina_val = 0; |
taurin | 0:e052602db102 | 93 | if(VCmonitor.rawRead(0x00,&ina_val) != 0){ |
taurin | 0:e052602db102 | 94 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 95 | return false; |
taurin | 0:e052602db102 | 96 | } |
taurin | 0:e052602db102 | 97 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 98 | return true; |
taurin | 0:e052602db102 | 99 | } |
taurin | 0:e052602db102 | 100 | |
taurin | 0:e052602db102 | 101 | void init(){ |
taurin | 15:1db5ee4fe7ce | 102 | if(!LRstatePin){ |
taurin | 13:5e3b4120dbbf | 103 | eruronTrim = ERURON_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 104 | drugTrim = DRUG_TRIM_INI_L; |
taurin | 13:5e3b4120dbbf | 105 | eruronMoveDeg = ERURON_MOVE_DEG_INI_L; |
taurin | 13:5e3b4120dbbf | 106 | drugMoveDeg = DRUG_MOVE_DEG_INI_L; |
taurin | 13:5e3b4120dbbf | 107 | } |
taurin | 13:5e3b4120dbbf | 108 | else{ |
taurin | 12:fd9d241843f4 | 109 | eruronTrim = ERURON_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 110 | drugTrim = DRUG_TRIM_INI_R; |
taurin | 12:fd9d241843f4 | 111 | eruronMoveDeg = ERURON_MOVE_DEG_INI_R; |
taurin | 13:5e3b4120dbbf | 112 | drugMoveDeg =DRUG_MOVE_DEG_INI_R; |
taurin | 12:fd9d241843f4 | 113 | } |
taurin | 0:e052602db102 | 114 | SERVO_FLAG = servoInit(); |
taurin | 0:e052602db102 | 115 | ADXL_FLAG = adxlInit(); |
taurin | 0:e052602db102 | 116 | INA_FLAG = inaInit(); |
taurin | 4:450cafd95ac3 | 117 | sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); |
taurin | 16:82310bf7c326 | 118 | // toStringTicker.attach(&toString,0.5); |
taurin | 16:82310bf7c326 | 119 | receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); |
taurin | 0:e052602db102 | 120 | } |
taurin | 0:e052602db102 | 121 | |
taurin | 0:e052602db102 | 122 | void updateDatas(){ |
taurin | 0:e052602db102 | 123 | if(ADXL_FLAG){ |
taurin | 0:e052602db102 | 124 | accelerometer.getOutput(acc); |
taurin | 0:e052602db102 | 125 | } |
taurin | 0:e052602db102 | 126 | if(INA_FLAG){ |
taurin | 0:e052602db102 | 127 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 128 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 129 | } |
taurin | 0:e052602db102 | 130 | for(int i = 0; i < 3; i++){ |
taurin | 0:e052602db102 | 131 | toSendDatas[i] = acc[i]; |
taurin | 0:e052602db102 | 132 | } |
taurin | 0:e052602db102 | 133 | toSendDatas[3] = (char)(V/100); |
taurin | 0:e052602db102 | 134 | } |
taurin | 0:e052602db102 | 135 | |
taurin | 0:e052602db102 | 136 | void receiveDatas(){ |
taurin | 0:e052602db102 | 137 | if(can.read(recmsg)){ |
taurin | 2:7fcb4f970a02 | 138 | for(int i = 0; i < CONTROL_VALUES_NUM; i++){ |
YusukeWakuta | 3:4417217b4f66 | 139 | controlValues[i] = recmsg.data[i]; |
taurin | 0:e052602db102 | 140 | } |
taurin | 1:9cc932a16d17 | 141 | led1 = !led1; |
taurin | 16:82310bf7c326 | 142 | //WriteServo(); |
taurin | 0:e052602db102 | 143 | } |
taurin | 0:e052602db102 | 144 | } |
taurin | 0:e052602db102 | 145 | |
taurin | 0:e052602db102 | 146 | void toString(){ |
taurin | 4:450cafd95ac3 | 147 | for(int i = 0; i <CONTROL_VALUES_NUM; i++){ |
taurin | 4:450cafd95ac3 | 148 | pc.printf("%d, ",controlValues[i]); |
taurin | 4:450cafd95ac3 | 149 | } |
taurin | 4:450cafd95ac3 | 150 | pc.printf("\n\r"); |
taurin | 0:e052602db102 | 151 | } |
taurin | 0:e052602db102 | 152 | |
taurin | 1:9cc932a16d17 | 153 | double calcPulse(int deg){ |
taurin | 4:450cafd95ac3 | 154 | return (0.0006+(deg/180.0)*(0.00235-0.00045)); |
taurin | 4:450cafd95ac3 | 155 | |
taurin | 1:9cc932a16d17 | 156 | } |
taurin | 1:9cc932a16d17 | 157 | |
taurin | 1:9cc932a16d17 | 158 | void WriteServo(){ |
taurin | 12:fd9d241843f4 | 159 | //if(debugServoPin){ |
taurin | 12:fd9d241843f4 | 160 | // led3 = 1; |
taurin | 12:fd9d241843f4 | 161 | // float a = eruronAna.read()*180; |
taurin | 12:fd9d241843f4 | 162 | // float b = drugAna.read()*180; |
taurin | 12:fd9d241843f4 | 163 | // eruronServo.pulsewidth(calcPulse(eruronAna.read()*180)); |
taurin | 12:fd9d241843f4 | 164 | // drugServo.pulsewidth(calcPulse(drugAna.read()*180)); |
taurin | 12:fd9d241843f4 | 165 | // pc.printf("eruron:%f drug:%f\n\r",a,b); |
taurin | 12:fd9d241843f4 | 166 | // } |
taurin | 12:fd9d241843f4 | 167 | // else{ |
taurin | 12:fd9d241843f4 | 168 | // led3 = 0; |
taurin | 4:450cafd95ac3 | 169 | eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1))); |
taurin | 4:450cafd95ac3 | 170 | drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1])); |
taurin | 12:fd9d241843f4 | 171 | //} |
taurin | 4:450cafd95ac3 | 172 | } |
taurin | 4:450cafd95ac3 | 173 | |
taurin | 4:450cafd95ac3 | 174 | void setTrim(){ |
taurin | 4:450cafd95ac3 | 175 | debugLED = 1; |
taurin | 12:fd9d241843f4 | 176 | if(EDstatePin){ |
taurin | 4:450cafd95ac3 | 177 | eruronTrim = eruronAna.read()*180; |
taurin | 12:fd9d241843f4 | 178 | eruronServo.pulsewidth(calcPulse(eruronTrim)); |
taurin | 12:fd9d241843f4 | 179 | } |
taurin | 12:fd9d241843f4 | 180 | else{ |
taurin | 4:450cafd95ac3 | 181 | drugTrim = drugAna.read()*180; |
taurin | 4:450cafd95ac3 | 182 | drugServo.pulsewidth(calcPulse(drugTrim)); |
taurin | 12:fd9d241843f4 | 183 | } |
YusukeWakuta | 14:1f6dd929d7de | 184 | //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim); |
YusukeWakuta | 14:1f6dd929d7de | 185 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 14:1f6dd929d7de | 186 | pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
taurin | 4:450cafd95ac3 | 187 | } |
taurin | 4:450cafd95ac3 | 188 | |
taurin | 4:450cafd95ac3 | 189 | void checkMaxDeg(){ |
taurin | 4:450cafd95ac3 | 190 | led4 = 1; |
taurin | 4:450cafd95ac3 | 191 | float eruronTemp = eruronAna.read()*180; |
taurin | 4:450cafd95ac3 | 192 | float drugTemp = drugAna.read()*180; |
taurin | 12:fd9d241843f4 | 193 | if(EDstatePin){ |
taurin | 4:450cafd95ac3 | 194 | eruronServo.pulsewidth(calcPulse(eruronTemp)); |
taurin | 12:fd9d241843f4 | 195 | eruronMoveDeg = eruronTemp-eruronTrim; |
taurin | 12:fd9d241843f4 | 196 | } |
taurin | 12:fd9d241843f4 | 197 | else{ |
taurin | 4:450cafd95ac3 | 198 | drugServo.pulsewidth(calcPulse(drugTemp)); |
taurin | 4:450cafd95ac3 | 199 | drugMoveDeg = drugTemp-drugTrim; |
taurin | 12:fd9d241843f4 | 200 | } |
YusukeWakuta | 14:1f6dd929d7de | 201 | // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
YusukeWakuta | 14:1f6dd929d7de | 202 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 14:1f6dd929d7de | 203 | pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
taurin | 4:450cafd95ac3 | 204 | wait_us(10); |
taurin | 1:9cc932a16d17 | 205 | } |
taurin | 0:e052602db102 | 206 | |
taurin | 0:e052602db102 | 207 | int main(){ |
taurin | 0:e052602db102 | 208 | init(); |
taurin | 0:e052602db102 | 209 | while(1){ |
taurin | 4:450cafd95ac3 | 210 | while(setTrimPin){ |
taurin | 4:450cafd95ac3 | 211 | setTrim(); |
taurin | 4:450cafd95ac3 | 212 | } |
taurin | 4:450cafd95ac3 | 213 | while(checkMaxDegPin){ |
taurin | 4:450cafd95ac3 | 214 | checkMaxDeg(); |
taurin | 4:450cafd95ac3 | 215 | } |
YusukeWakuta | 14:1f6dd929d7de | 216 | // pc.printf("eT:%f\n\r",eruronTrim); |
YusukeWakuta | 14:1f6dd929d7de | 217 | pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim); |
YusukeWakuta | 14:1f6dd929d7de | 218 | pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg); |
taurin | 4:450cafd95ac3 | 219 | led4 = 0; |
YusukeWakuta | 14:1f6dd929d7de | 220 | |
taurin | 4:450cafd95ac3 | 221 | debugLED = 0; |
taurin | 16:82310bf7c326 | 222 | //receiveDatas(); |
taurin | 1:9cc932a16d17 | 223 | WriteServo(); |
taurin | 4:450cafd95ac3 | 224 | updateDatas(); |
taurin | 2:7fcb4f970a02 | 225 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 226 | } |
taurin | 0:e052602db102 | 227 | } |