albatross / Mbed 2 deprecated Control_Yokutan_CAN_ver1

1/29 操舵翼端CAN

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

main.cpp@0:e052602db102, 2016-01-29 (annotated)

Committer:
taurin
Date:
Fri Jan 29 13:38:29 2016 +0000
Revision:
0:e052602db102
1/29 ????CAN

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 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 }