albatross / Mbed 2 deprecated ControlYokutan2018

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

Dependencies:   Control_Yokutan_CANver1 XBusServo mbed mbed-rtos

Fork of ControlYokutan2017_2 by albatross

main.cpp@71:a00561d455d1, 2017-10-21 (annotated)

Committer:
tsumagari
Date:
Sat Oct 21 06:52:10 2017 +0000
Branch:
mpu????????
Revision:
71:a00561d455d1
Parent:
70:7da315abec17
Child:
72:aef1ec8c66c7
can??????????servoOff????????

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 42:bf98a29e81ac 5 #include "rtos.h"
YusukeWakuta 24:d416722b4aad 6 #include "XBusServo.h"
YusukeWakuta 57:d7b709dd1c4f 7 #include "math.h"
taurin 4:450cafd95ac3 8
tsumagari 22:b38bc18ec3a1 9 #define TO_SEND_DATAS_NUM 7
taurin 2:7fcb4f970a02 10 #define INIT_SERVO_PERIOD_MS 20
taurin 4:450cafd95ac3 11 #define WAIT_LOOP_TIME 0.02
YusukeWakuta 47:5e3a836deaba 12 #define CONTROL_VALUES_NUM 7
taurin 0:e052602db102 13 #define TO_SEND_CAN_ID 100
taurin 4:450cafd95ac3 14 #define SEND_DATAS_LOOP_TIME 0.1
tsumagari 33:d075918d4846 15 #define RECEIVE_DATAS_LOOP_TIME 0.1
taurin 12:fd9d241843f4 16
YusukeWakuta 42:bf98a29e81ac 17 #define MPU_LOOP_TIME 0.01
YusukeWakuta 42:bf98a29e81ac 18 #define MPU_DELT_MIN 250
YusukeWakuta 42:bf98a29e81ac 19
tsumagari 67:94fe2180f39a 20 #define ERURON_MOVE_DEG_INI_R 14.4*0.8//18.0 //degree
tsumagari 67:94fe2180f39a 21 #define DRUG_MOVE_DEG_INI_R 0.49
tsumagari 66:2d1a7f270e27 22 #define ERURON_TRIM_INI_R 0.39633 //値lowerすると頭上げ 0.37で後縁一致
YusukeWakuta 58:f84bd22fd586 23 #define DRUG_TRIM_INI_R 0.37
taurin 12:fd9d241843f4 24
tsumagari 64:9d7582920f50 25 #define ERURON_MOVE_DEG_INI_L -15.52//-19.4 //degree
YusukeWakuta 63:52b882a5968c 26 #define DRUG_MOVE_DEG_INI_L -0.52
tsumagari 67:94fe2180f39a 27 #define ERURON_TRIM_INI_L 0.41567 // 値を大きくすると頭上げ
YusukeWakuta 58:f84bd22fd586 28 #define DRUG_TRIM_INI_L 0.73//値を小さくすると開く側
taurin 2:7fcb4f970a02 29
tsumagari 67:94fe2180f39a 30
tsumagari 67:94fe2180f39a 31 #define debug pc.printf
tsumagari 67:94fe2180f39a 32 #define debugMPU //pc.printf
tsumagari 67:94fe2180f39a 33
tsumagari 67:94fe2180f39a 34 const char *configfilename = "/local/CONFIG.csv";
tsumagari 67:94fe2180f39a 35
YusukeWakuta 38:b492990e2b56 36 /*ドラッグラダー
YusukeWakuta 37:1f71ca1e5dd1 37 初期値 0.65
YusukeWakuta 37:1f71ca1e5dd1 38 最大角0.99
YusukeWakuta 36:ddf4aa818e88 39 */
YusukeWakuta 24:d416722b4aad 40
taurin 0:e052602db102 41 CAN can(p30,p29);
taurin 0:e052602db102 42 CANMessage recmsg;
taurin 0:e052602db102 43 Serial pc(USBTX,USBRX);
taurin 0:e052602db102 44 I2C ina226_i2c(p28,p27);
taurin 0:e052602db102 45 INA226 VCmonitor(ina226_i2c);
tsumagari 70:7da315abec17 46 PwmOut drugServo(p23);
tsumagari 71:a00561d455d1 47 PwmOut eruronServo(p25);
taurin 1:9cc932a16d17 48 DigitalOut led1(LED1);
tsumagari 69:073116d31a56 49 AnalogIn setDeg10(p20);
tsumagari 69:073116d31a56 50 AnalogIn setDeg1(p19);
tsumagari 71:a00561d455d1 51 DigitalIn IsRPin(p13,PullDown);
tsumagari 71:a00561d455d1 52 DigitalIn InSetModePin(p14,PullDown);
tsumagari 71:a00561d455d1 53 DigitalIn EDstatePin(p15,PullDown);
tsumagari 71:a00561d455d1 54 DigitalIn TrimMaxDegPin(p16,PullDown);
tsumagari 71:a00561d455d1 55 DigitalOut servoOff(p17);
YusukeWakuta 44:624a4469ae21 56 DigitalOut led2(LED2);
taurin 4:450cafd95ac3 57 DigitalOut led3(LED3);
taurin 4:450cafd95ac3 58 DigitalOut led4(LED4);
taurin 4:450cafd95ac3 59 Ticker sendDatasTicker;
tsumagari 22:b38bc18ec3a1 60 //Ticker toStringTicker;
taurin 16:82310bf7c326 61 Ticker receiveDatasTicker;
YusukeWakuta 42:bf98a29e81ac 62 MPU6050 mpu6050;
YusukeWakuta 42:bf98a29e81ac 63 Timer t;
YusukeWakuta 42:bf98a29e81ac 64
tsumagari 67:94fe2180f39a 65 LocalFileSystem local("local");
tsumagari 67:94fe2180f39a 66
YusukeWakuta 42:bf98a29e81ac 67 //Set up I2C, (SDA,SCL)
YusukeWakuta 42:bf98a29e81ac 68 I2C i2c_mpu(p9,p10);
taurin 16:82310bf7c326 69
taurin 0:e052602db102 70 char toSendDatas[TO_SEND_DATAS_NUM];
YusukeWakuta 60:a45dc19a6001 71 char floatValues[10];
taurin 12:fd9d241843f4 72 float eruronTrim;
taurin 12:fd9d241843f4 73 float drugTrim;
taurin 12:fd9d241843f4 74 float eruronMoveDeg;
taurin 12:fd9d241843f4 75 float drugMoveDeg;
YusukeWakuta 59:f007e543f8c9 76 float eruronfloat = 0.0;
taurin 0:e052602db102 77 unsigned short ina_val;
taurin 0:e052602db102 78 double V,C;
taurin 0:e052602db102 79 bool SERVO_FLAG;
taurin 0:e052602db102 80 bool INA_FLAG;
tsumagari 22:b38bc18ec3a1 81 bool MPU_FLAG;
YusukeWakuta 42:bf98a29e81ac 82 int gyroX;
YusukeWakuta 42:bf98a29e81ac 83 int gyroY;
YusukeWakuta 42:bf98a29e81ac 84 int gyroZ;
YusukeWakuta 42:bf98a29e81ac 85 float sum = 0;
YusukeWakuta 61:988e3f4280ac 86 float drugInput = 0.0;
YusukeWakuta 42:bf98a29e81ac 87 uint32_t sumCount = 0;
taurin 0:e052602db102 88
tsumagari 22:b38bc18ec3a1 89 char gyro_c[6] = {0,0,0,0,0,0};
taurin 4:450cafd95ac3 90
taurin 4:450cafd95ac3 91 void toString();
taurin 16:82310bf7c326 92 void receiveDatas();
taurin 16:82310bf7c326 93 void WriteServo();
YusukeWakuta 42:bf98a29e81ac 94 void MpuInit();
YusukeWakuta 42:bf98a29e81ac 95 void mpuProcessing(void const *arg);
taurin 0:e052602db102 96
YusukeWakuta 24:d416722b4aad 97 Ticker gTimer;
YusukeWakuta 24:d416722b4aad 98
YusukeWakuta 26:f14579683f98 99 bool servoInit()
YusukeWakuta 26:f14579683f98 100 {
taurin 4:450cafd95ac3 101 drugServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:e052602db102 102 return true;
taurin 0:e052602db102 103 }
taurin 0:e052602db102 104
YusukeWakuta 26:f14579683f98 105 void sendDatas()
YusukeWakuta 24:d416722b4aad 106 {
YusukeWakuta 26:f14579683f98 107 if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
taurin 4:450cafd95ac3 108 }
taurin 4:450cafd95ac3 109 }
taurin 4:450cafd95ac3 110
YusukeWakuta 42:bf98a29e81ac 111 void MpuInit()
YusukeWakuta 42:bf98a29e81ac 112 {
YusukeWakuta 42:bf98a29e81ac 113 i2c_mpu.frequency(400000); // use fast (400 kHz) I2C
YusukeWakuta 42:bf98a29e81ac 114 t.start();
YusukeWakuta 42:bf98a29e81ac 115 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
YusukeWakuta 42:bf98a29e81ac 116 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
YusukeWakuta 42:bf98a29e81ac 117 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 118 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
YusukeWakuta 42:bf98a29e81ac 119 Thread::wait(100);
YusukeWakuta 42:bf98a29e81ac 120 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
YusukeWakuta 42:bf98a29e81ac 121 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
YusukeWakuta 42:bf98a29e81ac 122 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
YusukeWakuta 42:bf98a29e81ac 123 mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
YusukeWakuta 42:bf98a29e81ac 124 Thread::wait(200);
YusukeWakuta 42:bf98a29e81ac 125 } else {
YusukeWakuta 42:bf98a29e81ac 126 }
YusukeWakuta 42:bf98a29e81ac 127 } else {
YusukeWakuta 42:bf98a29e81ac 128 //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
YusukeWakuta 42:bf98a29e81ac 129 }
YusukeWakuta 42:bf98a29e81ac 130 }
YusukeWakuta 42:bf98a29e81ac 131
YusukeWakuta 42:bf98a29e81ac 132
YusukeWakuta 42:bf98a29e81ac 133 void mpuProcessing(void const *arg)
YusukeWakuta 42:bf98a29e81ac 134 {
YusukeWakuta 42:bf98a29e81ac 135 MpuInit();
YusukeWakuta 42:bf98a29e81ac 136 while(1) {
YusukeWakuta 42:bf98a29e81ac 137 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
YusukeWakuta 42:bf98a29e81ac 138 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 139 mpu6050.getAres();
YusukeWakuta 42:bf98a29e81ac 140 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 141 ay = (float)accelCount[1]*aRes - accelBias[1];
YusukeWakuta 42:bf98a29e81ac 142 az = (float)accelCount[2]*aRes - accelBias[2];
YusukeWakuta 42:bf98a29e81ac 143 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 144 mpu6050.getGres();
YusukeWakuta 42:bf98a29e81ac 145 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
YusukeWakuta 42:bf98a29e81ac 146 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
YusukeWakuta 42:bf98a29e81ac 147 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
YusukeWakuta 42:bf98a29e81ac 148 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
YusukeWakuta 42:bf98a29e81ac 149 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
YusukeWakuta 42:bf98a29e81ac 150 }
YusukeWakuta 42:bf98a29e81ac 151 Now = t.read_us();
YusukeWakuta 42:bf98a29e81ac 152 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
YusukeWakuta 42:bf98a29e81ac 153 lastUpdate = Now;
YusukeWakuta 42:bf98a29e81ac 154 sum += deltat;
YusukeWakuta 42:bf98a29e81ac 155 sumCount++;
YusukeWakuta 42:bf98a29e81ac 156 if(lastUpdate - firstUpdate > 10000000.0f) {
YusukeWakuta 42:bf98a29e81ac 157 beta = 0.04; // decrease filter gain after stabilized
YusukeWakuta 42:bf98a29e81ac 158 zeta = 0.015; // increasey bias drift gain after stabilized
YusukeWakuta 42:bf98a29e81ac 159 }
YusukeWakuta 42:bf98a29e81ac 160 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
YusukeWakuta 42:bf98a29e81ac 161 delt_t = t.read_ms() - count;
YusukeWakuta 42:bf98a29e81ac 162 if (delt_t > MPU_DELT_MIN) {
YusukeWakuta 42:bf98a29e81ac 163 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
YusukeWakuta 42:bf98a29e81ac 164 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
YusukeWakuta 42:bf98a29e81ac 165 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
YusukeWakuta 42:bf98a29e81ac 166 pitch *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 167 yaw *= 180.0f / PI;
YusukeWakuta 42:bf98a29e81ac 168 roll *= 180.0f / PI;
YusukeWakuta 48:0bd406fa4a7f 169 // myled= !myled;
YusukeWakuta 42:bf98a29e81ac 170 count = t.read_ms();
YusukeWakuta 42:bf98a29e81ac 171 sum = 0;
YusukeWakuta 42:bf98a29e81ac 172 sumCount = 0;
YusukeWakuta 42:bf98a29e81ac 173 }
tsumagari 64:9d7582920f50 174 gyro_c[0]=(char)((int)pitch);
tsumagari 64:9d7582920f50 175 gyro_c[1]=(char)(int)((pitch - (int)pitch)*100);
tsumagari 64:9d7582920f50 176 gyro_c[2]=(char)((int)roll);
tsumagari 64:9d7582920f50 177 gyro_c[3]=(char)(int)((roll - (int)roll)*100);
tsumagari 64:9d7582920f50 178 gyro_c[4]=(char)((int)yaw);
tsumagari 64:9d7582920f50 179 gyro_c[5]=(char)(int)((yaw - (int)yaw)*100);
tsumagari 67:94fe2180f39a 180 debugMPU("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw);
YusukeWakuta 42:bf98a29e81ac 181 Thread::wait(1);
YusukeWakuta 42:bf98a29e81ac 182 }//while(1)
YusukeWakuta 42:bf98a29e81ac 183 }
YusukeWakuta 42:bf98a29e81ac 184
YusukeWakuta 42:bf98a29e81ac 185
YusukeWakuta 26:f14579683f98 186 bool inaInit()
YusukeWakuta 26:f14579683f98 187 {
YusukeWakuta 26:f14579683f98 188 if(!VCmonitor.isExist()) {
taurin 0:e052602db102 189 pc.printf("VCmonitor NOT FOUND\n");
taurin 0:e052602db102 190 return false;
taurin 0:e052602db102 191 }
taurin 0:e052602db102 192 ina_val = 0;
YusukeWakuta 26:f14579683f98 193 if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
taurin 0:e052602db102 194 pc.printf("VCmonitor READ ERROR\n");
taurin 0:e052602db102 195 return false;
taurin 0:e052602db102 196 }
taurin 0:e052602db102 197 VCmonitor.setCurrentCalibration();
taurin 0:e052602db102 198 return true;
taurin 0:e052602db102 199 }
taurin 0:e052602db102 200
YusukeWakuta 57:d7b709dd1c4f 201 //動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
YusukeWakuta 57:d7b709dd1c4f 202 double ConvertDeg(double servo)
YusukeWakuta 57:d7b709dd1c4f 203 {
YusukeWakuta 62:1db967d29809 204 return 0.0011 * pow(servo,3) + 0.017 * pow(servo,2) + 2.3019 * servo - 0.2269;
YusukeWakuta 62:1db967d29809 205 //return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
YusukeWakuta 57:d7b709dd1c4f 206 }
YusukeWakuta 57:d7b709dd1c4f 207
YusukeWakuta 57:d7b709dd1c4f 208 //ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
YusukeWakuta 57:d7b709dd1c4f 209 double GetValueByHorn(double deg)
YusukeWakuta 57:d7b709dd1c4f 210 {
YusukeWakuta 57:d7b709dd1c4f 211 return deg*(0.10/25.7);
YusukeWakuta 57:d7b709dd1c4f 212 }
YusukeWakuta 57:d7b709dd1c4f 213
YusukeWakuta 57:d7b709dd1c4f 214 double GetFloatByErebon(double erebonDeg)
YusukeWakuta 57:d7b709dd1c4f 215 {
YusukeWakuta 57:d7b709dd1c4f 216 double servoDeg = ConvertDeg(erebonDeg);
YusukeWakuta 61:988e3f4280ac 217 return GetValueByHorn(servoDeg);
YusukeWakuta 57:d7b709dd1c4f 218 }
tsumagari 67:94fe2180f39a 219 // configファイル作成
tsumagari 67:94fe2180f39a 220 int makeConfigFile() {
tsumagari 67:94fe2180f39a 221 FILE *fp;
tsumagari 67:94fe2180f39a 222 if((fp = fopen(configfilename, "w")) == NULL) {
tsumagari 67:94fe2180f39a 223 pc.printf("can't open %s\n", configfilename);
tsumagari 67:94fe2180f39a 224 return -1;
tsumagari 67:94fe2180f39a 225 }
tsumagari 67:94fe2180f39a 226 fprintf(fp, "// This is Yokutan %s.\n", IsRPin ? "R" : "L");
tsumagari 67:94fe2180f39a 227 fprintf(fp, "eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg\n");
tsumagari 67:94fe2180f39a 228 fclose(fp);
tsumagari 67:94fe2180f39a 229 return 0;
tsumagari 67:94fe2180f39a 230 }
tsumagari 67:94fe2180f39a 231
tsumagari 67:94fe2180f39a 232 // config内容をファイルに追記
tsumagari 67:94fe2180f39a 233 int writeConfig() {
tsumagari 67:94fe2180f39a 234 FILE *fp;
tsumagari 67:94fe2180f39a 235 if((fp = fopen(configfilename, "a")) == NULL) {
tsumagari 67:94fe2180f39a 236 pc.printf("can't open %s\n", configfilename);
tsumagari 67:94fe2180f39a 237 return -1;
tsumagari 67:94fe2180f39a 238 }
tsumagari 67:94fe2180f39a 239 fprintf(fp, "%f,%f,%f,%f\n", eruronTrim, drugTrim, eruronMoveDeg, drugMoveDeg);
tsumagari 67:94fe2180f39a 240 fclose(fp);
tsumagari 67:94fe2180f39a 241 return 0;
tsumagari 67:94fe2180f39a 242 }
tsumagari 67:94fe2180f39a 243 // configの最新情報を読み込み
tsumagari 67:94fe2180f39a 244 int readConfig() {
tsumagari 67:94fe2180f39a 245 FILE *fp;
tsumagari 67:94fe2180f39a 246 char s[256];
tsumagari 67:94fe2180f39a 247 if((fp = fopen(configfilename, "r")) == NULL) {
tsumagari 67:94fe2180f39a 248 return -1;
tsumagari 67:94fe2180f39a 249 }
tsumagari 67:94fe2180f39a 250 while(fgets(s, 255, fp) != NULL) {
tsumagari 67:94fe2180f39a 251 sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg);
tsumagari 67:94fe2180f39a 252 }
tsumagari 67:94fe2180f39a 253 fclose(fp);
tsumagari 67:94fe2180f39a 254 debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
tsumagari 67:94fe2180f39a 255 return 0;
tsumagari 67:94fe2180f39a 256 }
YusukeWakuta 57:d7b709dd1c4f 257
YusukeWakuta 26:f14579683f98 258 void init()
YusukeWakuta 26:f14579683f98 259 {
tsumagari 67:94fe2180f39a 260 if(readConfig() == -1) {
tsumagari 67:94fe2180f39a 261 makeConfigFile();
tsumagari 67:94fe2180f39a 262 if(IsRPin) {
tsumagari 67:94fe2180f39a 263 eruronTrim = ERURON_TRIM_INI_R;
tsumagari 67:94fe2180f39a 264 drugTrim = DRUG_TRIM_INI_R;
tsumagari 67:94fe2180f39a 265 eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
tsumagari 67:94fe2180f39a 266 drugMoveDeg =DRUG_MOVE_DEG_INI_R;
tsumagari 67:94fe2180f39a 267 } else {
tsumagari 67:94fe2180f39a 268 eruronTrim = ERURON_TRIM_INI_L;
tsumagari 67:94fe2180f39a 269 drugTrim = DRUG_TRIM_INI_L;
tsumagari 67:94fe2180f39a 270 eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
tsumagari 67:94fe2180f39a 271 drugMoveDeg = DRUG_MOVE_DEG_INI_L;
tsumagari 67:94fe2180f39a 272 }
taurin 12:fd9d241843f4 273 }
taurin 0:e052602db102 274 SERVO_FLAG = servoInit();
taurin 0:e052602db102 275 INA_FLAG = inaInit();
taurin 4:450cafd95ac3 276 sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
taurin 16:82310bf7c326 277 // toStringTicker.attach(&toString,0.5);
taurin 16:82310bf7c326 278 receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
taurin 0:e052602db102 279 }
taurin 0:e052602db102 280
YusukeWakuta 26:f14579683f98 281 void updateDatas()
YusukeWakuta 26:f14579683f98 282 {
YusukeWakuta 26:f14579683f98 283 if(INA_FLAG) {
taurin 0:e052602db102 284 int tmp = VCmonitor.getVoltage(&V);
taurin 0:e052602db102 285 tmp = VCmonitor.getCurrent(&C);
taurin 0:e052602db102 286 }
YusukeWakuta 42:bf98a29e81ac 287
YusukeWakuta 26:f14579683f98 288 for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
tsumagari 22:b38bc18ec3a1 289 toSendDatas[i] = gyro_c[i];
tsumagari 22:b38bc18ec3a1 290 }
tsumagari 22:b38bc18ec3a1 291 // toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
YusukeWakuta 40:ad98da5da7bf 292 toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)V;
tsumagari 71:a00561d455d1 293 pc.printf("test\n\r");
taurin 0:e052602db102 294 }
taurin 0:e052602db102 295
YusukeWakuta 26:f14579683f98 296 void receiveDatas()
YusukeWakuta 26:f14579683f98 297 {
YusukeWakuta 61:988e3f4280ac 298 if(can.read(recmsg)) { //ここの中でpc.printfすると重すぎて固まるので注意
tsumagari 71:a00561d455d1 299 for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
YusukeWakuta 60:a45dc19a6001 300 floatValues[i] = recmsg.data[i];
taurin 0:e052602db102 301 }
YusukeWakuta 61:988e3f4280ac 302 drugInput = recmsg.data[5]- '0';
tsumagari 71:a00561d455d1 303 servoOff = recmsg.data[6]-'0';
YusukeWakuta 60:a45dc19a6001 304 eruronfloat = atof(floatValues);
taurin 1:9cc932a16d17 305 led1 = !led1;
taurin 0:e052602db102 306 }
taurin 0:e052602db102 307 }
taurin 0:e052602db102 308
YusukeWakuta 42:bf98a29e81ac 309 double calcPulse(float analog)
YusukeWakuta 26:f14579683f98 310 {
tsumagari 53:3eeaafa49707 311 return (0.0006 + (analog)*(0.00240-0.00060) );
taurin 1:9cc932a16d17 312 }
taurin 1:9cc932a16d17 313
YusukeWakuta 29:516a5d383488 314 void WriteServo()
YusukeWakuta 26:f14579683f98 315 {
YusukeWakuta 62:1db967d29809 316 // for(int i = 0; i< 10; i++) {
YusukeWakuta 61:988e3f4280ac 317 // pc.printf("%c",floatValues[i]);
YusukeWakuta 61:988e3f4280ac 318 // }
YusukeWakuta 61:988e3f4280ac 319 // pc.printf(" : %f",eruronfloat);
YusukeWakuta 61:988e3f4280ac 320 // pc.printf("\n\r");
YusukeWakuta 61:988e3f4280ac 321 drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
YusukeWakuta 59:f007e543f8c9 322 eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
tsumagari 68:ae3c3500074c 323 pc.printf("ef:%10f %5f\n\r",eruronfloat,drugInput);
taurin 4:450cafd95ac3 324 }
taurin 4:450cafd95ac3 325
YusukeWakuta 26:f14579683f98 326 void setTrim()
YusukeWakuta 26:f14579683f98 327 {
YusukeWakuta 44:624a4469ae21 328 led2 = 1;
YusukeWakuta 26:f14579683f98 329 if(EDstatePin) {
tsumagari 69:073116d31a56 330 eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 50:b3a8f8e88c50 331 eruronServo.pulsewidth(calcPulse(eruronTrim));
YusukeWakuta 26:f14579683f98 332 } else {
tsumagari 69:073116d31a56 333 drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 26:f14579683f98 334 drugServo.pulsewidth(calcPulse(drugTrim));
taurin 12:fd9d241843f4 335 }
YusukeWakuta 14:1f6dd929d7de 336 //pc.printf("eruronTrim:%f drugTrim:%f\n\r",eruronTrim,drugTrim);
YusukeWakuta 26:f14579683f98 337 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 59:f007e543f8c9 338 pc.printf("eMD:%f dMD:%f ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 339 }
taurin 4:450cafd95ac3 340
YusukeWakuta 26:f14579683f98 341 void setMaxDeg()
YusukeWakuta 26:f14579683f98 342 {
taurin 4:450cafd95ac3 343 led4 = 1;
tsumagari 69:073116d31a56 344 float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
tsumagari 69:073116d31a56 345 float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
YusukeWakuta 26:f14579683f98 346 if(EDstatePin) {
YusukeWakuta 26:f14579683f98 347 eruronMoveDeg = eruronTemp-eruronTrim;
YusukeWakuta 49:8522856fe0cd 348 eruronServo.pulsewidth(calcPulse(eruronTemp));
YusukeWakuta 26:f14579683f98 349 } else {
tsumagari 68:ae3c3500074c 350 drugMoveDeg = drugTemp-drugTrim;
YusukeWakuta 26:f14579683f98 351 drugServo.pulsewidth(calcPulse(drugTemp));
taurin 12:fd9d241843f4 352 }
YusukeWakuta 14:1f6dd929d7de 353 pc.printf("eruronTrim:%f drugTrim:%f ",eruronTrim,drugTrim);
YusukeWakuta 59:f007e543f8c9 354 pc.printf("eMD:%f dMD:%f ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
taurin 4:450cafd95ac3 355 wait_us(10);
taurin 1:9cc932a16d17 356 }
taurin 0:e052602db102 357
YusukeWakuta 26:f14579683f98 358 int main()
YusukeWakuta 26:f14579683f98 359 {
taurin 0:e052602db102 360 init();
YusukeWakuta 42:bf98a29e81ac 361 Thread mpu_thread(&mpuProcessing);
YusukeWakuta 26:f14579683f98 362
YusukeWakuta 24:d416722b4aad 363 // start motion
tsumagari 33:d075918d4846 364 // gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 26:f14579683f98 365
YusukeWakuta 26:f14579683f98 366 while(1) {
tsumagari 68:ae3c3500074c 367 if(InSetModePin){
tsumagari 68:ae3c3500074c 368 do{
tsumagari 68:ae3c3500074c 369 if(TrimMaxDegPin)
tsumagari 68:ae3c3500074c 370 setTrim();
tsumagari 68:ae3c3500074c 371 else
tsumagari 68:ae3c3500074c 372 setMaxDeg();
tsumagari 68:ae3c3500074c 373 }while(InSetModePin);
tsumagari 67:94fe2180f39a 374 writeConfig();
taurin 4:450cafd95ac3 375 }
taurin 4:450cafd95ac3 376 led4 = 0;
YusukeWakuta 44:624a4469ae21 377 led2 = 0;
tsumagari 68:ae3c3500074c 378 receiveDatas();
tsumagari 71:a00561d455d1 379 sendDatas();
tsumagari 71:a00561d455d1 380 pc.printf("V:%f\n\r",V);
taurin 1:9cc932a16d17 381 WriteServo();
taurin 4:450cafd95ac3 382 updateDatas();
tsumagari 22:b38bc18ec3a1 383 led3 = !led3;
taurin 2:7fcb4f970a02 384 wait(WAIT_LOOP_TIME);
taurin 0:e052602db102 385 }
taurin 0:e052602db102 386 }