albatross / Mbed 2 deprecated Souda_Yokutan_ver4_3

＋３度のやつ

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of Souda_Yokutan_ver4 by albatross

main.cpp@2:7fcb4f970a02, 2016-02-16 (annotated)

Committer:
taurin
Date:
Tue Feb 16 09:49:50 2016 +0000
Revision:
2:7fcb4f970a02
Parent:
1:9cc932a16d17
Child:
3:4417217b4f66
2?16?

Who changed what in which revision?

UserRevisionLine numberNew 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
taurin 2:7fcb4f970a02 7 #define WAIT_LOOP_TIME 1
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);
taurin 0:e052602db102 17 PwmOut servo1(p21);
taurin 0:e052602db102 18 PwmOut servo2(p22);
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(){
taurin 0:e052602db102 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];
taurin 0:e052602db102 82 }
taurin 0:e052602db102 83 toSendDatas[3] = (char)(V/100);
taurin 0:e052602db102 84 toSendDatas[4] = (char)(C/0100);
taurin 0:e052602db102 85 }
taurin 0:e052602db102 86
taurin 0:e052602db102 87 void receiveDatas(){
taurin 0:e052602db102 88 if(can.read(recmsg)){
taurin 2:7fcb4f970a02 89 for(int i = 0; i < CONTROL_VALUES_NUM; i++){
taurin 2:7fcb4f970a02 90 //controlValues[i] = recmsg.data[i];
taurin 2:7fcb4f970a02 91 controlValues[i] = 1 - controlValues[i];
taurin 2:7fcb4f970a02 92 //pc.printf("%d:%d ",i,recmsg.data[i]);
taurin 0:e052602db102 93 }
taurin 2:7fcb4f970a02 94 pc.printf("\n\r");
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]);
taurin 2:7fcb4f970a02 103 // pc.printf("%f:",V);
taurin 2:7fcb4f970a02 104 // pc.printf("%f:",C);
taurin 2:7fcb4f970a02 105 // //}
taurin 2:7fcb4f970a02 106 for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin 2:7fcb4f970a02 107 pc.printf("%d:%d, ",i,controlValues[i]);
taurin 2:7fcb4f970a02 108 }
taurin 2:7fcb4f970a02 109 pc.printf("%d",INA_FLAG);
taurin 0:e052602db102 110 pc.printf("\n\r");
taurin 0:e052602db102 111 }
taurin 0:e052602db102 112
taurin 0:e052602db102 113 void sendDatas(){
taurin 0:e052602db102 114 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
taurin 0:e052602db102 115 pc.printf("resend suc\n\r");
taurin 0:e052602db102 116 }
taurin 0:e052602db102 117
taurin 1:9cc932a16d17 118 double calcPulse(int deg){
taurin 1:9cc932a16d17 119 return (0.00093+(deg/180.0)*(0.00235-0.00077));
taurin 1:9cc932a16d17 120 }
taurin 1:9cc932a16d17 121
taurin 1:9cc932a16d17 122 void WriteServo(){
taurin 2:7fcb4f970a02 123 //if(controlValues[0] == 1){
taurin 2:7fcb4f970a02 124 // servo1.pulsewidth(calcPulse(90));
taurin 2:7fcb4f970a02 125 // }
taurin 2:7fcb4f970a02 126 // else{
taurin 2:7fcb4f970a02 127 // servo1.pulsewidth(calcPulse(45));
taurin 2:7fcb4f970a02 128 // }
taurin 2:7fcb4f970a02 129 // if(controlValues[1] == 1){
taurin 2:7fcb4f970a02 130 // servo1.pulsewidth(calcPulse(90));
taurin 2:7fcb4f970a02 131 // }
taurin 2:7fcb4f970a02 132 // else{
taurin 2:7fcb4f970a02 133 // servo1.pulsewidth(calcPulse(45));
taurin 2:7fcb4f970a02 134 // }
taurin 2:7fcb4f970a02 135 servo1.pulsewidth(calcPulse(a.read()*170));
taurin 2:7fcb4f970a02 136 servo2.pulsewidth(calcPulse(b.read()*170));
taurin 2:7fcb4f970a02 137 pc.printf("%f", a.read());
taurin 1:9cc932a16d17 138 }
taurin 0:e052602db102 139
taurin 0:e052602db102 140 int main(){
taurin 0:e052602db102 141 init();
taurin 0:e052602db102 142 while(1){
taurin 0:e052602db102 143 receiveDatas();
taurin 0:e052602db102 144 updateDatas();
taurin 1:9cc932a16d17 145 WriteServo();
taurin 0:e052602db102 146 toString();
taurin 0:e052602db102 147 sendDatas();
taurin 2:7fcb4f970a02 148 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 149 }
taurin 0:e052602db102 150 }