albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

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?

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