5月28日のTF成功したやつ
Dependencies: ADXL345_I2C Control_Yokutan_CANver1 mbed
Fork of Souda_Yokutan_ver4_6 by
main.cpp@3:4417217b4f66, 2016-02-17 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Feb 17 01:56:21 2016 +0000
- Revision:
- 3:4417217b4f66
- Parent:
- 2:7fcb4f970a02
- Child:
- 4:450cafd95ac3
- Child:
- 5:7459a428e16e
- Child:
- 6:a839ccbf7f9a
????????????????INA??????????????
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 | 0:e052602db102 | 5 | #define TO_SEND_DATAS_NUM 5 |
taurin | 2:7fcb4f970a02 | 6 | #define INIT_SERVO_PERIOD_MS 20 |
YusukeWakuta | 3:4417217b4f66 | 7 | #define WAIT_LOOP_TIME 0.2 |
taurin | 0:e052602db102 | 8 | #define CONTROL_VALUES_NUM 2 |
taurin | 0:e052602db102 | 9 | #define TO_SEND_CAN_ID 100 |
taurin | 2:7fcb4f970a02 | 10 | |
taurin | 0:e052602db102 | 11 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 12 | CANMessage recmsg; |
taurin | 0:e052602db102 | 13 | Serial pc(USBTX,USBRX); |
taurin | 0:e052602db102 | 14 | ADXL345_I2C accelerometer(p9, p10); |
taurin | 0:e052602db102 | 15 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 16 | INA226 VCmonitor(ina226_i2c); |
YusukeWakuta | 3:4417217b4f66 | 17 | PwmOut servo1(p22); |
YusukeWakuta | 3:4417217b4f66 | 18 | PwmOut servo2(p23); |
taurin | 1:9cc932a16d17 | 19 | DigitalOut led1(LED1); |
taurin | 2:7fcb4f970a02 | 20 | AnalogIn a(p20); |
taurin | 2:7fcb4f970a02 | 21 | AnalogIn b(p19); |
taurin | 0:e052602db102 | 22 | |
taurin | 0:e052602db102 | 23 | char toSendDatas[TO_SEND_DATAS_NUM]; |
taurin | 1:9cc932a16d17 | 24 | char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug |
taurin | 0:e052602db102 | 25 | |
taurin | 0:e052602db102 | 26 | int counter = 0; |
taurin | 0:e052602db102 | 27 | int eruron_deg = 0; |
taurin | 0:e052602db102 | 28 | int drug_deg = 0; |
taurin | 0:e052602db102 | 29 | unsigned short ina_val; |
taurin | 0:e052602db102 | 30 | double V,C; |
taurin | 0:e052602db102 | 31 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 32 | bool ADXL_FLAG; |
taurin | 0:e052602db102 | 33 | bool INA_FLAG; |
taurin | 0:e052602db102 | 34 | |
taurin | 0:e052602db102 | 35 | int acc[3] = {0,0,0}; |
taurin | 0:e052602db102 | 36 | |
taurin | 0:e052602db102 | 37 | bool servoInit(){ |
taurin | 0:e052602db102 | 38 | servo1.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 39 | servo2.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 40 | return true; |
taurin | 0:e052602db102 | 41 | } |
taurin | 0:e052602db102 | 42 | |
taurin | 0:e052602db102 | 43 | bool adxlInit(){ |
taurin | 0:e052602db102 | 44 | accelerometer.setPowerControl(0x00); |
taurin | 0:e052602db102 | 45 | accelerometer.setDataFormatControl(0x0B); |
taurin | 0:e052602db102 | 46 | accelerometer.setDataRate(ADXL345_3200HZ); |
taurin | 0:e052602db102 | 47 | accelerometer.setPowerControl(0x08); |
taurin | 0:e052602db102 | 48 | return true; |
taurin | 0:e052602db102 | 49 | } |
taurin | 0:e052602db102 | 50 | |
taurin | 0:e052602db102 | 51 | bool inaInit(){ |
taurin | 0:e052602db102 | 52 | if(!VCmonitor.isExist()){ |
taurin | 0:e052602db102 | 53 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 54 | return false; |
taurin | 0:e052602db102 | 55 | } |
taurin | 0:e052602db102 | 56 | ina_val = 0; |
taurin | 0:e052602db102 | 57 | if(VCmonitor.rawRead(0x00,&ina_val) != 0){ |
taurin | 0:e052602db102 | 58 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 59 | return false; |
taurin | 0:e052602db102 | 60 | } |
taurin | 0:e052602db102 | 61 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 62 | return true; |
taurin | 0:e052602db102 | 63 | } |
taurin | 0:e052602db102 | 64 | |
taurin | 0:e052602db102 | 65 | void init(){ |
YusukeWakuta | 3:4417217b4f66 | 66 | // pc.printf("Receiver\n\r"); |
taurin | 0:e052602db102 | 67 | SERVO_FLAG = servoInit(); |
taurin | 0:e052602db102 | 68 | ADXL_FLAG = adxlInit(); |
taurin | 0:e052602db102 | 69 | INA_FLAG = inaInit(); |
taurin | 0:e052602db102 | 70 | } |
taurin | 0:e052602db102 | 71 | |
taurin | 0:e052602db102 | 72 | void updateDatas(){ |
taurin | 0:e052602db102 | 73 | if(ADXL_FLAG){ |
taurin | 0:e052602db102 | 74 | accelerometer.getOutput(acc); |
taurin | 0:e052602db102 | 75 | } |
taurin | 0:e052602db102 | 76 | if(INA_FLAG){ |
taurin | 0:e052602db102 | 77 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 78 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 79 | } |
taurin | 0:e052602db102 | 80 | for(int i = 0; i < 3; i++){ |
taurin | 0:e052602db102 | 81 | toSendDatas[i] = acc[i]; |
YusukeWakuta | 3:4417217b4f66 | 82 | //pc.printf("%d\t",acc[i]); |
taurin | 0:e052602db102 | 83 | } |
YusukeWakuta | 3:4417217b4f66 | 84 | //pc.printf("\n"); |
taurin | 0:e052602db102 | 85 | toSendDatas[3] = (char)(V/100); |
taurin | 0:e052602db102 | 86 | toSendDatas[4] = (char)(C/0100); |
taurin | 0:e052602db102 | 87 | } |
taurin | 0:e052602db102 | 88 | |
taurin | 0:e052602db102 | 89 | void receiveDatas(){ |
taurin | 0:e052602db102 | 90 | if(can.read(recmsg)){ |
taurin | 2:7fcb4f970a02 | 91 | for(int i = 0; i < CONTROL_VALUES_NUM; i++){ |
YusukeWakuta | 3:4417217b4f66 | 92 | controlValues[i] = recmsg.data[i]; |
YusukeWakuta | 3:4417217b4f66 | 93 | // pc.printf("%d:%d ",i,recmsg.data[i]); |
taurin | 0:e052602db102 | 94 | } |
taurin | 1:9cc932a16d17 | 95 | led1 = !led1; |
taurin | 0:e052602db102 | 96 | } |
taurin | 0:e052602db102 | 97 | } |
taurin | 0:e052602db102 | 98 | |
taurin | 0:e052602db102 | 99 | void toString(){ |
taurin | 0:e052602db102 | 100 | //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){ |
taurin | 2:7fcb4f970a02 | 101 | // pc.printf("%i:",toSendDatas[3]); |
taurin | 2:7fcb4f970a02 | 102 | // pc.printf("%i:",toSendDatas[4]); |
YusukeWakuta | 3:4417217b4f66 | 103 | pc.printf("%f:",V); |
YusukeWakuta | 3:4417217b4f66 | 104 | pc.printf("%f:",C); |
taurin | 2:7fcb4f970a02 | 105 | // //} |
YusukeWakuta | 3:4417217b4f66 | 106 | //for(int i = 0; i <CONTROL_VALUES_NUM; i++){ |
YusukeWakuta | 3:4417217b4f66 | 107 | // // pc.printf("%d, ",controlValues[i]); |
YusukeWakuta | 3:4417217b4f66 | 108 | // |
YusukeWakuta | 3:4417217b4f66 | 109 | // } |
YusukeWakuta | 3:4417217b4f66 | 110 | //pc.printf("\n\r"); |
YusukeWakuta | 3:4417217b4f66 | 111 | // pc.printf("%f",a.read()); |
YusukeWakuta | 3:4417217b4f66 | 112 | //pc.printf("\n\r"); |
taurin | 0:e052602db102 | 113 | } |
taurin | 0:e052602db102 | 114 | |
taurin | 0:e052602db102 | 115 | void sendDatas(){ |
YusukeWakuta | 3:4417217b4f66 | 116 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){ |
YusukeWakuta | 3:4417217b4f66 | 117 | // |
YusukeWakuta | 3:4417217b4f66 | 118 | // pc.printf("resend suc\n\r"); |
taurin | 0:e052602db102 | 119 | } |
YusukeWakuta | 3:4417217b4f66 | 120 | } |
taurin | 1:9cc932a16d17 | 121 | double calcPulse(int deg){ |
taurin | 1:9cc932a16d17 | 122 | return (0.00093+(deg/180.0)*(0.00235-0.00077)); |
taurin | 1:9cc932a16d17 | 123 | } |
taurin | 1:9cc932a16d17 | 124 | |
taurin | 1:9cc932a16d17 | 125 | void WriteServo(){ |
YusukeWakuta | 3:4417217b4f66 | 126 | if(controlValues[0] == (char)1){ |
YusukeWakuta | 3:4417217b4f66 | 127 | servo1.pulsewidth(calcPulse(90)); |
YusukeWakuta | 3:4417217b4f66 | 128 | } |
YusukeWakuta | 3:4417217b4f66 | 129 | else{ |
YusukeWakuta | 3:4417217b4f66 | 130 | servo1.pulsewidth(calcPulse(45)); |
YusukeWakuta | 3:4417217b4f66 | 131 | } |
YusukeWakuta | 3:4417217b4f66 | 132 | if(controlValues[1] == (char)1){ |
YusukeWakuta | 3:4417217b4f66 | 133 | servo2.pulsewidth(calcPulse(90)); |
YusukeWakuta | 3:4417217b4f66 | 134 | } |
YusukeWakuta | 3:4417217b4f66 | 135 | else{ |
YusukeWakuta | 3:4417217b4f66 | 136 | servo2.pulsewidth(calcPulse(45)); |
YusukeWakuta | 3:4417217b4f66 | 137 | } |
YusukeWakuta | 3:4417217b4f66 | 138 | //servo1.pulsewidth(calcPulse(a.read()*170)); |
YusukeWakuta | 3:4417217b4f66 | 139 | // servo2.pulsewidth(calcPulse(b.read()*170)); |
YusukeWakuta | 3:4417217b4f66 | 140 | //pc.printf("%f", a.read()); |
taurin | 1:9cc932a16d17 | 141 | } |
taurin | 0:e052602db102 | 142 | |
taurin | 0:e052602db102 | 143 | int main(){ |
taurin | 0:e052602db102 | 144 | init(); |
taurin | 0:e052602db102 | 145 | while(1){ |
taurin | 0:e052602db102 | 146 | receiveDatas(); |
taurin | 1:9cc932a16d17 | 147 | WriteServo(); |
YusukeWakuta | 3:4417217b4f66 | 148 | updateDatas(); |
taurin | 0:e052602db102 | 149 | toString(); |
taurin | 0:e052602db102 | 150 | sendDatas(); |
taurin | 2:7fcb4f970a02 | 151 | wait(WAIT_LOOP_TIME); |
taurin | 0:e052602db102 | 152 | } |
taurin | 0:e052602db102 | 153 | } |