albatross / Mbed 2 deprecated ControlYokutan2017_2

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@32:b03557a08efa, 2017-03-10 (annotated)

Committer:
YusukeWakuta
Date:
Fri Mar 10 12:59:41 2017 +0000
Branch:
XBus???
Revision:
32:b03557a08efa
Parent:
31:5d22ebe5f705
Child:
33:d075918d4846
Child:
34:53d2723d9195
????0???

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
taurin 16:82310bf7c326 13 #define RECEIVE_DATAS_LOOP_TIME 0.05
taurin 12:fd9d241843f4 14
YusukeWakuta 32:b03557a08efa 15 #define ERURON_MOVE_DEG_INI_R 0
YusukeWakuta 32:b03557a08efa 16 #define DRUG_MOVE_DEG_INI_R 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 32:b03557a08efa 20 #define ERURON_MOVE_DEG_INI_L 0
YusukeWakuta 32:b03557a08efa 21 #define DRUG_MOVE_DEG_INI_L 0
YusukeWakuta 32:b03557a08efa 22 #define ERURON_TRIM_INI_L 0
YusukeWakuta 32:b03557a08efa 23 #define DRUG_TRIM_INI_L 0
taurin 2:7fcb4f970a02 24
YusukeWakuta 24:d416722b4aad 25 #define kMaxServoNum 1 // 1 - 50
YusukeWakuta 24:d416722b4aad 26 #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec))
YusukeWakuta 24:d416722b4aad 27 #define kMotionInterval 10 // flame / sec
YusukeWakuta 24:d416722b4aad 28 #define kMotionMinMark 0x1249
YusukeWakuta 24:d416722b4aad 29 #define kMotionEndMark 0xED86
YusukeWakuta 24:d416722b4aad 30
taurin 0:e052602db102 31 CAN can(p30,p29);
taurin 0:e052602db102 32 CANMessage recmsg;
taurin 0:e052602db102 33 Serial pc(USBTX,USBRX);
tsumagari 22:b38bc18ec3a1 34 MPU6050 mpu(p9,p10);
taurin 0:e052602db102 35 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 36 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 37 PwmOut drugServo(p22);
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 23:d551db88df65 53 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4: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 static const uint8_t servoChannel = 0x01;
YusukeWakuta 24:d416722b4aad 74
YusukeWakuta 27:591cf9139ae7 75 XBusServo gXBus(p13, NC, NC, kMaxServoNum);
YusukeWakuta 24:d416722b4aad 76 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 77
YusukeWakuta 26:f14579683f98 78 bool servoInit()
YusukeWakuta 26:f14579683f98 79 {
taurin 4:450cafd95ac3 80 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 81 return true;
taurin 0:e052602db102 82 }
taurin 0:e052602db102 83
YusukeWakuta 26:f14579683f98 84 void sendDatas()
YusukeWakuta 24:d416722b4aad 85 {
YusukeWakuta 26:f14579683f98 86 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
taurin 4:450cafd95ac3 87 }
taurin 4:450cafd95ac3 88 }
taurin 4:450cafd95ac3 89
YusukeWakuta 26:f14579683f98 90 bool inaInit()
YusukeWakuta 26:f14579683f98 91 {
YusukeWakuta 26:f14579683f98 92 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 93 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 94 return false;
taurin 0:e052602db102 95 }
taurin 0:e052602db102 96 ina_val = 0;
YusukeWakuta 26:f14579683f98 97 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 98 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 99 return false;
taurin 0:e052602db102 100 }
taurin 0:e052602db102 101 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 102 return true;
taurin 0:e052602db102 103 }
taurin 0:e052602db102 104
YusukeWakuta 26:f14579683f98 105 XBusError initXBus()
YusukeWakuta 26:f14579683f98 106 {
YusukeWakuta 24:d416722b4aad 107 XBusError result;
YusukeWakuta 26:f14579683f98 108 result = gXBus.start();
YusukeWakuta 26:f14579683f98 109 if (result != kXBusError_NoError) {
YusukeWakuta 26:f14579683f98 110 gXBus.stop();
YusukeWakuta 26:f14579683f98 111 return result;
YusukeWakuta 26:f14579683f98 112 }
YusukeWakuta 26:f14579683f98 113 result = gXBus.addServo(servoChannel, kXbusServoNeutral);
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 return kXBusError_NoError;
YusukeWakuta 26:f14579683f98 119 }
YusukeWakuta 26:f14579683f98 120
YusukeWakuta 26:f14579683f98 121 void init()
YusukeWakuta 26:f14579683f98 122 {
YusukeWakuta 26:f14579683f98 123 if(!LRstatePin) {
taurin 13:5e3b4120dbbf 124 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 125 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 126 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 127 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
YusukeWakuta 26:f14579683f98 128 } else {
taurin 12:fd9d241843f4 129 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 130 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 131 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 132 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 133 }
taurin 0:e052602db102 134 SERVO_FLAG = servoInit();
tsumagari 22:b38bc18ec3a1 135 MPU_FLAG = mpu.testConnection();
taurin 0:e052602db102 136 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 137 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 138 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 139 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
YusukeWakuta 24:d416722b4aad 140
YusukeWakuta 26:f14579683f98 141 initXBus();
taurin 0:e052602db102 142 }
taurin 0:e052602db102 143
YusukeWakuta 26:f14579683f98 144 void updateDatas()
YusukeWakuta 26:f14579683f98 145 {
YusukeWakuta 26:f14579683f98 146 if(INA_FLAG) {
taurin 0:e052602db102 147 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 148 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 149 }
YusukeWakuta 26:f14579683f98 150 if(MPU_FLAG) {
tsumagari 22:b38bc18ec3a1 151 mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin 0:e052602db102 152 }
YusukeWakuta 26:f14579683f98 153 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
tsumagari 22:b38bc18ec3a1 154 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 155 }
tsumagari 22:b38bc18ec3a1 156 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
tsumagari 22:b38bc18ec3a1 157 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
taurin 0:e052602db102 158 }
taurin 0:e052602db102 159
YusukeWakuta 26:f14579683f98 160 void receiveDatas()
YusukeWakuta 26:f14579683f98 161 {
YusukeWakuta 26:f14579683f98 162 if(can.read(recmsg)) {
YusukeWakuta 26:f14579683f98 163 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 3:4417217b4f66 164 controlValues[i] = recmsg.data[i];
tsumagari 23:d551db88df65 165 if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
taurin 0:e052602db102 166 }
tsumagari 23:d551db88df65 167 eruronfloat = *(const float *)floatvalues;
taurin 1:9cc932a16d17 168 led1 = !led1;
taurin 0:e052602db102 169 }
taurin 0:e052602db102 170 }
taurin 0:e052602db102 171
YusukeWakuta 26:f14579683f98 172 double calcPulse(int deg)
YusukeWakuta 26:f14579683f98 173 {
taurin 4:450cafd95ac3 174 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 1:9cc932a16d17 175 }
taurin 1:9cc932a16d17 176
YusukeWakuta 26:f14579683f98 177 void XbusIntervalHandler()
YusukeWakuta 26:f14579683f98 178 {
YusukeWakuta 26:f14579683f98 179 uint16_t diff = kMotionEndMark - kMotionMinMark;
YusukeWakuta 32:b03557a08efa 180 XbusValue = (uint16_t)(diff * (eruronTrim + eruronMoveDeg * eruronfloat / 2.0) + kMotionMinMark);
YusukeWakuta 31:5d22ebe5f705 181 //pc.printf("%f",value);
YusukeWakuta 31:5d22ebe5f705 182 gXBus.setServo(servoChannel, XbusValue);
YusukeWakuta 29:516a5d383488 183 gXBus.sendChannelDataPacket();
YusukeWakuta 26:f14579683f98 184 }
YusukeWakuta 26:f14579683f98 185
YusukeWakuta 29:516a5d383488 186 void WriteServo()
YusukeWakuta 26:f14579683f98 187 {
YusukeWakuta 26:f14579683f98 188 drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
taurin 4:450cafd95ac3 189 }
taurin 4:450cafd95ac3 190
YusukeWakuta 26:f14579683f98 191 void setTrim()
YusukeWakuta 26:f14579683f98 192 {
taurin 4:450cafd95ac3 193 debugLED = 1;
YusukeWakuta 26:f14579683f98 194 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 195 eruronTrim = eruronAna.read();
YusukeWakuta 26:f14579683f98 196 } else {
YusukeWakuta 26:f14579683f98 197 drugTrim = drugAna.read()*180;
YusukeWakuta 26:f14579683f98 198 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 199 }
YusukeWakuta 14:1f6dd929d7de 200 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 201 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 202 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 203 }
taurin 4:450cafd95ac3 204
YusukeWakuta 26:f14579683f98 205 void setMaxDeg()
YusukeWakuta 26:f14579683f98 206 {
taurin 4:450cafd95ac3 207 led4 = 1;
YusukeWakuta 26:f14579683f98 208 float eruronTemp = eruronAna.read();
taurin 4:450cafd95ac3 209 float drugTemp = drugAna.read()*180;
YusukeWakuta 26:f14579683f98 210 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 211 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 26:f14579683f98 212 } else {
YusukeWakuta 26:f14579683f98 213 drugServo.pulsewidth(calcPulse(drugTemp));
YusukeWakuta 26:f14579683f98 214 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 215 }
YusukeWakuta 26:f14579683f98 216 // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
YusukeWakuta 14:1f6dd929d7de 217 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 218 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 219 wait_us(10);
taurin 1:9cc932a16d17 220 }
taurin 0:e052602db102 221
YusukeWakuta 26:f14579683f98 222 int main()
YusukeWakuta 26:f14579683f98 223 {
taurin 0:e052602db102 224 init();
YusukeWakuta 24:d416722b4aad 225 XBusError result;
YusukeWakuta 31:5d22ebe5f705 226
YusukeWakuta 30:00041540e23c 227 setTrimPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 228 setMaxDegPin.mode(PullDown);
YusukeWakuta 30:00041540e23c 229 EDstatePin.mode(PullDown);
YusukeWakuta 30:00041540e23c 230 LRstatePin.mode(PullDown);
YusukeWakuta 26:f14579683f98 231
YusukeWakuta 24:d416722b4aad 232 // start motion
YusukeWakuta 24:d416722b4aad 233 gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 234
YusukeWakuta 26:f14579683f98 235 while(1) {
YusukeWakuta 26:f14579683f98 236 while(setTrimPin) {
taurin 4:450cafd95ac3 237 setTrim();
taurin 4:450cafd95ac3 238 }
YusukeWakuta 26:f14579683f98 239 while (setMaxDegPin) {
YusukeWakuta 29:516a5d383488 240 setMaxDeg();
taurin 4:450cafd95ac3 241 }
YusukeWakuta 26:f14579683f98 242 // pc.printf("eT:%f\n\r",eruronTrim);
YusukeWakuta 32:b03557a08efa 243 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 32:b03557a08efa 244 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
YusukeWakuta 32:b03557a08efa 245 // pc.printf("%c,%c,%c,DG:%c\n\r",controlValues[0],controlValues[1],controlValues[2],controlValues[3]);
taurin 4:450cafd95ac3 246 led4 = 0;
YusukeWakuta 26:f14579683f98 247
taurin 4:450cafd95ac3 248 debugLED = 0;
taurin 16:82310bf7c326 249 //receiveDatas();
tsumagari 22:b38bc18ec3a1 250 // sendDatas();
taurin 1:9cc932a16d17 251 WriteServo();
taurin 4:450cafd95ac3 252 updateDatas();
tsumagari 22:b38bc18ec3a1 253 // pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
YusukeWakuta 32:b03557a08efa 254 // pc.printf("%f\n\r",XbusValue);
tsumagari 22:b38bc18ec3a1 255 led3 = !led3;
taurin 2:7fcb4f970a02 256 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 257 }
taurin 0:e052602db102 258 }