albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@1:9cc932a16d17, 2016-02-13 (annotated)

Committer:
taurin
Date:
Sat Feb 13 11:35:50 2016 +0000
Revision:
1:9cc932a16d17
Parent:
0:e052602db102
Child:
2:7fcb4f970a02
2/13

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 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 1:9cc932a16d17 18 DigitalOut led1(LED1);
taurin 0:e052602db102 19
taurin 0:e052602db102 20 char toSendDatas[TO_SEND_DATAS_NUM];
taurin 1:9cc932a16d17 21 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
taurin 0:e052602db102 22
taurin 0:e052602db102 23 int counter = 0;
taurin 0:e052602db102 24 int eruron_deg = 0;
taurin 0:e052602db102 25 int drug_deg = 0;
taurin 0:e052602db102 26 unsigned short ina_val;
taurin 0:e052602db102 27 double V,C;
taurin 0:e052602db102 28 bool SERVO_FLAG;
taurin 0:e052602db102 29 bool ADXL_FLAG;
taurin 0:e052602db102 30 bool INA_FLAG;
taurin 0:e052602db102 31
taurin 0:e052602db102 32 int acc[3] = {0,0,0};
taurin 0:e052602db102 33
taurin 0:e052602db102 34 bool servoInit(){
taurin 0:e052602db102 35 servo1.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 36 servo2.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 37 return true;
taurin 0:e052602db102 38 }
taurin 0:e052602db102 39
taurin 0:e052602db102 40 bool adxlInit(){
taurin 0:e052602db102 41 accelerometer.setPowerControl(0x00);
taurin 0:e052602db102 42 accelerometer.setDataFormatControl(0x0B);
taurin 0:e052602db102 43 accelerometer.setDataRate(ADXL345_3200HZ);
taurin 0:e052602db102 44 accelerometer.setPowerControl(0x08);
taurin 0:e052602db102 45 return true;
taurin 0:e052602db102 46 }
taurin 0:e052602db102 47
taurin 0:e052602db102 48 bool inaInit(){
taurin 0:e052602db102 49 if(!VCmonitor.isExist()){
taurin 0:e052602db102 50 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 51 return false;
taurin 0:e052602db102 52 }
taurin 0:e052602db102 53 ina_val = 0;
taurin 0:e052602db102 54 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 0:e052602db102 55 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 56 return false;
taurin 0:e052602db102 57 }
taurin 0:e052602db102 58 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 59 return true;
taurin 0:e052602db102 60 }
taurin 0:e052602db102 61
taurin 0:e052602db102 62 void init(){
taurin 0:e052602db102 63 pc.printf("Receiver\n\r");
taurin 0:e052602db102 64 SERVO_FLAG = servoInit();
taurin 0:e052602db102 65 ADXL_FLAG = adxlInit();
taurin 0:e052602db102 66 INA_FLAG = inaInit();
taurin 0:e052602db102 67 }
taurin 0:e052602db102 68
taurin 0:e052602db102 69 void updateDatas(){
taurin 0:e052602db102 70 if(ADXL_FLAG){
taurin 0:e052602db102 71 accelerometer.getOutput(acc);
taurin 0:e052602db102 72 }
taurin 0:e052602db102 73 if(INA_FLAG){
taurin 0:e052602db102 74 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 75 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 76 }
taurin 0:e052602db102 77 for(int i = 0; i < 3; i++){
taurin 0:e052602db102 78 toSendDatas[i] = acc[i];
taurin 0:e052602db102 79 }
taurin 0:e052602db102 80 toSendDatas[3] = (char)(V/100);
taurin 0:e052602db102 81 toSendDatas[4] = (char)(C/0100);
taurin 0:e052602db102 82 }
taurin 0:e052602db102 83
taurin 0:e052602db102 84 void receiveDatas(){
taurin 0:e052602db102 85 if(can.read(recmsg)){
taurin 0:e052602db102 86 for(int i = 0; i < recmsg.len; i++){
taurin 0:e052602db102 87 controlValues[i] = recmsg.data[i];
taurin 0:e052602db102 88 }
taurin 1:9cc932a16d17 89 led1 = !led1;
taurin 0:e052602db102 90 }
taurin 0:e052602db102 91 }
taurin 0:e052602db102 92
taurin 0:e052602db102 93 void toString(){
taurin 0:e052602db102 94 //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
taurin 0:e052602db102 95 pc.printf("%i:",toSendDatas[3]);
taurin 0:e052602db102 96 pc.printf("%i:",toSendDatas[4]);
taurin 0:e052602db102 97 pc.printf("%f:",V);
taurin 0:e052602db102 98 pc.printf("%f:",C);
taurin 0:e052602db102 99 //}
taurin 0:e052602db102 100 //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin 0:e052602db102 101 // pc.printf("%i:",controlValues[i]);
taurin 0:e052602db102 102 // }
taurin 0:e052602db102 103 // pc.printf("%d",INA_FLAG);
taurin 0:e052602db102 104 pc.printf("\n\r");
taurin 0:e052602db102 105 }
taurin 0:e052602db102 106
taurin 0:e052602db102 107 void sendDatas(){
taurin 0:e052602db102 108 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
taurin 0:e052602db102 109 pc.printf("resend suc\n\r");
taurin 0:e052602db102 110 }
taurin 0:e052602db102 111
taurin 1:9cc932a16d17 112 double calcPulse(int deg){
taurin 1:9cc932a16d17 113 return (0.00093+(deg/180.0)*(0.00235-0.00077));
taurin 1:9cc932a16d17 114 }
taurin 1:9cc932a16d17 115
taurin 1:9cc932a16d17 116 void WriteServo(){
taurin 1:9cc932a16d17 117 if(controlValues[0]){
taurin 1:9cc932a16d17 118 servo1.pulsewidth(calcPulse(90));
taurin 1:9cc932a16d17 119 }
taurin 1:9cc932a16d17 120 else{
taurin 1:9cc932a16d17 121 servo1.pulsewidth(calcPulse(45));
taurin 1:9cc932a16d17 122 }
taurin 1:9cc932a16d17 123 }
taurin 0:e052602db102 124
taurin 0:e052602db102 125 int main(){
taurin 0:e052602db102 126 init();
taurin 0:e052602db102 127 while(1){
taurin 0:e052602db102 128 receiveDatas();
taurin 0:e052602db102 129 updateDatas();
taurin 1:9cc932a16d17 130 WriteServo();
taurin 0:e052602db102 131 toString();
taurin 0:e052602db102 132 sendDatas();
taurin 0:e052602db102 133 wait_ms(WAIT_LOOP_TIME_MS);
taurin 0:e052602db102 134 }
taurin 0:e052602db102 135 }