albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@11:44fea1d768b5, 2016-02-25 (annotated)

Committer:
taurin
Date:
Thu Feb 25 09:44:58 2016 +0000
Revision:
11:44fea1d768b5
Parent:
10:439e73c9a207
Child:
12:fd9d241843f4
2/25 ??????ver1;

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 4:450cafd95ac3 5
taurin 4:450cafd95ac3 6 #define TO_SEND_DATAS_NUM 4
taurin 2:7fcb4f970a02 7 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 8 #define WAIT_LOOP_TIME 0.02
taurin 0:e052602db102 9 #define CONTROL_VALUES_NUM 2
taurin 0:e052602db102 10 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 11 #define ADXL_MEAN_NUM 10
taurin 4:450cafd95ac3 12 #define SEND_DATAS_LOOP_TIME 0.1
taurin 10:439e73c9a207 13 #define ERURON_MOVE_DEG_INI 10
taurin 10:439e73c9a207 14 #define DRUG_MOVE_DEG_INI -92
taurin 10:439e73c9a207 15 #define ERURON_TRIM_INI 96
taurin 10:439e73c9a207 16 #define DRUG_TRIM_INI 109
taurin 4:450cafd95ac3 17 #define start 510
taurin 4:450cafd95ac3 18 #define end 2390
taurin 2:7fcb4f970a02 19
taurin 0:e052602db102 20 CAN can(p30,p29);
taurin 0:e052602db102 21 CANMessage recmsg;
taurin 0:e052602db102 22 Serial pc(USBTX,USBRX);
taurin 0:e052602db102 23 ADXL345_I2C accelerometer(p9, p10);
taurin 0:e052602db102 24 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 25 INA226 VCmonitor(ina226_i2c);
taurin 11:44fea1d768b5 26 PwmOut drugServo(p23);
taurin 11:44fea1d768b5 27 PwmOut eruronServo(p22);
taurin 1:9cc932a16d17 28 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 29 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 30 AnalogIn eruronAna(p19);
taurin 4:450cafd95ac3 31 DigitalIn setTrimPin(p15);
taurin 4:450cafd95ac3 32 DigitalIn debugServoPin(p16);
taurin 4:450cafd95ac3 33 DigitalIn checkMaxDegPin(p17);
taurin 4:450cafd95ac3 34 DigitalOut debugLED(LED2);
taurin 4:450cafd95ac3 35 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 36 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 37 Ticker sendDatasTicker;
taurin 4:450cafd95ac3 38 Ticker toStringTicker;
taurin 0:e052602db102 39
taurin 0:e052602db102 40 char toSendDatas[TO_SEND_DATAS_NUM];
taurin 1:9cc932a16d17 41 char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
taurin 0:e052602db102 42
taurin 10:439e73c9a207 43 float eruronTrim = ERURON_TRIM_INI;
taurin 10:439e73c9a207 44 float drugTrim = DRUG_TRIM_INI;
taurin 4:450cafd95ac3 45 float eruronMoveDeg = ERURON_MOVE_DEG_INI;
taurin 4:450cafd95ac3 46 float drugMoveDeg = DRUG_MOVE_DEG_INI;
taurin 0:e052602db102 47 unsigned short ina_val;
taurin 0:e052602db102 48 double V,C;
taurin 0:e052602db102 49 bool SERVO_FLAG;
taurin 0:e052602db102 50 bool ADXL_FLAG;
taurin 0:e052602db102 51 bool INA_FLAG;
taurin 0:e052602db102 52
taurin 0:e052602db102 53 int acc[3] = {0,0,0};
taurin 4:450cafd95ac3 54 char acc_mean[3][ADXL_MEAN_NUM];
taurin 4:450cafd95ac3 55 int adxl_mean_counter = 0;
taurin 4:450cafd95ac3 56
taurin 4:450cafd95ac3 57 void toString();
taurin 0:e052602db102 58
taurin 0:e052602db102 59 bool servoInit(){
taurin 4:450cafd95ac3 60 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 4:450cafd95ac3 61 eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 62 return true;
taurin 0:e052602db102 63 }
taurin 0:e052602db102 64
taurin 0:e052602db102 65 bool adxlInit(){
taurin 0:e052602db102 66 accelerometer.setPowerControl(0x00);
taurin 0:e052602db102 67 accelerometer.setDataFormatControl(0x0B);
taurin 0:e052602db102 68 accelerometer.setDataRate(ADXL345_3200HZ);
taurin 0:e052602db102 69 accelerometer.setPowerControl(0x08);
taurin 0:e052602db102 70 return true;
taurin 0:e052602db102 71 }
taurin 0:e052602db102 72
taurin 4:450cafd95ac3 73 void sendDatas(){
taurin 4:450cafd95ac3 74 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
taurin 4:450cafd95ac3 75 }
taurin 4:450cafd95ac3 76 }
taurin 4:450cafd95ac3 77
taurin 0:e052602db102 78 bool inaInit(){
taurin 0:e052602db102 79 if(!VCmonitor.isExist()){
taurin 0:e052602db102 80 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 81 return false;
taurin 0:e052602db102 82 }
taurin 0:e052602db102 83 ina_val = 0;
taurin 0:e052602db102 84 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 0:e052602db102 85 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 86 return false;
taurin 0:e052602db102 87 }
taurin 0:e052602db102 88 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 89 return true;
taurin 0:e052602db102 90 }
taurin 0:e052602db102 91
taurin 0:e052602db102 92 void init(){
taurin 0:e052602db102 93 SERVO_FLAG = servoInit();
taurin 0:e052602db102 94 ADXL_FLAG = adxlInit();
taurin 0:e052602db102 95 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 96 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 4:450cafd95ac3 97 // toStringTicker.attach(&toString,0.5);
taurin 0:e052602db102 98 }
taurin 0:e052602db102 99
taurin 0:e052602db102 100 void updateDatas(){
taurin 0:e052602db102 101 if(ADXL_FLAG){
taurin 0:e052602db102 102 accelerometer.getOutput(acc);
taurin 0:e052602db102 103 }
taurin 0:e052602db102 104 if(INA_FLAG){
taurin 0:e052602db102 105 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 106 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 107 }
taurin 0:e052602db102 108 for(int i = 0; i < 3; i++){
taurin 0:e052602db102 109 toSendDatas[i] = acc[i];
taurin 0:e052602db102 110 }
taurin 0:e052602db102 111 toSendDatas[3] = (char)(V/100);
taurin 0:e052602db102 112 }
taurin 0:e052602db102 113
taurin 0:e052602db102 114 void receiveDatas(){
taurin 0:e052602db102 115 if(can.read(recmsg)){
taurin 2:7fcb4f970a02 116 for(int i = 0; i < CONTROL_VALUES_NUM; i++){
YusukeWakuta 3:4417217b4f66 117 controlValues[i] = recmsg.data[i];
taurin 0:e052602db102 118 }
taurin 1:9cc932a16d17 119 led1 = !led1;
taurin 0:e052602db102 120 }
taurin 0:e052602db102 121 }
taurin 0:e052602db102 122
taurin 0:e052602db102 123 void toString(){
taurin 4:450cafd95ac3 124 for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin 4:450cafd95ac3 125 pc.printf("%d, ",controlValues[i]);
taurin 4:450cafd95ac3 126 }
taurin 4:450cafd95ac3 127 pc.printf("\n\r");
taurin 0:e052602db102 128 }
taurin 0:e052602db102 129
taurin 1:9cc932a16d17 130 double calcPulse(int deg){
taurin 4:450cafd95ac3 131 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:450cafd95ac3 132
taurin 1:9cc932a16d17 133 }
taurin 1:9cc932a16d17 134
taurin 1:9cc932a16d17 135 void WriteServo(){
taurin 4:450cafd95ac3 136 if(debugServoPin){
taurin 4:450cafd95ac3 137 led3 = 1;
taurin 4:450cafd95ac3 138 float a = eruronAna.read()*180;
taurin 4:450cafd95ac3 139 float b = drugAna.read()*180;
taurin 4:450cafd95ac3 140 eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
taurin 4:450cafd95ac3 141 drugServo.pulsewidth(calcPulse(drugAna.read()*180));
taurin 10:439e73c9a207 142 pc.printf("eruron:%f drug:%f\n\r",a,b);
taurin 4:450cafd95ac3 143 }
YusukeWakuta 3:4417217b4f66 144 else{
taurin 4:450cafd95ac3 145 led3 = 0;
taurin 4:450cafd95ac3 146 eruronServo.pulsewidth(calcPulse(eruronTrim+eruronMoveDeg*(controlValues[0]-1)));
taurin 4:450cafd95ac3 147 drugServo.pulsewidth(calcPulse(drugTrim+drugMoveDeg*controlValues[1]));
YusukeWakuta 3:4417217b4f66 148 }
taurin 4:450cafd95ac3 149 }
taurin 4:450cafd95ac3 150
taurin 4:450cafd95ac3 151 void setTrim(){
taurin 4:450cafd95ac3 152 debugLED = 1;
taurin 4:450cafd95ac3 153 eruronTrim = eruronAna.read()*180;
taurin 4:450cafd95ac3 154 drugTrim = drugAna.read()*180;
taurin 4:450cafd95ac3 155 eruronServo.pulsewidth(calcPulse(eruronTrim));
taurin 4:450cafd95ac3 156 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 4:450cafd95ac3 157 pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
taurin 4:450cafd95ac3 158 }
taurin 4:450cafd95ac3 159
taurin 4:450cafd95ac3 160 void checkMaxDeg(){
taurin 4:450cafd95ac3 161 led4 = 1;
taurin 4:450cafd95ac3 162 float eruronTemp = eruronAna.read()*180;
taurin 4:450cafd95ac3 163 float drugTemp = drugAna.read()*180;
taurin 4:450cafd95ac3 164 eruronServo.pulsewidth(calcPulse(eruronTemp));
taurin 4:450cafd95ac3 165 drugServo.pulsewidth(calcPulse(drugTemp));
taurin 4:450cafd95ac3 166 eruronMoveDeg = eruronTemp-eruronTrim;
taurin 4:450cafd95ac3 167 drugMoveDeg = drugTemp-drugTrim;
taurin 10:439e73c9a207 168 pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
taurin 4:450cafd95ac3 169 wait_us(10);
taurin 1:9cc932a16d17 170 }
taurin 0:e052602db102 171
taurin 0:e052602db102 172 int main(){
taurin 0:e052602db102 173 init();
taurin 0:e052602db102 174 while(1){
taurin 4:450cafd95ac3 175 while(setTrimPin){
taurin 4:450cafd95ac3 176 setTrim();
taurin 4:450cafd95ac3 177 }
taurin 4:450cafd95ac3 178 while(checkMaxDegPin){
taurin 4:450cafd95ac3 179 checkMaxDeg();
taurin 4:450cafd95ac3 180 }
taurin 4:450cafd95ac3 181 led4 = 0;
taurin 4:450cafd95ac3 182 debugLED = 0;
taurin 0:e052602db102 183 receiveDatas();
taurin 1:9cc932a16d17 184 WriteServo();
taurin 4:450cafd95ac3 185 updateDatas();
taurin 2:7fcb4f970a02 186 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 187 }
taurin 0:e052602db102 188 }