albatross / Mbed 2 deprecated ControlYokutan2017_2

ControlMainでの変更に対応して、新しくレポジトリを作りました

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@37:1f71ca1e5dd1, 2017-03-11 (annotated)

Committer:
YusukeWakuta
Date:
Sat Mar 11 16:22:16 2017 +0000
Branch:
XBus???
Revision:
37:1f71ca1e5dd1
Parent:
36:ddf4aa818e88
Child:
38:b492990e2b56
??????????????

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