albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@16:82310bf7c326, 2016-03-18 (annotated)

Committer:
taurin
Date:
Fri Mar 18 06:13:47 2016 +0000
Revision:
16:82310bf7c326
Parent:
15:1db5ee4fe7ce 
3/18 ????;

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