albatross / Mbed 2 deprecated Souda_Yokutan_ver3

3/18 翼端操舵

Dependencies:   ADXL345_I2C Control_Yokutan_CANver1 mbed

Fork of ControlYokutan02 by albatross

main.cpp@4:450cafd95ac3, 2016-02-24 (annotated)

Committer:
taurin
Date:
Wed Feb 24 10:21:46 2016 +0000
Revision:
4:450cafd95ac3
Parent:
3:4417217b4f66
Child:
10:439e73c9a207
2/24 ????;

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