![](/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@0:e052602db102, 2016-01-29 (annotated)
- Committer:
- taurin
- Date:
- Fri Jan 29 13:38:29 2016 +0000
- Revision:
- 0:e052602db102
- Child:
- 1:9cc932a16d17
1/29 ????CAN
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 | 0:e052602db102 | 6 | #define INIT_SERVO_PERIOD_MS 20000 |
taurin | 0:e052602db102 | 7 | #define WAIT_LOOP_TIME_MS 10 |
taurin | 0:e052602db102 | 8 | #define CONTROL_VALUES_NUM 2 |
taurin | 0:e052602db102 | 9 | #define TO_SEND_CAN_ID 100 |
taurin | 0:e052602db102 | 10 | CAN can(p30,p29); |
taurin | 0:e052602db102 | 11 | CANMessage recmsg; |
taurin | 0:e052602db102 | 12 | Serial pc(USBTX,USBRX); |
taurin | 0:e052602db102 | 13 | ADXL345_I2C accelerometer(p9, p10); |
taurin | 0:e052602db102 | 14 | I2C ina226_i2c(p28,p27); |
taurin | 0:e052602db102 | 15 | INA226 VCmonitor(ina226_i2c); |
taurin | 0:e052602db102 | 16 | PwmOut servo1(p21); |
taurin | 0:e052602db102 | 17 | PwmOut servo2(p22); |
taurin | 0:e052602db102 | 18 | |
taurin | 0:e052602db102 | 19 | char toSendDatas[TO_SEND_DATAS_NUM]; |
taurin | 0:e052602db102 | 20 | char controlValues[CONTROL_VALUES_NUM]; |
taurin | 0:e052602db102 | 21 | |
taurin | 0:e052602db102 | 22 | int counter = 0; |
taurin | 0:e052602db102 | 23 | int eruron_deg = 0; |
taurin | 0:e052602db102 | 24 | int drug_deg = 0; |
taurin | 0:e052602db102 | 25 | unsigned short ina_val; |
taurin | 0:e052602db102 | 26 | double V,C; |
taurin | 0:e052602db102 | 27 | bool SERVO_FLAG; |
taurin | 0:e052602db102 | 28 | bool ADXL_FLAG; |
taurin | 0:e052602db102 | 29 | bool INA_FLAG; |
taurin | 0:e052602db102 | 30 | |
taurin | 0:e052602db102 | 31 | int acc[3] = {0,0,0}; |
taurin | 0:e052602db102 | 32 | |
taurin | 0:e052602db102 | 33 | bool servoInit(){ |
taurin | 0:e052602db102 | 34 | servo1.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 35 | servo2.period_ms(INIT_SERVO_PERIOD_MS); |
taurin | 0:e052602db102 | 36 | return true; |
taurin | 0:e052602db102 | 37 | } |
taurin | 0:e052602db102 | 38 | |
taurin | 0:e052602db102 | 39 | bool adxlInit(){ |
taurin | 0:e052602db102 | 40 | accelerometer.setPowerControl(0x00); |
taurin | 0:e052602db102 | 41 | accelerometer.setDataFormatControl(0x0B); |
taurin | 0:e052602db102 | 42 | accelerometer.setDataRate(ADXL345_3200HZ); |
taurin | 0:e052602db102 | 43 | accelerometer.setPowerControl(0x08); |
taurin | 0:e052602db102 | 44 | return true; |
taurin | 0:e052602db102 | 45 | } |
taurin | 0:e052602db102 | 46 | |
taurin | 0:e052602db102 | 47 | bool inaInit(){ |
taurin | 0:e052602db102 | 48 | if(!VCmonitor.isExist()){ |
taurin | 0:e052602db102 | 49 | pc.printf("VCmonitor NOT FOUND\n"); |
taurin | 0:e052602db102 | 50 | return false; |
taurin | 0:e052602db102 | 51 | } |
taurin | 0:e052602db102 | 52 | ina_val = 0; |
taurin | 0:e052602db102 | 53 | if(VCmonitor.rawRead(0x00,&ina_val) != 0){ |
taurin | 0:e052602db102 | 54 | pc.printf("VCmonitor READ ERROR\n"); |
taurin | 0:e052602db102 | 55 | return false; |
taurin | 0:e052602db102 | 56 | } |
taurin | 0:e052602db102 | 57 | VCmonitor.setCurrentCalibration(); |
taurin | 0:e052602db102 | 58 | return true; |
taurin | 0:e052602db102 | 59 | } |
taurin | 0:e052602db102 | 60 | |
taurin | 0:e052602db102 | 61 | void init(){ |
taurin | 0:e052602db102 | 62 | pc.printf("Receiver\n\r"); |
taurin | 0:e052602db102 | 63 | SERVO_FLAG = servoInit(); |
taurin | 0:e052602db102 | 64 | ADXL_FLAG = adxlInit(); |
taurin | 0:e052602db102 | 65 | INA_FLAG = inaInit(); |
taurin | 0:e052602db102 | 66 | } |
taurin | 0:e052602db102 | 67 | |
taurin | 0:e052602db102 | 68 | void updateDatas(){ |
taurin | 0:e052602db102 | 69 | if(ADXL_FLAG){ |
taurin | 0:e052602db102 | 70 | accelerometer.getOutput(acc); |
taurin | 0:e052602db102 | 71 | } |
taurin | 0:e052602db102 | 72 | if(INA_FLAG){ |
taurin | 0:e052602db102 | 73 | int tmp = VCmonitor.getVoltage(&V); |
taurin | 0:e052602db102 | 74 | tmp = VCmonitor.getCurrent(&C); |
taurin | 0:e052602db102 | 75 | } |
taurin | 0:e052602db102 | 76 | for(int i = 0; i < 3; i++){ |
taurin | 0:e052602db102 | 77 | toSendDatas[i] = acc[i]; |
taurin | 0:e052602db102 | 78 | } |
taurin | 0:e052602db102 | 79 | toSendDatas[3] = (char)(V/100); |
taurin | 0:e052602db102 | 80 | toSendDatas[4] = (char)(C/0100); |
taurin | 0:e052602db102 | 81 | } |
taurin | 0:e052602db102 | 82 | |
taurin | 0:e052602db102 | 83 | void receiveDatas(){ |
taurin | 0:e052602db102 | 84 | if(can.read(recmsg)){ |
taurin | 0:e052602db102 | 85 | for(int i = 0; i < recmsg.len; i++){ |
taurin | 0:e052602db102 | 86 | controlValues[i] = recmsg.data[i]; |
taurin | 0:e052602db102 | 87 | } |
taurin | 0:e052602db102 | 88 | } |
taurin | 0:e052602db102 | 89 | } |
taurin | 0:e052602db102 | 90 | |
taurin | 0:e052602db102 | 91 | void toString(){ |
taurin | 0:e052602db102 | 92 | //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){ |
taurin | 0:e052602db102 | 93 | pc.printf("%i:",toSendDatas[3]); |
taurin | 0:e052602db102 | 94 | pc.printf("%i:",toSendDatas[4]); |
taurin | 0:e052602db102 | 95 | pc.printf("%f:",V); |
taurin | 0:e052602db102 | 96 | pc.printf("%f:",C); |
taurin | 0:e052602db102 | 97 | //} |
taurin | 0:e052602db102 | 98 | //for(int i = 0; i <CONTROL_VALUES_NUM; i++){ |
taurin | 0:e052602db102 | 99 | // pc.printf("%i:",controlValues[i]); |
taurin | 0:e052602db102 | 100 | // } |
taurin | 0:e052602db102 | 101 | // pc.printf("%d",INA_FLAG); |
taurin | 0:e052602db102 | 102 | pc.printf("\n\r"); |
taurin | 0:e052602db102 | 103 | } |
taurin | 0:e052602db102 | 104 | |
taurin | 0:e052602db102 | 105 | void sendDatas(){ |
taurin | 0:e052602db102 | 106 | if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) |
taurin | 0:e052602db102 | 107 | pc.printf("resend suc\n\r"); |
taurin | 0:e052602db102 | 108 | } |
taurin | 0:e052602db102 | 109 | |
taurin | 0:e052602db102 | 110 | void |
taurin | 0:e052602db102 | 111 | |
taurin | 0:e052602db102 | 112 | int main(){ |
taurin | 0:e052602db102 | 113 | init(); |
taurin | 0:e052602db102 | 114 | while(1){ |
taurin | 0:e052602db102 | 115 | receiveDatas(); |
taurin | 0:e052602db102 | 116 | updateDatas(); |
taurin | 0:e052602db102 | 117 | //WriteServo(); |
taurin | 0:e052602db102 | 118 | toString(); |
taurin | 0:e052602db102 | 119 | sendDatas(); |
taurin | 0:e052602db102 | 120 | wait_ms(WAIT_LOOP_TIME_MS); |
taurin | 0:e052602db102 | 121 | } |
taurin | 0:e052602db102 | 122 | } |