2017年度の製作を開始します。
Dependencies: Control_Yokutan_CANver1 ADXL345_I2C mbed MPU6050
Fork of Souda_Yokutan_ver528TF by
Diff: main.cpp
- Branch:
- XBus???
- Revision:
- 29:516a5d383488
- Parent:
- 28:99686a3f0e86
- Child:
- 30:00041540e23c
diff -r 99686a3f0e86 -r 516a5d383488 main.cpp --- a/main.cpp Tue Mar 07 15:48:01 2017 +0000 +++ b/main.cpp Fri Mar 10 07:48:48 2017 +0000 @@ -1,6 +1,5 @@ //翼端can program #include "mbed.h" -//#include "ADXL345_I2C.h" #include "MPU6050.h" #include "INA226.hpp" #include "XBusServo.h" @@ -10,7 +9,6 @@ #define WAIT_LOOP_TIME 0.02 #define CONTROL_VALUES_NUM sizeof(float) + 1 #define TO_SEND_CAN_ID 100 -#define ADXL_MEAN_NUM 10 #define SEND_DATAS_LOOP_TIME 0.1 #define RECEIVE_DATAS_LOOP_TIME 0.05 @@ -33,12 +31,10 @@ CAN can(p30,p29); CANMessage recmsg; Serial pc(USBTX,USBRX); -//ADXL345_I2C accelerometer(p9, p10); MPU6050 mpu(p9,p10); I2C ina226_i2c(p28,p27); INA226 VCmonitor(ina226_i2c); PwmOut drugServo(p22); -PwmOut eruronServo(p23); DigitalOut led1(LED1); AnalogIn drugAna(p20); AnalogIn eruronAna(p19); @@ -64,7 +60,6 @@ unsigned short ina_val; double V,C; bool SERVO_FLAG; -bool ADXL_FLAG; bool INA_FLAG; bool MPU_FLAG; @@ -82,7 +77,6 @@ bool servoInit() { drugServo.period_ms(INIT_SERVO_PERIOD_MS); - eruronServo.period_ms(INIT_SERVO_PERIOD_MS); return true; } @@ -137,7 +131,6 @@ drugMoveDeg =DRUG_MOVE_DEG_INI_R; } SERVO_FLAG = servoInit(); -// ADXL_FLAG = adxlInit(); MPU_FLAG = mpu.testConnection(); INA_FLAG = inaInit(); sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); @@ -149,9 +142,6 @@ void updateDatas() { -// if(ADXL_FLAG){ -//// accelerometer.getOutput(acc); -// } if(INA_FLAG) { int tmp = VCmonitor.getVoltage(&V); tmp = VCmonitor.getCurrent(&C); @@ -188,14 +178,14 @@ uint16_t value; uint16_t diff = kMotionEndMark - kMotionMinMark; value = (uint16_t)(diff * (eruronTrim + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark); - gXBus.setServo(servoChannel, value); - gXBus.sendChannelDataPacket(); + gXBus.setServo(servoChannel, value); + gXBus.sendChannelDataPacket(); } - void WriteServo() +void WriteServo() { + pc.printf("%d",controlValues[4]); drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4])); - eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronfloat-1) )); } void setTrim() @@ -203,7 +193,6 @@ debugLED = 1; if(EDstatePin) { eruronTrim = eruronAna.read(); - eruronServo.pulsewidth(calcPulse(eruronTrim)); } else { drugTrim = drugAna.read()*180; drugServo.pulsewidth(calcPulse(drugTrim)); @@ -219,7 +208,6 @@ float eruronTemp = eruronAna.read(); float drugTemp = drugAna.read()*180; if(EDstatePin) { - eruronServo.pulsewidth(calcPulse(eruronTemp)); eruronMoveDeg = eruronTemp-eruronTrim; } else { drugServo.pulsewidth(calcPulse(drugTemp)); @@ -244,7 +232,7 @@ setTrim(); } while (setMaxDegPin) { - setMaxDeg(); + setMaxDeg(); } // pc.printf("eT:%f\n\r",eruronTrim); pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);