albatross / Mbed 2 deprecated ControlYokutan2017_2

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017 by albatross

main.cpp@27:591cf9139ae7, 2017-02-18 (annotated)

Committer:
YusukeWakuta
Date:
Sat Feb 18 14:39:21 2017 +0000
Branch:
XBus???
Revision:
27:591cf9139ae7
Parent:
26:f14579683f98
Child:
28:99686a3f0e86
XBus?mpu??????????XBUS?????

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 "ADXL345_I2C.h"
tsumagari 22:b38bc18ec3a1 4 #include "MPU6050.h"
taurin 0:e052602db102 5 #include "INA226.hpp"
YusukeWakuta 24:d416722b4aad 6 #include "XBusServo.h"
taurin 4:450cafd95ac3 7
tsumagari 22:b38bc18ec3a1 8 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 9 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 10 #define WAIT_LOOP_TIME 0.02
tsumagari 23:d551db88df65 11 #define CONTROL_VALUES_NUM sizeof(float) + 1
taurin 0:e052602db102 12 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 13 #define ADXL_MEAN_NUM 10
taurin 4:450cafd95ac3 14 #define SEND_DATAS_LOOP_TIME 0.1
taurin 16:82310bf7c326 15 #define RECEIVE_DATAS_LOOP_TIME 0.05
taurin 12:fd9d241843f4 16
tsumagari 23:d551db88df65 17 #define ERURON_MOVE_DEG_INI_R 30 // もともと10
YusukeWakuta 18:253f0526b8b3 18 #define DRUG_MOVE_DEG_INI_R 76
YusukeWakuta 20:a5a85df4de13 19 #define ERURON_TRIM_INI_R 97 //元々94　
YusukeWakuta 18:253f0526b8b3 20 #define DRUG_TRIM_INI_R 33
taurin 12:fd9d241843f4 21
tsumagari 23:d551db88df65 22 #define ERURON_MOVE_DEG_INI_L -30 //もともと-7
YusukeWakuta 17:d91ce54dd09a 23 #define DRUG_MOVE_DEG_INI_L -80
YusukeWakuta 20:a5a85df4de13 24 #define ERURON_TRIM_INI_L 113 //元々95
YusukeWakuta 17:d91ce54dd09a 25 #define DRUG_TRIM_INI_L 110
taurin 2:7fcb4f970a02 26
YusukeWakuta 24:d416722b4aad 27 #define kMaxServoNum 1 // 1 - 50
YusukeWakuta 24:d416722b4aad 28 #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec))
YusukeWakuta 24:d416722b4aad 29 #define kMotionInterval 10 // flame / sec
YusukeWakuta 24:d416722b4aad 30 #define kMotionMinMark 0x1249
YusukeWakuta 24:d416722b4aad 31 #define kMotionEndMark 0xED86
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 //ADXL345_I2C accelerometer(p9, p10);
tsumagari 22:b38bc18ec3a1 37 MPU6050 mpu(p9,p10);
taurin 0:e052602db102 38 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 39 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 40 PwmOut drugServo(p22);
taurin 13:5e3b4120dbbf 41 PwmOut eruronServo(p23);
taurin 1:9cc932a16d17 42 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 43 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 44 AnalogIn eruronAna(p19);
taurin 12:fd9d241843f4 45 DigitalIn LRstatePin(p14);
taurin 4:450cafd95ac3 46 DigitalIn setTrimPin(p15);
taurin 12:fd9d241843f4 47 DigitalIn EDstatePin(p16);
YusukeWakuta 26:f14579683f98 48 DigitalIn setMaxDegPin(p17);
taurin 4:450cafd95ac3 49 DigitalOut debugLED(LED2);
taurin 4:450cafd95ac3 50 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 51 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 52 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 53 //Ticker toStringTicker;
taurin 16:82310bf7c326 54 Ticker receiveDatasTicker;
taurin 16:82310bf7c326 55
taurin 0:e052602db102 56 char toSendDatas[TO_SEND_DATAS_NUM];
tsumagari 23:d551db88df65 57 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug
tsumagari 23:d551db88df65 58 char floatvalues[sizeof(float)];
taurin 12:fd9d241843f4 59 float eruronTrim;
taurin 12:fd9d241843f4 60 float drugTrim;
taurin 12:fd9d241843f4 61 float eruronMoveDeg;
taurin 12:fd9d241843f4 62 float drugMoveDeg;
tsumagari 23:d551db88df65 63 float eruronfloat;
taurin 0:e052602db102 64 unsigned short ina_val;
taurin 0:e052602db102 65 double V,C;
taurin 0:e052602db102 66 bool SERVO_FLAG;
taurin 0:e052602db102 67 bool ADXL_FLAG;
taurin 0:e052602db102 68 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 69 bool MPU_FLAG;
taurin 0:e052602db102 70
tsumagari 22:b38bc18ec3a1 71 char gyro_c[6] = {0,0,0,0,0,0};
taurin 4:450cafd95ac3 72
taurin 4:450cafd95ac3 73 void toString();
taurin 16:82310bf7c326 74 void receiveDatas();
taurin 16:82310bf7c326 75 void WriteServo();
taurin 0:e052602db102 76
YusukeWakuta 24:d416722b4aad 77 static const uint8_t servoChannel = 0x01;
YusukeWakuta 24:d416722b4aad 78
YusukeWakuta 27:591cf9139ae7 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 4:450cafd95ac3 85 eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 86 return true;
taurin 0:e052602db102 87 }
taurin 0:e052602db102 88
YusukeWakuta 26:f14579683f98 89 void sendDatas()
YusukeWakuta 24:d416722b4aad 90 {
YusukeWakuta 26:f14579683f98 91 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
taurin 4:450cafd95ac3 92 }
taurin 4:450cafd95ac3 93 }
taurin 4:450cafd95ac3 94
YusukeWakuta 26:f14579683f98 95 bool inaInit()
YusukeWakuta 26:f14579683f98 96 {
YusukeWakuta 26:f14579683f98 97 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 98 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 99 return false;
taurin 0:e052602db102 100 }
taurin 0:e052602db102 101 ina_val = 0;
YusukeWakuta 26:f14579683f98 102 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 103 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 104 return false;
taurin 0:e052602db102 105 }
taurin 0:e052602db102 106 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 107 return true;
taurin 0:e052602db102 108 }
taurin 0:e052602db102 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 26:f14579683f98 125
YusukeWakuta 26:f14579683f98 126 void init()
YusukeWakuta 26:f14579683f98 127 {
YusukeWakuta 26:f14579683f98 128 if(!LRstatePin) {
taurin 13:5e3b4120dbbf 129 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 130 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 131 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 132 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
YusukeWakuta 26:f14579683f98 133 } else {
taurin 12:fd9d241843f4 134 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 135 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 136 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 137 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 138 }
taurin 0:e052602db102 139 SERVO_FLAG = servoInit();
tsumagari 22:b38bc18ec3a1 140 // ADXL_FLAG = adxlInit();
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 26:f14579683f98 147 initXBus();
taurin 0:e052602db102 148 }
taurin 0:e052602db102 149
YusukeWakuta 26:f14579683f98 150 void updateDatas()
YusukeWakuta 26:f14579683f98 151 {
tsumagari 22:b38bc18ec3a1 152 // if(ADXL_FLAG){
tsumagari 22:b38bc18ec3a1 153 //// accelerometer.getOutput(acc);
tsumagari 22:b38bc18ec3a1 154 // }
YusukeWakuta 26:f14579683f98 155 if(INA_FLAG) {
taurin 0:e052602db102 156 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 157 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 158 }
YusukeWakuta 26:f14579683f98 159 if(MPU_FLAG) {
tsumagari 22:b38bc18ec3a1 160 mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin 0:e052602db102 161 }
YusukeWakuta 26:f14579683f98 162 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
tsumagari 22:b38bc18ec3a1 163 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 164 }
tsumagari 22:b38bc18ec3a1 165 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
tsumagari 22:b38bc18ec3a1 166 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
taurin 0:e052602db102 167 }
taurin 0:e052602db102 168
YusukeWakuta 26:f14579683f98 169 void receiveDatas()
YusukeWakuta 26:f14579683f98 170 {
YusukeWakuta 26:f14579683f98 171 if(can.read(recmsg)) {
YusukeWakuta 26:f14579683f98 172 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 3:4417217b4f66 173 controlValues[i] = recmsg.data[i];
tsumagari 23:d551db88df65 174 if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
taurin 0:e052602db102 175 }
tsumagari 23:d551db88df65 176 eruronfloat = *(const float *)floatvalues;
taurin 1:9cc932a16d17 177 led1 = !led1;
taurin 0:e052602db102 178 }
taurin 0:e052602db102 179 }
taurin 0:e052602db102 180
YusukeWakuta 26:f14579683f98 181 double calcPulse(int deg)
YusukeWakuta 26:f14579683f98 182 {
taurin 4:450cafd95ac3 183 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 1:9cc932a16d17 184 }
taurin 1:9cc932a16d17 185
YusukeWakuta 26:f14579683f98 186 void XbusIntervalHandler()
YusukeWakuta 26:f14579683f98 187 {
YusukeWakuta 26:f14579683f98 188 uint16_t value;
YusukeWakuta 26:f14579683f98 189 uint16_t diff = kMotionEndMark - kMotionMinMark;
YusukeWakuta 26:f14579683f98 190 value = (uint16_t)(diff * (eruronTrim + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
YusukeWakuta 26:f14579683f98 191 gXBus.setServo(servoChannel, value);
YusukeWakuta 26:f14579683f98 192 gXBus.sendChannelDataPacket();
YusukeWakuta 26:f14579683f98 193 }
YusukeWakuta 26:f14579683f98 194
YusukeWakuta 26:f14579683f98 195 void WriteServo()
YusukeWakuta 26:f14579683f98 196 {
YusukeWakuta 26:f14579683f98 197 drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
YusukeWakuta 26:f14579683f98 198 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
taurin 4:450cafd95ac3 199 }
taurin 4:450cafd95ac3 200
YusukeWakuta 26:f14579683f98 201 void setTrim()
YusukeWakuta 26:f14579683f98 202 {
taurin 4:450cafd95ac3 203 debugLED = 1;
YusukeWakuta 26:f14579683f98 204 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 205 eruronTrim = eruronAna.read();
YusukeWakuta 26:f14579683f98 206 eruronServo.pulsewidth(calcPulse(eruronTrim));
YusukeWakuta 26:f14579683f98 207 } else {
YusukeWakuta 26:f14579683f98 208 drugTrim = drugAna.read()*180;
YusukeWakuta 26:f14579683f98 209 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 210 }
YusukeWakuta 14:1f6dd929d7de 211 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 212 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 213 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 214 }
taurin 4:450cafd95ac3 215
YusukeWakuta 26:f14579683f98 216 void setMaxDeg()
YusukeWakuta 26:f14579683f98 217 {
taurin 4:450cafd95ac3 218 led4 = 1;
YusukeWakuta 26:f14579683f98 219 float eruronTemp = eruronAna.read();
taurin 4:450cafd95ac3 220 float drugTemp = drugAna.read()*180;
YusukeWakuta 26:f14579683f98 221 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 222 eruronServo.pulsewidth(calcPulse(eruronTemp));
YusukeWakuta 26:f14579683f98 223 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 26:f14579683f98 224 } else {
YusukeWakuta 26:f14579683f98 225 drugServo.pulsewidth(calcPulse(drugTemp));
YusukeWakuta 26:f14579683f98 226 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 227 }
YusukeWakuta 26:f14579683f98 228 // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
YusukeWakuta 14:1f6dd929d7de 229 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 230 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 231 wait_us(10);
taurin 1:9cc932a16d17 232 }
taurin 0:e052602db102 233
YusukeWakuta 26:f14579683f98 234 int main()
YusukeWakuta 26:f14579683f98 235 {
taurin 0:e052602db102 236 init();
YusukeWakuta 24:d416722b4aad 237 XBusError result;
YusukeWakuta 26:f14579683f98 238
YusukeWakuta 24:d416722b4aad 239 // start motion
YusukeWakuta 24:d416722b4aad 240 gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 241
YusukeWakuta 26:f14579683f98 242 while(1) {
YusukeWakuta 26:f14579683f98 243 while(setTrimPin) {
taurin 4:450cafd95ac3 244 setTrim();
taurin 4:450cafd95ac3 245 }
YusukeWakuta 26:f14579683f98 246 while (setMaxDegPin) {
YusukeWakuta 26:f14579683f98 247 setMaxDeg();
taurin 4:450cafd95ac3 248 }
YusukeWakuta 26:f14579683f98 249 // pc.printf("eT:%f\n\r",eruronTrim);
YusukeWakuta 26:f14579683f98 250 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 251 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 252 led4 = 0;
YusukeWakuta 26:f14579683f98 253
taurin 4:450cafd95ac3 254 debugLED = 0;
taurin 16:82310bf7c326 255 //receiveDatas();
tsumagari 22:b38bc18ec3a1 256 // sendDatas();
taurin 1:9cc932a16d17 257 WriteServo();
taurin 4:450cafd95ac3 258 updateDatas();
tsumagari 22:b38bc18ec3a1 259 // pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
tsumagari 22:b38bc18ec3a1 260 led3 = !led3;
taurin 2:7fcb4f970a02 261 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 262 }
taurin 0:e052602db102 263 }