albatross / Mbed 2 deprecated ControlYokutan2018

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@36:ddf4aa818e88, 2017-03-11 (annotated)

Committer:
YusukeWakuta
Date:
Sat Mar 11 11:06:20 2017 +0000
Branch:
XBus???
Revision:
36:ddf4aa818e88
Parent:
35:707119a675dc
Child:
37:1f71ca1e5dd1
????????

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