albatross / Mbed 2 deprecated ControlYokutan2018

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@25:e8bfb629e1b1, 2017-02-15 (annotated)

Committer:
YusukeWakuta
Date:
Wed Feb 15 13:29:20 2017 +0000
Branch:
XBus???
Revision:
25:e8bfb629e1b1
Parent:
24:d416722b4aad
Parent:
23:d551db88df65
Child:
26:f14579683f98
merge

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 kXBusTx p9
YusukeWakuta 24:d416722b4aad 28 #define kMaxServoNum 1 // 1 - 50
YusukeWakuta 24:d416722b4aad 29 #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec))
YusukeWakuta 24:d416722b4aad 30 #define kMotionInterval 10 // flame / sec
YusukeWakuta 24:d416722b4aad 31 #define kMotionMinMark 0x1249
YusukeWakuta 24:d416722b4aad 32 #define kMotionEndMark 0xED86
YusukeWakuta 24:d416722b4aad 33
taurin 0:e052602db102 34 CAN can(p30,p29);
taurin 0:e052602db102 35 CANMessage recmsg;
taurin 0:e052602db102 36 Serial pc(USBTX,USBRX);
tsumagari 22:b38bc18ec3a1 37 //ADXL345_I2C accelerometer(p9, p10);
tsumagari 22:b38bc18ec3a1 38 MPU6050 mpu(p9,p10);
taurin 0:e052602db102 39 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 40 INA226 VCmonitor(ina226_i2c);
taurin 13:5e3b4120dbbf 41 PwmOut drugServo(p22);
taurin 13:5e3b4120dbbf 42 PwmOut eruronServo(p23);
taurin 1:9cc932a16d17 43 DigitalOut led1(LED1);
taurin 4:450cafd95ac3 44 AnalogIn drugAna(p20);
taurin 4:450cafd95ac3 45 AnalogIn eruronAna(p19);
taurin 12:fd9d241843f4 46 DigitalIn LRstatePin(p14);
taurin 4:450cafd95ac3 47 DigitalIn setTrimPin(p15);
taurin 12:fd9d241843f4 48 DigitalIn EDstatePin(p16);
taurin 4:450cafd95ac3 49 DigitalIn checkMaxDegPin(p17);
taurin 4:450cafd95ac3 50 DigitalOut debugLED(LED2);
taurin 4:450cafd95ac3 51 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 52 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 53 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 54 //Ticker toStringTicker;
taurin 16:82310bf7c326 55 Ticker receiveDatasTicker;
taurin 16:82310bf7c326 56
taurin 0:e052602db102 57 char toSendDatas[TO_SEND_DATAS_NUM];
tsumagari 23:d551db88df65 58 char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug
tsumagari 23:d551db88df65 59 char floatvalues[sizeof(float)];
taurin 12:fd9d241843f4 60 float eruronTrim;
taurin 12:fd9d241843f4 61 float drugTrim;
taurin 12:fd9d241843f4 62 float eruronMoveDeg;
taurin 12:fd9d241843f4 63 float drugMoveDeg;
tsumagari 23:d551db88df65 64 float eruronfloat;
taurin 0:e052602db102 65 unsigned short ina_val;
taurin 0:e052602db102 66 double V,C;
taurin 0:e052602db102 67 bool SERVO_FLAG;
taurin 0:e052602db102 68 bool ADXL_FLAG;
taurin 0:e052602db102 69 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 70 bool MPU_FLAG;
taurin 0:e052602db102 71
tsumagari 22:b38bc18ec3a1 72 //int16_t acc[3] = {0,0,0};
tsumagari 22:b38bc18ec3a1 73 char gyro_c[6] = {0,0,0,0,0,0};
tsumagari 22:b38bc18ec3a1 74 //char acc_mean[3][ADXL_MEAN_NUM];
tsumagari 22:b38bc18ec3a1 75 //int adxl_mean_counter = 0;
taurin 4:450cafd95ac3 76
taurin 4:450cafd95ac3 77 void toString();
taurin 16:82310bf7c326 78 void receiveDatas();
taurin 16:82310bf7c326 79 void WriteServo();
taurin 0:e052602db102 80
YusukeWakuta 24:d416722b4aad 81 static const uint8_t servoChannel = 0x01;
YusukeWakuta 24:d416722b4aad 82
YusukeWakuta 24:d416722b4aad 83 XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
YusukeWakuta 24:d416722b4aad 84 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 85
taurin 0:e052602db102 86 bool servoInit(){
taurin 4:450cafd95ac3 87 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 4:450cafd95ac3 88 eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 89 return true;
taurin 0:e052602db102 90 }
taurin 0:e052602db102 91
YusukeWakuta 24:d416722b4aad 92 //=============================================================
YusukeWakuta 24:d416722b4aad 93 // XbusIntervalHandler()
YusukeWakuta 24:d416722b4aad 94 // play motion !
YusukeWakuta 24:d416722b4aad 95 //=============================================================
YusukeWakuta 24:d416722b4aad 96 void XbusIntervalHandler()
YusukeWakuta 24:d416722b4aad 97 {
YusukeWakuta 24:d416722b4aad 98 uint16_t value;
YusukeWakuta 24:d416722b4aad 99 uint16_t diff = kMotionEndMark - kMotionMinMark;
YusukeWakuta 24:d416722b4aad 100 value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
YusukeWakuta 24:d416722b4aad 101 gXBus.setServo(servoChannel, value);
YusukeWakuta 24:d416722b4aad 102 gXBus.sendChannelDataPacket();
YusukeWakuta 24:d416722b4aad 103 }
YusukeWakuta 24:d416722b4aad 104
tsumagari 22:b38bc18ec3a1 105 //bool adxlInit(){
tsumagari 22:b38bc18ec3a1 106 // accelerometer.setPowerControl(0x00);
tsumagari 22:b38bc18ec3a1 107 // accelerometer.setDataFormatControl(0x0B);
tsumagari 22:b38bc18ec3a1 108 // accelerometer.setDataRate(ADXL345_3200HZ);
tsumagari 22:b38bc18ec3a1 109 // accelerometer.setPowerControl(0x08);
tsumagari 22:b38bc18ec3a1 110 // return true;
tsumagari 22:b38bc18ec3a1 111 //}
taurin 0:e052602db102 112
taurin 4:450cafd95ac3 113 void sendDatas(){
taurin 4:450cafd95ac3 114 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
taurin 4:450cafd95ac3 115 }
taurin 4:450cafd95ac3 116 }
taurin 4:450cafd95ac3 117
taurin 0:e052602db102 118 bool inaInit(){
taurin 0:e052602db102 119 if(!VCmonitor.isExist()){
taurin 0:e052602db102 120 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 121 return false;
taurin 0:e052602db102 122 }
taurin 0:e052602db102 123 ina_val = 0;
taurin 0:e052602db102 124 if(VCmonitor.rawRead(0x00,&ina_val) != 0){
taurin 0:e052602db102 125 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 126 return false;
taurin 0:e052602db102 127 }
taurin 0:e052602db102 128 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 129 return true;
taurin 0:e052602db102 130 }
taurin 0:e052602db102 131
YusukeWakuta 24:d416722b4aad 132 XBusError init(){
YusukeWakuta 24:d416722b4aad 133
YusukeWakuta 24:d416722b4aad 134 XBusError result;
YusukeWakuta 24:d416722b4aad 135
taurin 15:1db5ee4fe7ce 136 if(!LRstatePin){
taurin 13:5e3b4120dbbf 137 eruronTrim = ERURON_TRIM_INI_L;
taurin 13:5e3b4120dbbf 138 drugTrim = DRUG_TRIM_INI_L;
taurin 13:5e3b4120dbbf 139 eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 140 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
taurin 13:5e3b4120dbbf 141 }
taurin 13:5e3b4120dbbf 142 else{
taurin 12:fd9d241843f4 143 eruronTrim = ERURON_TRIM_INI_R;
taurin 12:fd9d241843f4 144 drugTrim = DRUG_TRIM_INI_R;
taurin 12:fd9d241843f4 145 eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
taurin 13:5e3b4120dbbf 146 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
taurin 12:fd9d241843f4 147 }
taurin 0:e052602db102 148 SERVO_FLAG = servoInit();
tsumagari 22:b38bc18ec3a1 149 // ADXL_FLAG = adxlInit();
tsumagari 22:b38bc18ec3a1 150 MPU_FLAG = mpu.testConnection();
taurin 0:e052602db102 151 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 152 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 153 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 154 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
YusukeWakuta 24:d416722b4aad 155
YusukeWakuta 24:d416722b4aad 156 //XBus
YusukeWakuta 24:d416722b4aad 157 // initialize XBus
YusukeWakuta 24:d416722b4aad 158 result = gXBus.start();
YusukeWakuta 24:d416722b4aad 159 if (result != kXBusError_NoError) {
YusukeWakuta 24:d416722b4aad 160 gXBus.stop();
YusukeWakuta 24:d416722b4aad 161 return result;
YusukeWakuta 24:d416722b4aad 162 }
YusukeWakuta 24:d416722b4aad 163 // initialize XBus Servos
YusukeWakuta 24:d416722b4aad 164 result = gXBus.addServo(servoChannel, kXbusServoNeutral);
YusukeWakuta 24:d416722b4aad 165 if (result != kXBusError_NoError) {
YusukeWakuta 24:d416722b4aad 166 gXBus.stop();
YusukeWakuta 24:d416722b4aad 167 return result;
YusukeWakuta 24:d416722b4aad 168 }
YusukeWakuta 24:d416722b4aad 169
YusukeWakuta 24:d416722b4aad 170 return kXBusError_NoError;
taurin 0:e052602db102 171 }
taurin 0:e052602db102 172
taurin 0:e052602db102 173 void updateDatas(){
tsumagari 22:b38bc18ec3a1 174 // if(ADXL_FLAG){
tsumagari 22:b38bc18ec3a1 175 //// accelerometer.getOutput(acc);
tsumagari 22:b38bc18ec3a1 176 // }
taurin 0:e052602db102 177 if(INA_FLAG){
taurin 0:e052602db102 178 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 179 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 180 }
tsumagari 22:b38bc18ec3a1 181 if(MPU_FLAG){
tsumagari 22:b38bc18ec3a1 182 mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
taurin 0:e052602db102 183 }
tsumagari 22:b38bc18ec3a1 184 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){
tsumagari 22:b38bc18ec3a1 185 // toSendDatas[i] = acc[i];
tsumagari 22:b38bc18ec3a1 186 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 187 }
tsumagari 22:b38bc18ec3a1 188 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
tsumagari 22:b38bc18ec3a1 189 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
taurin 0:e052602db102 190 }
taurin 0:e052602db102 191
taurin 0:e052602db102 192 void receiveDatas(){
taurin 0:e052602db102 193 if(can.read(recmsg)){
taurin 2:7fcb4f970a02 194 for(int i = 0; i < CONTROL_VALUES_NUM; i++){
YusukeWakuta 3:4417217b4f66 195 controlValues[i] = recmsg.data[i];
tsumagari 23:d551db88df65 196 if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
taurin 0:e052602db102 197 }
tsumagari 23:d551db88df65 198 eruronfloat = *(const float *)floatvalues;
taurin 1:9cc932a16d17 199 led1 = !led1;
taurin 16:82310bf7c326 200 //WriteServo();
taurin 0:e052602db102 201 }
taurin 0:e052602db102 202 }
taurin 0:e052602db102 203
taurin 0:e052602db102 204 void toString(){
taurin 4:450cafd95ac3 205 for(int i = 0; i <CONTROL_VALUES_NUM; i++){
taurin 4:450cafd95ac3 206 pc.printf("%d, ",controlValues[i]);
taurin 4:450cafd95ac3 207 }
taurin 4:450cafd95ac3 208 pc.printf("\n\r");
taurin 0:e052602db102 209 }
taurin 0:e052602db102 210
taurin 1:9cc932a16d17 211 double calcPulse(int deg){
taurin 4:450cafd95ac3 212 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:450cafd95ac3 213
taurin 1:9cc932a16d17 214 }
taurin 1:9cc932a16d17 215
taurin 1:9cc932a16d17 216 void WriteServo(){
taurin 12:fd9d241843f4 217 //if(debugServoPin){
taurin 12:fd9d241843f4 218 // led3 = 1;
taurin 12:fd9d241843f4 219 // float a = eruronAna.read()*180;
taurin 12:fd9d241843f4 220 // float b = drugAna.read()*180;
taurin 12:fd9d241843f4 221 // eruronServo.pulsewidth(calcPulse(eruronAna.read()*180));
taurin 12:fd9d241843f4 222 // drugServo.pulsewidth(calcPulse(drugAna.read()*180));
taurin 12:fd9d241843f4 223 // pc.printf("eruron:%f drug:%f\n\r",a,b);
taurin 12:fd9d241843f4 224 // }
taurin 12:fd9d241843f4 225 // else{
taurin 12:fd9d241843f4 226 // led3 = 0;
tsumagari 23:d551db88df65 227 // eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (controlValues[0]-1) ));
tsumagari 23:d551db88df65 228 drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
tsumagari 23:d551db88df65 229 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) ));
taurin 12:fd9d241843f4 230 //}
taurin 4:450cafd95ac3 231 }
taurin 4:450cafd95ac3 232
taurin 4:450cafd95ac3 233 void setTrim(){
taurin 4:450cafd95ac3 234 debugLED = 1;
taurin 12:fd9d241843f4 235 if(EDstatePin){
taurin 4:450cafd95ac3 236 eruronTrim = eruronAna.read()*180;
taurin 12:fd9d241843f4 237 eruronServo.pulsewidth(calcPulse(eruronTrim));
taurin 12:fd9d241843f4 238 }
taurin 12:fd9d241843f4 239 else{
taurin 4:450cafd95ac3 240 drugTrim = drugAna.read()*180;
taurin 4:450cafd95ac3 241 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 242 }
YusukeWakuta 14:1f6dd929d7de 243 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 14:1f6dd929d7de 244 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 245 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 246 }
taurin 4:450cafd95ac3 247
taurin 4:450cafd95ac3 248 void checkMaxDeg(){
taurin 4:450cafd95ac3 249 led4 = 1;
taurin 4:450cafd95ac3 250 float eruronTemp = eruronAna.read()*180;
taurin 4:450cafd95ac3 251 float drugTemp = drugAna.read()*180;
taurin 12:fd9d241843f4 252 if(EDstatePin){
taurin 4:450cafd95ac3 253 eruronServo.pulsewidth(calcPulse(eruronTemp));
taurin 12:fd9d241843f4 254 eruronMoveDeg = eruronTemp-eruronTrim;
taurin 12:fd9d241843f4 255 }
taurin 12:fd9d241843f4 256 else{
taurin 4:450cafd95ac3 257 drugServo.pulsewidth(calcPulse(drugTemp));
taurin 4:450cafd95ac3 258 drugMoveDeg = drugTemp-drugTrim;
taurin 12:fd9d241843f4 259 }
YusukeWakuta 14:1f6dd929d7de 260 // pc.printf("eMD:%f dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
YusukeWakuta 14:1f6dd929d7de 261 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 262 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 263 wait_us(10);
taurin 1:9cc932a16d17 264 }
taurin 0:e052602db102 265
taurin 0:e052602db102 266 int main(){
taurin 0:e052602db102 267 init();
YusukeWakuta 24:d416722b4aad 268 XBusError result;
YusukeWakuta 24:d416722b4aad 269
YusukeWakuta 24:d416722b4aad 270 // start motion
YusukeWakuta 24:d416722b4aad 271 gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 24:d416722b4aad 272
taurin 0:e052602db102 273 while(1){
taurin 4:450cafd95ac3 274 while(setTrimPin){
taurin 4:450cafd95ac3 275 setTrim();
taurin 4:450cafd95ac3 276 }
taurin 4:450cafd95ac3 277 while(checkMaxDegPin){
taurin 4:450cafd95ac3 278 checkMaxDeg();
taurin 4:450cafd95ac3 279 }
YusukeWakuta 14:1f6dd929d7de 280 // pc.printf("eT:%f\n\r",eruronTrim);
YusukeWakuta 14:1f6dd929d7de 281 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
tsumagari 23:d551db88df65 282 pc.printf("eMD:%f dMD:%f ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 283 led4 = 0;
YusukeWakuta 14:1f6dd929d7de 284
taurin 4:450cafd95ac3 285 debugLED = 0;
taurin 16:82310bf7c326 286 //receiveDatas();
tsumagari 22:b38bc18ec3a1 287 // sendDatas();
taurin 1:9cc932a16d17 288 WriteServo();
taurin 4:450cafd95ac3 289 updateDatas();
tsumagari 22:b38bc18ec3a1 290 // pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
tsumagari 22:b38bc18ec3a1 291 led3 = !led3;
taurin 2:7fcb4f970a02 292 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 293 }
taurin 0:e052602db102 294 }