albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@9:57683fc5c110, 2016-02-17 (annotated)

Committer:
YusukeWakuta
Date:
Wed Feb 17 02:53:01 2016 +0000
Revision:
9:57683fc5c110
Parent:
8:a28e4a469b8d 
??????

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 5:7459a428e16e 18 PwmOut servo2(p23);// eruron
taurin 1:9cc932a16d17 19 DigitalOut led1(LED1);
taurin 2:7fcb4f970a02 20 AnalogIn a(p20);
taurin 2:7fcb4f970a02 21 AnalogIn b(p19);
YusukeWakuta 7:d6a3e47e6ef9 22 DigitalIn switch1(p15);
taurin 0:e052602db102 23
taurin 0:e052602db102 24 char toSendDatas[TO_SEND_DATAS_NUM];
taurin 1:9cc932a16d17 25 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
taurin 0:e052602db102 26
taurin 0:e052602db102 27 int counter = 0;
taurin 0:e052602db102 28 int eruron_deg = 0;
taurin 0:e052602db102 29 int drug_deg = 0;
taurin 0:e052602db102 30 unsigned short ina_val;
taurin 0:e052602db102 31 double V,C;
taurin 0:e052602db102 32 bool SERVO_FLAG;
taurin 0:e052602db102 33 bool ADXL_FLAG;
taurin 0:e052602db102 34 bool INA_FLAG;
YusukeWakuta 9:57683fc5c110 35 int neutral_a = 45;
YusukeWakuta 9:57683fc5c110 36 int neutral_b = 45;
taurin 0:e052602db102 37
taurin 0:e052602db102 38 int acc[3] = {0,0,0};
taurin 0:e052602db102 39
taurin 0:e052602db102 40 bool servoInit(){
taurin 0:e052602db102 41 servo1.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 42 servo2.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 43 return true;
taurin 0:e052602db102 44 }
taurin 0:e052602db102 45
taurin 0:e052602db102 46 bool adxlInit(){
taurin 0:e052602db102 47 accelerometer.setPowerControl(0x00);
taurin 0:e052602db102 48 accelerometer.setDataFormatControl(0x0B);
taurin 0:e052602db102 49 accelerometer.setDataRate(ADXL345_3200HZ);
taurin 0:e052602db102 50 accelerometer.setPowerControl(0x08);
taurin 0:e052602db102 51 return true;
taurin 0:e052602db102 52 }
taurin 0:e052602db102 53
taurin 0:e052602db102 54 bool inaInit(){
taurin 0:e052602db102 55 if(!VCmonitor.isExist()){
taurin 0:e052602db102 56 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 57 return false;
taurin 0:e052602db102 58 }
taurin 0:e052602db102 59 ina_val = 0;
taurin 0:e052602db102 60 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 0:e052602db102 61 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 62 return false;
taurin 0:e052602db102 63 }
taurin 0:e052602db102 64 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 65 return true;
taurin 0:e052602db102 66 }
taurin 0:e052602db102 67
taurin 0:e052602db102 68 void init(){
YusukeWakuta 3:4417217b4f66 69 // pc.printf("Receiver\n\r");
taurin 0:e052602db102 70 SERVO_FLAG = servoInit();
taurin 0:e052602db102 71 ADXL_FLAG = adxlInit();
taurin 0:e052602db102 72 INA_FLAG = inaInit();
taurin 0:e052602db102 73 }
taurin 0:e052602db102 74
taurin 0:e052602db102 75 void updateDatas(){
taurin 0:e052602db102 76 if(ADXL_FLAG){
taurin 0:e052602db102 77 accelerometer.getOutput(acc);
taurin 0:e052602db102 78 }
taurin 0:e052602db102 79 if(INA_FLAG){
taurin 0:e052602db102 80 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 81 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 82 }
taurin 0:e052602db102 83 for(int i = 0; i < 3; i++){
taurin 0:e052602db102 84 toSendDatas[i] = acc[i];
YusukeWakuta 3:4417217b4f66 85 //pc.printf("%d\t",acc[i]);
taurin 0:e052602db102 86 }
YusukeWakuta 3:4417217b4f66 87 //pc.printf("\n");
taurin 0:e052602db102 88 toSendDatas[3] = (char)(V/100);
taurin 0:e052602db102 89 toSendDatas[4] = (char)(C/0100);
taurin 0:e052602db102 90 }
taurin 0:e052602db102 91
taurin 0:e052602db102 92 void receiveDatas(){
taurin 0:e052602db102 93 if(can.read(recmsg)){
taurin 2:7fcb4f970a02 94 for(int i = 0; i < CONTROL_VALUES_NUM; i++){
YusukeWakuta 3:4417217b4f66 95 controlValues[i] = recmsg.data[i];
YusukeWakuta 3:4417217b4f66 96 // pc.printf("%d:%d ",i,recmsg.data[i]);
taurin 0:e052602db102 97 }
taurin 1:9cc932a16d17 98 led1 = !led1;
taurin 0:e052602db102 99 }
taurin 0:e052602db102 100 }
taurin 0:e052602db102 101
taurin 0:e052602db102 102 void toString(){
taurin 0:e052602db102 103 //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
taurin 2:7fcb4f970a02 104 // pc.printf("%i:",toSendDatas[3]);
taurin 2:7fcb4f970a02 105 // pc.printf("%i:",toSendDatas[4]);
YusukeWakuta 3:4417217b4f66 106 pc.printf("%f:",V);
YusukeWakuta 3:4417217b4f66 107 pc.printf("%f:",C);
taurin 2:7fcb4f970a02 108 // //}
YusukeWakuta 3:4417217b4f66 109 //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
YusukeWakuta 3:4417217b4f66 110 // // pc.printf("%d, ",controlValues[i]);
YusukeWakuta 3:4417217b4f66 111 //
YusukeWakuta 3:4417217b4f66 112 // }
YusukeWakuta 3:4417217b4f66 113 //pc.printf("\n\r");
YusukeWakuta 3:4417217b4f66 114 // pc.printf("%f",a.read());
YusukeWakuta 3:4417217b4f66 115 //pc.printf("\n\r");
taurin 0:e052602db102 116 }
taurin 0:e052602db102 117
taurin 0:e052602db102 118 void sendDatas(){
YusukeWakuta 3:4417217b4f66 119 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
YusukeWakuta 3:4417217b4f66 120 //
YusukeWakuta 3:4417217b4f66 121 // pc.printf("resend suc\n\r");
taurin 0:e052602db102 122 }
YusukeWakuta 3:4417217b4f66 123 }
taurin 1:9cc932a16d17 124 double calcPulse(int deg){
taurin 1:9cc932a16d17 125 return (0.00093+(deg/180.0)*(0.00235-0.00077));
taurin 1:9cc932a16d17 126 }
taurin 1:9cc932a16d17 127
YusukeWakuta 5:7459a428e16e 128 void WriteServo(int a,int b){
YusukeWakuta 7:d6a3e47e6ef9 129 if(controlValues[0] == (char)1)
YusukeWakuta 7:d6a3e47e6ef9 130 servo1.pulsewidth(calcPulse(90));
YusukeWakuta 3:4417217b4f66 131 else{
YusukeWakuta 5:7459a428e16e 132 servo1.pulsewidth(calcPulse(a));
YusukeWakuta 3:4417217b4f66 133 }
YusukeWakuta 3:4417217b4f66 134 if(controlValues[1] == (char)1){
YusukeWakuta 3:4417217b4f66 135 servo2.pulsewidth(calcPulse(90));
YusukeWakuta 3:4417217b4f66 136 }
YusukeWakuta 3:4417217b4f66 137 else{
YusukeWakuta 5:7459a428e16e 138 servo2.pulsewidth(calcPulse(b));
YusukeWakuta 3:4417217b4f66 139 //servo1.pulsewidth(calcPulse(a.read()*170));
YusukeWakuta 3:4417217b4f66 140 // servo2.pulsewidth(calcPulse(b.read()*170));
YusukeWakuta 3:4417217b4f66 141 //pc.printf("%f", a.read());
taurin 1:9cc932a16d17 142 }
YusukeWakuta 7:d6a3e47e6ef9 143 }
YusukeWakuta 5:7459a428e16e 144
YusukeWakuta 5:7459a428e16e 145 //初期迎角を変える
YusukeWakuta 5:7459a428e16e 146 void Debug(){
YusukeWakuta 5:7459a428e16e 147 if(switch1 == 1){
YusukeWakuta 8:a28e4a469b8d 148 neutral_a = (int)(a * 90);
YusukeWakuta 8:a28e4a469b8d 149 neutral_b = (int)(b * 90);
YusukeWakuta 7:d6a3e47e6ef9 150 WriteServo(neutral_a,neutral_b);
YusukeWakuta 7:d6a3e47e6ef9 151
YusukeWakuta 5:7459a428e16e 152 }
YusukeWakuta 5:7459a428e16e 153 }
YusukeWakuta 5:7459a428e16e 154
taurin 0:e052602db102 155 int main(){
taurin 0:e052602db102 156 init();
taurin 0:e052602db102 157 while(1){
taurin 0:e052602db102 158 receiveDatas();
YusukeWakuta 5:7459a428e16e 159 WriteServo(neutral_a,neutral_b);
YusukeWakuta 3:4417217b4f66 160 updateDatas();
taurin 0:e052602db102 161 toString();
taurin 0:e052602db102 162 sendDatas();
YusukeWakuta 5:7459a428e16e 163 Debug();
taurin 2:7fcb4f970a02 164 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 165 }
taurin 0:e052602db102 166 }