albatross / Mbed 2 deprecated ControlYokutan2018

2018年度用翼端mbedプログラム

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@40:ad98da5da7bf, 2017-03-11 (annotated)

Committer:
YusukeWakuta
Date:
Sat Mar 11 22:42:45 2017 +0000
Branch:
XBus???
Revision:
40:ad98da5da7bf
Parent:
39:c6036315831a
Child:
41:efebfdd955ac
Child:
42:bf98a29e81ac
????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 0:e052602db102 1 //翼端can program
taurin 0:e052602db102 2 #include "mbed.h"
tsumagari 22:b38bc18ec3a1 3 #include "MPU6050.h"
taurin 0:e052602db102 4 #include "INA226.hpp"
YusukeWakuta 24:d416722b4aad 5 #include "XBusServo.h"
taurin 4:450cafd95ac3 6
tsumagari 22:b38bc18ec3a1 7 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 8 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 9 #define WAIT_LOOP_TIME 0.02
tsumagari 23:d551db88df65 10 #define CONTROL_VALUES_NUM sizeof(float) + 1
taurin 0:e052602db102 11 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 12 #define SEND_DATAS_LOOP_TIME 0.1
tsumagari 33:d075918d4846 13 #define RECEIVE_DATAS_LOOP_TIME 0.1
taurin 12:fd9d241843f4 14
YusukeWakuta 40:ad98da5da7bf 15 #define ERURON_MOVE_DEG_INI_R 1.0
YusukeWakuta 39:c6036315831a 16 #define DRUG_MOVE_DEG_INI_R 0.32
YusukeWakuta 32:b03557a08efa 17 #define ERURON_TRIM_INI_R 0
YusukeWakuta 39:c6036315831a 18 #define DRUG_TRIM_INI_R 0.62
taurin 12:fd9d241843f4 19
YusukeWakuta 40:ad98da5da7bf 20 #define ERURON_MOVE_DEG_INI_L 1.0
YusukeWakuta 39:c6036315831a 21 #define DRUG_MOVE_DEG_INI_L 0.32
YusukeWakuta 32:b03557a08efa 22 #define ERURON_TRIM_INI_L 0
YusukeWakuta 39:c6036315831a 23 #define DRUG_TRIM_INI_L 0.62
taurin 2:7fcb4f970a02 24
YusukeWakuta 38:b492990e2b56 25 /*ドラッグラダー
YusukeWakuta 37:1f71ca1e5dd1 26 初期値 0.65
YusukeWakuta 37:1f71ca1e5dd1 27 最大角0.99
YusukeWakuta 36:ddf4aa818e88 28 */
YusukeWakuta 24:d416722b4aad 29
taurin 0:e052602db102 30 CAN can(p30,p29);
taurin 0:e052602db102 31 CANMessage recmsg;
taurin 0:e052602db102 32 Serial pc(USBTX,USBRX);
tsumagari 22:b38bc18ec3a1 33 MPU6050 mpu(p9,p10);
taurin 0:e052602db102 34 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 35 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 36 PwmOut drugServo(p22);
tsumagari 33:d075918d4846 37 PwmOut eruronServo(p23);
taurin 1:9cc932a16d17 38 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 39 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 40 AnalogIn eruronAna(p19);
YusukeWakuta 28:99686a3f0e86 41 DigitalIn LRstatePin(p11);
YusukeWakuta 28:99686a3f0e86 42 DigitalIn setTrimPin(p12);
YusukeWakuta 28:99686a3f0e86 43 DigitalIn EDstatePin(p14);
YusukeWakuta 28:99686a3f0e86 44 DigitalIn setMaxDegPin(p15);
taurin 4:450cafd95ac3 45 DigitalOut debugLED(LED2);
taurin 4:450cafd95ac3 46 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 47 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 48 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 49 //Ticker toStringTicker;
taurin 16:82310bf7c326 50 Ticker receiveDatasTicker;
taurin 16:82310bf7c326 51
taurin 0:e052602db102 52 char toSendDatas[TO_SEND_DATAS_NUM];
tsumagari 33:d075918d4846 53 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
tsumagari 23:d551db88df65 54 char floatvalues[sizeof(float)];
taurin 12:fd9d241843f4 55 float eruronTrim;
taurin 12:fd9d241843f4 56 float drugTrim;
taurin 12:fd9d241843f4 57 float eruronMoveDeg;
taurin 12:fd9d241843f4 58 float drugMoveDeg;
tsumagari 23:d551db88df65 59 float eruronfloat;
taurin 0:e052602db102 60 unsigned short ina_val;
taurin 0:e052602db102 61 double V,C;
taurin 0:e052602db102 62 bool SERVO_FLAG;
taurin 0:e052602db102 63 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 64 bool MPU_FLAG;
YusukeWakuta 31:5d22ebe5f705 65 uint16_t XbusValue;
taurin 0:e052602db102 66
tsumagari 22:b38bc18ec3a1 67 char gyro_c[6] = {0,0,0,0,0,0};
taurin 4:450cafd95ac3 68
taurin 4:450cafd95ac3 69 void toString();
taurin 16:82310bf7c326 70 void receiveDatas();
taurin 16:82310bf7c326 71 void WriteServo();
taurin 0:e052602db102 72
YusukeWakuta 24:d416722b4aad 73 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 74
YusukeWakuta 26:f14579683f98 75 bool servoInit()
YusukeWakuta 26:f14579683f98 76 {
taurin 4:450cafd95ac3 77 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 78 return true;
taurin 0:e052602db102 79 }
taurin 0:e052602db102 80
YusukeWakuta 26:f14579683f98 81 void sendDatas()
YusukeWakuta 24:d416722b4aad 82 {
YusukeWakuta 26:f14579683f98 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
YusukeWakuta 26:f14579683f98 87 bool inaInit()
YusukeWakuta 26:f14579683f98 88 {
YusukeWakuta 26:f14579683f98 89 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 90 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 91 return false;
taurin 0:e052602db102 92 }
taurin 0:e052602db102 93 ina_val = 0;
YusukeWakuta 26:f14579683f98 94 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 95 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 96 return false;
taurin 0:e052602db102 97 }
taurin 0:e052602db102 98 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 99 return true;
taurin 0:e052602db102 100 }
taurin 0:e052602db102 101
YusukeWakuta 26:f14579683f98 102 void init()
YusukeWakuta 26:f14579683f98 103 {
YusukeWakuta 26:f14579683f98 104 if(!LRstatePin) {
taurin 13:5e3b4120dbbf 105 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 106 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 107 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 108 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
YusukeWakuta 26:f14579683f98 109 } else {
taurin 12:fd9d241843f4 110 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 111 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 112 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 113 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 114 }
taurin 0:e052602db102 115 SERVO_FLAG = servoInit();
tsumagari 22:b38bc18ec3a1 116 MPU_FLAG = mpu.testConnection();
taurin 0:e052602db102 117 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 118 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 119 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 120 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
YusukeWakuta 24:d416722b4aad 121
YusukeWakuta 36:ddf4aa818e88 122 // initXBus();
taurin 0:e052602db102 123 }
taurin 0:e052602db102 124
YusukeWakuta 26:f14579683f98 125 void updateDatas()
YusukeWakuta 26:f14579683f98 126 {
YusukeWakuta 26:f14579683f98 127 if(INA_FLAG) {
taurin 0:e052602db102 128 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 129 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 130 }
YusukeWakuta 26:f14579683f98 131 if(MPU_FLAG) {
tsumagari 22:b38bc18ec3a1 132 mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin 0:e052602db102 133 }
YusukeWakuta 26:f14579683f98 134 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
tsumagari 22:b38bc18ec3a1 135 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 136 }
tsumagari 22:b38bc18ec3a1 137 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
YusukeWakuta 40:ad98da5da7bf 138 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V;
taurin 0:e052602db102 139 }
taurin 0:e052602db102 140
YusukeWakuta 26:f14579683f98 141 void receiveDatas()
YusukeWakuta 26:f14579683f98 142 {
YusukeWakuta 26:f14579683f98 143 if(can.read(recmsg)) {
YusukeWakuta 26:f14579683f98 144 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 3:4417217b4f66 145 controlValues[i] = recmsg.data[i];
tsumagari 33:d075918d4846 146 if(i<sizeof(float)) floatvalues[i] = controlValues[i];
taurin 0:e052602db102 147 }
tsumagari 23:d551db88df65 148 eruronfloat = *(const float *)floatvalues;
taurin 1:9cc932a16d17 149 led1 = !led1;
taurin 0:e052602db102 150 }
taurin 0:e052602db102 151 }
taurin 0:e052602db102 152
YusukeWakuta 35:707119a675dc 153 double calcPulse(float deg)
YusukeWakuta 26:f14579683f98 154 {
YusukeWakuta 38:b492990e2b56 155 return (0.0006+(deg)*(0.00220-0.00045));
YusukeWakuta 37:1f71ca1e5dd1 156 /*
YusukeWakuta 37:1f71ca1e5dd1 157 int start=510, end=2390;
YusukeWakuta 37:1f71ca1e5dd1 158 while(1) {
YusukeWakuta 37:1f71ca1e5dd1 159 // pc.printf("%f\n\r",(start + (double)(end - start) * analogIn.read()));
YusukeWakuta 37:1f71ca1e5dd1 160 pc.printf("%f\n\r",analogIn.read());
YusukeWakuta 37:1f71ca1e5dd1 161 pwm.pulsewidth_us(start + (double)(end - start) * analogIn.read());
YusukeWakuta 37:1f71ca1e5dd1 162 */
taurin 1:9cc932a16d17 163 }
taurin 1:9cc932a16d17 164
YusukeWakuta 35:707119a675dc 165 float SampleFloat(float f) //小数点以下第二位を切り捨て
YusukeWakuta 26:f14579683f98 166 {
YusukeWakuta 36:ddf4aa818e88 167 int temp = ((f + 0.025) * 100.0) / 5;
YusukeWakuta 35:707119a675dc 168 float result = temp / 20.0;
YusukeWakuta 35:707119a675dc 169 return result;
YusukeWakuta 26:f14579683f98 170 }
YusukeWakuta 26:f14579683f98 171
YusukeWakuta 29:516a5d383488 172 void WriteServo()
YusukeWakuta 26:f14579683f98 173 {
YusukeWakuta 38:b492990e2b56 174 drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)] / 2.0));
YusukeWakuta 38:b492990e2b56 175 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * SampleFloat((eruronfloat))));
YusukeWakuta 38:b492990e2b56 176 pc.printf("drValue::%f ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float)],SampleFloat((eruronfloat)));
YusukeWakuta 37:1f71ca1e5dd1 177 // pc.printf("raw:%f sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
taurin 4:450cafd95ac3 178 }
taurin 4:450cafd95ac3 179
YusukeWakuta 26:f14579683f98 180 void setTrim()
YusukeWakuta 26:f14579683f98 181 {
taurin 4:450cafd95ac3 182 debugLED = 1;
YusukeWakuta 26:f14579683f98 183 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 184 eruronTrim = eruronAna.read();
YusukeWakuta 26:f14579683f98 185 } else {
YusukeWakuta 37:1f71ca1e5dd1 186 drugTrim = drugAna.read();
YusukeWakuta 26:f14579683f98 187 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 188 }
YusukeWakuta 14:1f6dd929d7de 189 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 190 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 191 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 192 }
taurin 4:450cafd95ac3 193
YusukeWakuta 26:f14579683f98 194 void setMaxDeg()
YusukeWakuta 26:f14579683f98 195 {
taurin 4:450cafd95ac3 196 led4 = 1;
YusukeWakuta 26:f14579683f98 197 float eruronTemp = eruronAna.read();
YusukeWakuta 37:1f71ca1e5dd1 198 float drugTemp = drugAna.read();
YusukeWakuta 26:f14579683f98 199 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 200 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 26:f14579683f98 201 } else {
YusukeWakuta 26:f14579683f98 202 drugServo.pulsewidth(calcPulse(drugTemp));
YusukeWakuta 26:f14579683f98 203 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 204 }
YusukeWakuta 14:1f6dd929d7de 205 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 206 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 207 wait_us(10);
taurin 1:9cc932a16d17 208 }
taurin 0:e052602db102 209
YusukeWakuta 26:f14579683f98 210 int main()
YusukeWakuta 26:f14579683f98 211 {
taurin 0:e052602db102 212 init();
YusukeWakuta 31:5d22ebe5f705 213
YusukeWakuta 30:00041540e23c 214 setTrimPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 215 setMaxDegPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 216 EDstatePin.mode(PullDown);
YusukeWakuta 30:00041540e23c 217 LRstatePin.mode(PullDown);
YusukeWakuta 26:f14579683f98 218
YusukeWakuta 24:d416722b4aad 219 // start motion
tsumagari 33:d075918d4846 220 // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 221
YusukeWakuta 26:f14579683f98 222 while(1) {
YusukeWakuta 26:f14579683f98 223 while(setTrimPin) {
taurin 4:450cafd95ac3 224 setTrim();
taurin 4:450cafd95ac3 225 }
YusukeWakuta 26:f14579683f98 226 while (setMaxDegPin) {
YusukeWakuta 29:516a5d383488 227 setMaxDeg();
taurin 4:450cafd95ac3 228 }
YusukeWakuta 37:1f71ca1e5dd1 229 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 37:1f71ca1e5dd1 230 pc.printf("eMD:%f dMD:%f ",eruronMoveDeg,drugMoveDeg);
taurin 4:450cafd95ac3 231 led4 = 0;
taurin 4:450cafd95ac3 232 debugLED = 0;
taurin 16:82310bf7c326 233 //receiveDatas();
tsumagari 22:b38bc18ec3a1 234 // sendDatas();
taurin 1:9cc932a16d17 235 WriteServo();
taurin 4:450cafd95ac3 236 updateDatas();
tsumagari 22:b38bc18ec3a1 237 led3 = !led3;
taurin 2:7fcb4f970a02 238 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 239 }
taurin 0:e052602db102 240 }