航空研究会 / Mbed 2 deprecated hassi_14

sdカード完成

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp@13:d8f49328c57f, 2019-02-12 (annotated)

Committer:
taknokolat
Date:
Tue Feb 12 06:28:06 2019 +0000
Revision:
13:d8f49328c57f
Parent:
12:a2c6207cbce3 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:84ddd6d354e1 1 #include "mbed.h"
Skykon 3:c18342e4fddd 2 #include "MPU6050_DMP6.h"
taknokolat 6:166746820555 3 #include "HMC5883L.h"
taknokolat 13:d8f49328c57f 4 #include "SDFileSystem.h"
Skykon 3:c18342e4fddd 5
Skykon 3:c18342e4fddd 6 //MPU_check用
Skykon 3:c18342e4fddd 7 #define PI 3.14159265358979
HARUKIDELTA 0:84ddd6d354e1 8
taknokolat 11:d44d137831b9 9 #define servo_NEUTRAL_R 1614
taknokolat 11:d44d137831b9 10 #define servo_NEUTRAL_L 1621
HARUKIDELTA 0:84ddd6d354e1 11 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 12 #define servo_slow_FORWARD_L 1560
taknokolat 12:a2c6207cbce3 13 #define servo_low_FORWARD_R 1560 //RasPi速さ調節用
taknokolat 12:a2c6207cbce3 14 #define servo_high_FORWARD_R 1860
taknokolat 12:a2c6207cbce3 15 #define servo_low_FORWARD_L 1560
taknokolat 12:a2c6207cbce3 16 #define servo_high_FORWARD_L 1860
taknokolat 12:a2c6207cbce3 17 #define servo_low_back_R 1360
taknokolat 12:a2c6207cbce3 18 #define servo_high_back_R 1160
taknokolat 12:a2c6207cbce3 19 #define servo_low_back_L 1360
taknokolat 12:a2c6207cbce3 20 #define servo_high_back_L 1160
taknokolat 13:d8f49328c57f 21 #define turntable_NEUTRAL 1180 //カメラ台座のサーボ
taknokolat 13:d8f49328c57f 22 #define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
taknokolat 13:d8f49328c57f 23 #define focus_NEUTRAL 1455 //焦点合わせ用サーボ
HARUKIDELTA 0:84ddd6d354e1 24
HARUKIDELTA 0:84ddd6d354e1 25 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 26 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 27 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 28 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 29 #define MOVE_BACK 4
Skykon 3:c18342e4fddd 30 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり
HARUKIDELTA 0:84ddd6d354e1 31 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 32 #define GOAL_RIGHT 7
Skykon 3:c18342e4fddd 33 #define MAX_FORWARD 8 //はやい_姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 34 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 35
taknokolat 12:a2c6207cbce3 36 int Servo_NEUTRAL_R; //SDcard用宣言
taknokolat 12:a2c6207cbce3 37 int Servo_NEUTRAL_L;
taknokolat 12:a2c6207cbce3 38 int Servo_FORWARD_R;
taknokolat 12:a2c6207cbce3 39 int Servo_FORWARD_L;
taknokolat 12:a2c6207cbce3 40 int Servo_back_R;
taknokolat 12:a2c6207cbce3 41 int Servo_back_L;
taknokolat 12:a2c6207cbce3 42 int Servo_slow_FORWARD_R;
taknokolat 12:a2c6207cbce3 43 int Servo_slow_FORWARD_L;
taknokolat 12:a2c6207cbce3 44 int Turntable_NEUTRAL;
taknokolat 13:d8f49328c57f 45 int Matchspeed; //機体角度調整
taknokolat 13:d8f49328c57f 46 int Focus_NEUTRAL;//カメラ焦点
Skykon 3:c18342e4fddd 47
taknokolat 1:290e621741fd 48 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 49 void getSF_Serial_pi();
taknokolat 1:290e621741fd 50
Skykon 3:c18342e4fddd 51 //MPU_check用
Skykon 3:c18342e4fddd 52 void SensingMPU();
taknokolat 7:8989a4b84695 53
taknokolat 7:8989a4b84695 54 void MoveCansat(char g_landingcommand);
Skykon 3:c18342e4fddd 55 void setup();
Skykon 3:c18342e4fddd 56 void Init_sensors();
Skykon 3:c18342e4fddd 57 void DisplayClock();
taknokolat 4:67f705d42f1e 58 void DebugPrint();
taknokolat 6:166746820555 59 void SensingHMC();
taknokolat 7:8989a4b84695 60 void MoveCameraBoard();
taknokolat 7:8989a4b84695 61 void MatchPosition();
taknokolat 7:8989a4b84695 62 void FocusAdjust();
Skykon 3:c18342e4fddd 63
taknokolat 12:a2c6207cbce3 64 //sd設定
taknokolat 12:a2c6207cbce3 65 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
taknokolat 12:a2c6207cbce3 66
taknokolat 13:d8f49328c57f 67 int SetOptions(int Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
taknokolat 13:d8f49328c57f 68 int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
taknokolat 12:a2c6207cbce3 69 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 13:d8f49328c57f 70 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL
taknokolat 12:a2c6207cbce3 71 );
taknokolat 12:a2c6207cbce3 72
taknokolat 6:166746820555 73 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
taknokolat 6:166746820555 74 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
Skykon 3:c18342e4fddd 75
taknokolat 7:8989a4b84695 76 bool setupFlag = false;
taknokolat 7:8989a4b84695 77 bool CameraDegFlag = false;
taknokolat 7:8989a4b84695 78 bool jevoisFlag = true;
taknokolat 4:67f705d42f1e 79
Skykon 3:c18342e4fddd 80 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 81
Skykon 3:c18342e4fddd 82 Timer t;
Skykon 3:c18342e4fddd 83
taknokolat 11:d44d137831b9 84 RawSerial pc(PA_2,PA_3,115200); //uart2
taknokolat 11:d44d137831b9 85 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 12:a2c6207cbce3 86 RawSerial pc2(PA_11,PB_12,115200); //uart6
taknokolat 11:d44d137831b9 87 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 11:d44d137831b9 88
HARUKIDELTA 5:8bfe95431ec0 89 //PWM pin宣言
taknokolat 6:166746820555 90 PwmOut servoR(PC_6); //TIM3_CH1 車輪右
taknokolat 6:166746820555 91 PwmOut servoL(PC_7); //TIM3_CH2 車輪左
HARUKIDELTA 5:8bfe95431ec0 92 PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
HARUKIDELTA 5:8bfe95431ec0 93 PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
HARUKIDELTA 5:8bfe95431ec0 94 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
HARUKIDELTA 5:8bfe95431ec0 95 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
HARUKIDELTA 5:8bfe95431ec0 96 PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
taknokolat 12:a2c6207cbce3 97 //PwmOut servoParachute(PA_11); //skipper USB端子より パラシュート切り離し
Skykon 3:c18342e4fddd 98
HARUKIDELTA 5:8bfe95431ec0 99 /*通信用のpinは
HARUKIDELTA 5:8bfe95431ec0 100 PA_3(UART2_Rx) skipperウラ
HARUKIDELTA 5:8bfe95431ec0 101 PA_12(UART6_Rx) skipperオモテ USB端子より
HARUKIDELTA 5:8bfe95431ec0 102 PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
HARUKIDELTA 5:8bfe95431ec0 103 */
HARUKIDELTA 5:8bfe95431ec0 104
HARUKIDELTA 5:8bfe95431ec0 105 /*超音波はRaspberryPiに積む*/
HARUKIDELTA 5:8bfe95431ec0 106
HARUKIDELTA 5:8bfe95431ec0 107 //外付けコンパス
taknokolat 6:166746820555 108 HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
HARUKIDELTA 0:84ddd6d354e1 109
taknokolat 2:f30666d7838b 110 char g_landingcommand='N';
taknokolat 1:290e621741fd 111
Skykon 3:c18342e4fddd 112 //MPU_check用
Skykon 3:c18342e4fddd 113 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 114
Skykon 3:c18342e4fddd 115
HARUKIDELTA 0:84ddd6d354e1 116 int main() {
taknokolat 1:290e621741fd 117
Skykon 3:c18342e4fddd 118 //MPU_check
Skykon 3:c18342e4fddd 119 setup();
Skykon 3:c18342e4fddd 120
taknokolat 11:d44d137831b9 121 servoCameradeg.pulsewidth_us(1400);
taknokolat 11:d44d137831b9 122
taknokolat 1:290e621741fd 123 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 124 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 12:a2c6207cbce3 125 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 126
HARUKIDELTA 0:84ddd6d354e1 127 while(1) {
taknokolat 7:8989a4b84695 128 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 129 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 130
taknokolat 10:63fe920595a7 131 MoveCameraBoard();
taknokolat 10:63fe920595a7 132
taknokolat 7:8989a4b84695 133 if(g_landingcommand!='N') MatchPosition();
taknokolat 8:d11a59d2a2f1 134
taknokolat 8:d11a59d2a2f1 135 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 8:d11a59d2a2f1 136 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 137
taknokolat 7:8989a4b84695 138 MoveCansat(g_landingcommand);
Skykon 3:c18342e4fddd 139 wait_ms(23);
taknokolat 8:d11a59d2a2f1 140
taknokolat 7:8989a4b84695 141 if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 142 else NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 143 }
HARUKIDELTA 0:84ddd6d354e1 144 }
HARUKIDELTA 0:84ddd6d354e1 145
Skykon 3:c18342e4fddd 146
taknokolat 1:290e621741fd 147 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 148 {
taknokolat 2:f30666d7838b 149 //NVIC_DisableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 150 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 151 switch(g_landingcommand){
taknokolat 2:f30666d7838b 152 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 0:84ddd6d354e1 153 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 154 servoL.pulsewidth_us(servo_NEUTRAL_L);
taknokolat 2:f30666d7838b 155 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 156 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 157 break;
taknokolat 1:290e621741fd 158
taknokolat 2:f30666d7838b 159 case 'Y': //MOVE_FORWARD
taknokolat 12:a2c6207cbce3 160 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 161 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 2:f30666d7838b 162 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 163 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 164 break;
taknokolat 1:290e621741fd 165
taknokolat 12:a2c6207cbce3 166 case 'l': //MOVE_LEFT Low Speed
taknokolat 12:a2c6207cbce3 167 servoR.pulsewidth_us(servo_low_FORWARD_R);
taknokolat 12:a2c6207cbce3 168 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:a2c6207cbce3 169 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:a2c6207cbce3 170
taknokolat 12:a2c6207cbce3 171 case 'L': //MOVE_LEFT High Speed
taknokolat 12:a2c6207cbce3 172 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 2:f30666d7838b 173 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 174 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 175
taknokolat 12:a2c6207cbce3 176 case 'r': //MOVE_RIGHT Low Speed
taknokolat 12:a2c6207cbce3 177 servoL.pulsewidth_us(servo_low_FORWARD_L);
taknokolat 2:f30666d7838b 178 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 179 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 180 break;
taknokolat 12:a2c6207cbce3 181
taknokolat 12:a2c6207cbce3 182 case 'R': //MOVE_RIGHT High Speed
taknokolat 12:a2c6207cbce3 183 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 2:f30666d7838b 184 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 185 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 186 break;
taknokolat 1:290e621741fd 187
taknokolat 1:290e621741fd 188 case 'G': //GOAL_FORWARD
HARUKIDELTA 0:84ddd6d354e1 189 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 190 servoL.pulsewidth_us(servo_slow_FORWARD_L);
taknokolat 2:f30666d7838b 191 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 192 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 193 break;
taknokolat 12:a2c6207cbce3 194
taknokolat 12:a2c6207cbce3 195 case '1': //MOVE_FORWARD Speed Level 1
taknokolat 12:a2c6207cbce3 196 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 197 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:a2c6207cbce3 198 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:a2c6207cbce3 199 wait(1);
taknokolat 12:a2c6207cbce3 200 do{
taknokolat 12:a2c6207cbce3 201 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:a2c6207cbce3 202 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:a2c6207cbce3 203 break;
taknokolat 12:a2c6207cbce3 204
taknokolat 12:a2c6207cbce3 205 case '2': //MOVE_FORWARD Speed Level 2
taknokolat 12:a2c6207cbce3 206 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 207 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:a2c6207cbce3 208 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:a2c6207cbce3 209 wait(2);
taknokolat 12:a2c6207cbce3 210 do{
taknokolat 12:a2c6207cbce3 211 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:a2c6207cbce3 212 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:a2c6207cbce3 213 break;
taknokolat 12:a2c6207cbce3 214
taknokolat 12:a2c6207cbce3 215 case '3': //MOVE_FORWARD Speed Level 3
taknokolat 12:a2c6207cbce3 216 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 217 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:a2c6207cbce3 218 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:a2c6207cbce3 219 wait(3);
taknokolat 12:a2c6207cbce3 220 do{
taknokolat 12:a2c6207cbce3 221 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:a2c6207cbce3 222 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:a2c6207cbce3 223 break;
taknokolat 12:a2c6207cbce3 224
taknokolat 12:a2c6207cbce3 225 case '4': //MOVE_FORWARD Speed Level 4
taknokolat 12:a2c6207cbce3 226 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 227 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 12:a2c6207cbce3 228 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 12:a2c6207cbce3 229 wait(4);
taknokolat 12:a2c6207cbce3 230 do{
taknokolat 12:a2c6207cbce3 231 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 12:a2c6207cbce3 232 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:a2c6207cbce3 233 break;
taknokolat 7:8989a4b84695 234
taknokolat 12:a2c6207cbce3 235 case '5': //MOVE_FORWARD Speed Level 5
taknokolat 12:a2c6207cbce3 236 servoR.pulsewidth_us(servo_high_FORWARD_R);
taknokolat 12:a2c6207cbce3 237 servoL.pulsewidth_us(servo_high_FORWARD_L);
taknokolat 7:8989a4b84695 238 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 239 wait(5);
taknokolat 10:63fe920595a7 240 do{
taknokolat 10:63fe920595a7 241 }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
taknokolat 7:8989a4b84695 242 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 243 break;
taknokolat 7:8989a4b84695 244
taknokolat 7:8989a4b84695 245 case 'M': //MatchPosition
taknokolat 13:d8f49328c57f 246 servoR.pulsewidth_us(matchspeed);
taknokolat 7:8989a4b84695 247 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 248 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 249 break;
taknokolat 7:8989a4b84695 250
taknokolat 12:a2c6207cbce3 251 case 'P': //jevoisからraspberry piへの切り替え
taknokolat 7:8989a4b84695 252 jevoisFlag = false;
taknokolat 7:8989a4b84695 253 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 254 break;
taknokolat 12:a2c6207cbce3 255
taknokolat 12:a2c6207cbce3 256 case 'S':
taknokolat 12:a2c6207cbce3 257 /*RasPiからの超音波判定(プログラムスタート部)*/
taknokolat 12:a2c6207cbce3 258 break;
taknokolat 1:290e621741fd 259
taknokolat 1:290e621741fd 260 default :
taknokolat 2:f30666d7838b 261 //NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 262 //NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 263 break;
taknokolat 1:290e621741fd 264
HARUKIDELTA 0:84ddd6d354e1 265 }
taknokolat 7:8989a4b84695 266
HARUKIDELTA 0:84ddd6d354e1 267 return;
taknokolat 1:290e621741fd 268 }
taknokolat 1:290e621741fd 269
taknokolat 7:8989a4b84695 270 void MoveCameraBoard(){
taknokolat 7:8989a4b84695 271 MoveCansat('N');
taknokolat 10:63fe920595a7 272 g_landingcommand='N';
taknokolat 11:d44d137831b9 273 servoTurnTable.pulsewidth_us(1800);
taknokolat 11:d44d137831b9 274 wait_ms(600);
taknokolat 13:d8f49328c57f 275 servoTurnTable.pulsewidth_us(Servo_NEUTRAL_R);
taknokolat 7:8989a4b84695 276 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 277 else wait(1);
taknokolat 7:8989a4b84695 278
taknokolat 7:8989a4b84695 279 if(g_landingcommand!='N') return;
taknokolat 7:8989a4b84695 280 if(!CameraDegFlag){
taknokolat 7:8989a4b84695 281 servoCameradeg.pulsewidth_us(1800);
taknokolat 7:8989a4b84695 282 CameraDegFlag=!CameraDegFlag;
taknokolat 7:8989a4b84695 283 }else{
taknokolat 11:d44d137831b9 284 servoCameradeg.pulsewidth_us(1400);
taknokolat 7:8989a4b84695 285 CameraDegFlag = !CameraDegFlag;
taknokolat 7:8989a4b84695 286 }
taknokolat 7:8989a4b84695 287
taknokolat 7:8989a4b84695 288 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 289 else wait(1);
taknokolat 7:8989a4b84695 290
taknokolat 7:8989a4b84695 291 return;
taknokolat 7:8989a4b84695 292 }
taknokolat 7:8989a4b84695 293
taknokolat 7:8989a4b84695 294 void MatchPosition(){
taknokolat 7:8989a4b84695 295 SensingMPU();
taknokolat 7:8989a4b84695 296 SensingHMC();
taknokolat 7:8989a4b84695 297 DebugPrint();
taknokolat 7:8989a4b84695 298
taknokolat 9:21cd5d18ad9e 299 while(nowAngle[YAW] <= nowAngle_HMC+5 && nowAngle[YAW] >= nowAngle_HMC-5){
taknokolat 7:8989a4b84695 300 MoveCansat('M');
taknokolat 9:21cd5d18ad9e 301 SensingMPU();
taknokolat 7:8989a4b84695 302 }
taknokolat 7:8989a4b84695 303 return;
taknokolat 7:8989a4b84695 304 }
taknokolat 7:8989a4b84695 305
taknokolat 7:8989a4b84695 306 void FocusAdjust(){
taknokolat 13:d8f49328c57f 307 servoCameraPinto.pulsewidth_us(focus_NEUTRAL-200);
taknokolat 11:d44d137831b9 308 pc.printf("set\r\n");
taknokolat 11:d44d137831b9 309 wait(1);
taknokolat 7:8989a4b84695 310
taknokolat 13:d8f49328c57f 311 servoCameraPinto.pulsewidth_us(focus_NEUTRAL+30);
taknokolat 11:d44d137831b9 312 pc.printf("check\r\n");
taknokolat 11:d44d137831b9 313 wait(3);
taknokolat 13:d8f49328c57f 314 servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
taknokolat 11:d44d137831b9 315
taknokolat 7:8989a4b84695 316 return;
taknokolat 7:8989a4b84695 317 }
taknokolat 7:8989a4b84695 318
taknokolat 1:290e621741fd 319 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 320
taknokolat 1:290e621741fd 321
taknokolat 1:290e621741fd 322 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 323
taknokolat 1:290e621741fd 324 static int bufcounter=0;
taknokolat 1:290e621741fd 325
taknokolat 1:290e621741fd 326
taknokolat 1:290e621741fd 327
taknokolat 1:290e621741fd 328 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 329
taknokolat 1:290e621741fd 330 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 331 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 332 //pc.printf("x");
taknokolat 1:290e621741fd 333 return;
taknokolat 1:290e621741fd 334 }
taknokolat 1:290e621741fd 335
taknokolat 1:290e621741fd 336
taknokolat 1:290e621741fd 337
taknokolat 1:290e621741fd 338 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 339
Skykon 3:c18342e4fddd 340 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 341
taknokolat 1:290e621741fd 342 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 343
taknokolat 1:290e621741fd 344 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 345 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 346 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 347 //wait_ms(20);
taknokolat 1:290e621741fd 348 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 349 bufcounter = 0;
taknokolat 1:290e621741fd 350 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 351 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 352 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 353 }
taknokolat 1:290e621741fd 354
taknokolat 1:290e621741fd 355 else if(bufcounter>=5){
taknokolat 1:290e621741fd 356 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 357 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 358 bufcounter = 0;
taknokolat 1:290e621741fd 359 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 360 }
taknokolat 1:290e621741fd 361 }
taknokolat 1:290e621741fd 362
taknokolat 1:290e621741fd 363
taknokolat 1:290e621741fd 364 }
taknokolat 1:290e621741fd 365
taknokolat 1:290e621741fd 366
taknokolat 12:a2c6207cbce3 367 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 368
taknokolat 7:8989a4b84695 369 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 370
taknokolat 1:290e621741fd 371 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 372
taknokolat 1:290e621741fd 373 static int bufcounter=0;
taknokolat 1:290e621741fd 374
taknokolat 1:290e621741fd 375
taknokolat 1:290e621741fd 376
taknokolat 1:290e621741fd 377 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 378
taknokolat 1:290e621741fd 379 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 380 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 381 //pc.printf("x");
taknokolat 1:290e621741fd 382 return;
taknokolat 1:290e621741fd 383 }
taknokolat 1:290e621741fd 384
taknokolat 1:290e621741fd 385
taknokolat 1:290e621741fd 386
taknokolat 1:290e621741fd 387 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 388
taknokolat 1:290e621741fd 389 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 390
taknokolat 1:290e621741fd 391 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 392
taknokolat 1:290e621741fd 393 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 394 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 395 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 396 //wait_ms(20);
taknokolat 1:290e621741fd 397 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 398 bufcounter = 0;
taknokolat 1:290e621741fd 399 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 400 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 401 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 402 }
taknokolat 1:290e621741fd 403
taknokolat 1:290e621741fd 404 else if(bufcounter>=5){
taknokolat 1:290e621741fd 405 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 406 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 407 bufcounter = 0;
taknokolat 1:290e621741fd 408 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 409 }
taknokolat 1:290e621741fd 410 }
taknokolat 2:f30666d7838b 411
taknokolat 7:8989a4b84695 412 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 413
taknokolat 12:a2c6207cbce3 414 }
taknokolat 1:290e621741fd 415
Skykon 3:c18342e4fddd 416
Skykon 3:c18342e4fddd 417 void setup(){
Skykon 3:c18342e4fddd 418
Skykon 3:c18342e4fddd 419 Init_sensors();
Skykon 3:c18342e4fddd 420 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 421
taknokolat 7:8989a4b84695 422 //NVIC_SetPriority(USART1_IRQn,0);
taknokolat 7:8989a4b84695 423 //NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 7:8989a4b84695 424 //NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 7:8989a4b84695 425 //NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 7:8989a4b84695 426 //NVIC_SetPriority(USART2_IRQn,4);
taknokolat 7:8989a4b84695 427
taknokolat 7:8989a4b84695 428 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 7:8989a4b84695 429 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 7:8989a4b84695 430
Skykon 3:c18342e4fddd 431 DisplayClock();
Skykon 3:c18342e4fddd 432 t.start();
Skykon 3:c18342e4fddd 433
Skykon 3:c18342e4fddd 434 pc.printf("MPU calibration start\r\n");
taknokolat 6:166746820555 435 pc.printf("HMC calibration start\r\n");
Skykon 3:c18342e4fddd 436
Skykon 3:c18342e4fddd 437 float offsetstart = t.read();
Skykon 3:c18342e4fddd 438 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 439 SensingMPU();
taknokolat 6:166746820555 440 SensingHMC();
Skykon 3:c18342e4fddd 441 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 6:166746820555 442 pc.printf("%3.2f\t",nowAngle_HMC);
Skykon 3:c18342e4fddd 443 pc.printf("\r\n");
Skykon 3:c18342e4fddd 444 }
Skykon 3:c18342e4fddd 445
Skykon 3:c18342e4fddd 446 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 447 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 448 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 449 nowAngle[PITCH] -=FirstPITCH;
taknokolat 6:166746820555 450 FirstYAW = nowAngle[YAW];
taknokolat 6:166746820555 451 nowAngle[YAW] -= FirstYAW;
taknokolat 6:166746820555 452
taknokolat 6:166746820555 453 g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 6:166746820555 454 nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 6:166746820555 455 if(nowAngle_HMC<0)nowAngle_HMC+=360;
Skykon 3:c18342e4fddd 456
Skykon 3:c18342e4fddd 457 wait(0.2);
Skykon 3:c18342e4fddd 458
Skykon 3:c18342e4fddd 459 pc.printf("All initialized\r\n");
taknokolat 4:67f705d42f1e 460 setupFlag=true;
taknokolat 1:290e621741fd 461 }
taknokolat 1:290e621741fd 462
Skykon 3:c18342e4fddd 463
Skykon 3:c18342e4fddd 464 void SensingMPU(){
Skykon 3:c18342e4fddd 465 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 466 //t_start = t.read_us();
Skykon 3:c18342e4fddd 467
taknokolat 7:8989a4b84695 468 float rpy[3] = {0};
Skykon 3:c18342e4fddd 469 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 470 static bool flg_checkoutlier = false;
Skykon 3:c18342e4fddd 471 NVIC_DisableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 472 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 473 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 474 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 475 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 476
Skykon 3:c18342e4fddd 477 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 478
Skykon 3:c18342e4fddd 479 NVIC_EnableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 480 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 481 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 482 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 483 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 484
Skykon 3:c18342e4fddd 485
Skykon 3:c18342e4fddd 486 //外れ値対策
Skykon 3:c18342e4fddd 487 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 488 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 489 rpy[PITCH] -= FirstPITCH;
taknokolat 4:67f705d42f1e 490 if(!setupFlag){
taknokolat 4:67f705d42f1e 491 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 492 }else{
taknokolat 4:67f705d42f1e 493 if(rpy[YAW] >= FirstYAW){
taknokolat 4:67f705d42f1e 494 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 495 }else{
taknokolat 4:67f705d42f1e 496 rpy[YAW] += 360.0f;
taknokolat 4:67f705d42f1e 497 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 498 }
taknokolat 4:67f705d42f1e 499 }
Skykon 3:c18342e4fddd 500
Skykon 3:c18342e4fddd 501 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
Skykon 3:c18342e4fddd 502 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 503 for(uint8_t i=0; i<3; i++){
Skykon 3:c18342e4fddd 504 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
Skykon 3:c18342e4fddd 505 }
Skykon 3:c18342e4fddd 506 count_changeRPY = 0;
Skykon 3:c18342e4fddd 507 }else count_changeRPY++;
Skykon 3:c18342e4fddd 508 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 509
Skykon 3:c18342e4fddd 510 }
Skykon 3:c18342e4fddd 511
Skykon 3:c18342e4fddd 512
Skykon 3:c18342e4fddd 513 void Init_sensors(){
Skykon 3:c18342e4fddd 514 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 515 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 516 }
Skykon 3:c18342e4fddd 517 }
Skykon 3:c18342e4fddd 518
Skykon 3:c18342e4fddd 519
Skykon 3:c18342e4fddd 520 void DisplayClock(){
Skykon 3:c18342e4fddd 521 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 522 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 523 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 524 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 525 pc.printf("\r\n");
taknokolat 4:67f705d42f1e 526 }
taknokolat 6:166746820555 527
taknokolat 6:166746820555 528 void SensingHMC(){
taknokolat 6:166746820555 529 //static int16_t deltaT = 0, t_start = 0;
taknokolat 6:166746820555 530 //t_start = t.read_us();
taknokolat 6:166746820555 531
taknokolat 7:8989a4b84695 532 float rpy=0;
taknokolat 6:166746820555 533 static uint16_t count_changeRPY = 0;
taknokolat 6:166746820555 534 static bool flg_checkoutlier = false;
taknokolat 6:166746820555 535 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 6:166746820555 536 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 6:166746820555 537 NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 538 NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 539 NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 4:67f705d42f1e 540
taknokolat 6:166746820555 541 rpy= compass.getHeadingXYDeg(20,50);
taknokolat 6:166746820555 542
taknokolat 6:166746820555 543 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 6:166746820555 544 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 6:166746820555 545 NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 6:166746820555 546 NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 6:166746820555 547 NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 6:166746820555 548
taknokolat 6:166746820555 549
taknokolat 6:166746820555 550 //外れ値対策
taknokolat 6:166746820555 551 //rpy*= 180.0f/PI;
taknokolat 6:166746820555 552 if(!setupFlag){
taknokolat 6:166746820555 553 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 554 }else{
taknokolat 6:166746820555 555 if(rpy >= g_FirstYAW_HMC){
taknokolat 6:166746820555 556 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 557 }else{
taknokolat 6:166746820555 558 rpy += 360.0f;
taknokolat 6:166746820555 559 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 560 }
taknokolat 6:166746820555 561 }
taknokolat 6:166746820555 562
taknokolat 6:166746820555 563 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 6:166746820555 564 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 6:166746820555 565
taknokolat 6:166746820555 566 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 6:166746820555 567
taknokolat 6:166746820555 568 count_changeRPY = 0;
taknokolat 6:166746820555 569 }else count_changeRPY++;
taknokolat 6:166746820555 570 flg_checkoutlier = false;
taknokolat 6:166746820555 571
taknokolat 6:166746820555 572 }
taknokolat 12:a2c6207cbce3 573
taknokolat 13:d8f49328c57f 574 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
taknokolat 13:d8f49328c57f 575 int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
taknokolat 13:d8f49328c57f 576 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 13:d8f49328c57f 577 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL
taknokolat 13:d8f49328c57f 578 ){
taknokolat 13:d8f49328c57f 579
taknokolat 13:d8f49328c57f 580 pc.printf("SDsetup start.\r\n");
taknokolat 13:d8f49328c57f 581
taknokolat 13:d8f49328c57f 582 FILE *fp;
taknokolat 13:d8f49328c57f 583 char parameter[20]; //文字列渡す用の配列
taknokolat 13:d8f49328c57f 584 int SDerrorcount = 0; //取得できなかった数を返す
taknokolat 13:d8f49328c57f 585 const char *paramNames[] = {
taknokolat 13:d8f49328c57f 586 "SERVO_NEUTRAL_R",
taknokolat 13:d8f49328c57f 587 "SERVO_NEUTRAL_L",
taknokolat 13:d8f49328c57f 588 "SERVO_HIGH_FORWARD_R",
taknokolat 13:d8f49328c57f 589 "SERVO_HIGH_FORWARD_L",
taknokolat 13:d8f49328c57f 590 "SERVO_SLOW_FORWARD_R",
taknokolat 13:d8f49328c57f 591 "SERVO_SLOW_FORWARD_L",
taknokolat 13:d8f49328c57f 592 "TURNTABLE_NEUTRAL",
taknokolat 13:d8f49328c57f 593 "MATCH_SPEED",
taknokolat 13:d8f49328c57f 594 "FOCUS_NEUTRAL"
taknokolat 13:d8f49328c57f 595 };
taknokolat 13:d8f49328c57f 596
taknokolat 13:d8f49328c57f 597 fp = fopen("/sd/option.txt","r");
taknokolat 13:d8f49328c57f 598
taknokolat 13:d8f49328c57f 599 if(fp != NULL){ //開けたら
taknokolat 13:d8f49328c57f 600 pc.printf("File was openned.\r\n");
taknokolat 13:d8f49328c57f 601 if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
taknokolat 13:d8f49328c57f 602 else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 13:d8f49328c57f 603 SDerrorcount++;
taknokolat 13:d8f49328c57f 604 }
taknokolat 13:d8f49328c57f 605 if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
taknokolat 13:d8f49328c57f 606 else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 13:d8f49328c57f 607 SDerrorcount++;
taknokolat 13:d8f49328c57f 608 }
taknokolat 13:d8f49328c57f 609 if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
taknokolat 13:d8f49328c57f 610 else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 13:d8f49328c57f 611 SDerrorcount++;
taknokolat 13:d8f49328c57f 612 }
taknokolat 13:d8f49328c57f 613 if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
taknokolat 13:d8f49328c57f 614 else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 13:d8f49328c57f 615 SDerrorcount++;
taknokolat 13:d8f49328c57f 616 }
taknokolat 13:d8f49328c57f 617 if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
taknokolat 13:d8f49328c57f 618 else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 13:d8f49328c57f 619 SDerrorcount++;
taknokolat 13:d8f49328c57f 620 }
taknokolat 13:d8f49328c57f 621 if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
taknokolat 13:d8f49328c57f 622 else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 13:d8f49328c57f 623 SDerrorcount++;
taknokolat 13:d8f49328c57f 624 }
taknokolat 13:d8f49328c57f 625
taknokolat 13:d8f49328c57f 626 if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
taknokolat 13:d8f49328c57f 627 else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 13:d8f49328c57f 628 SDerrorcount++;
taknokolat 13:d8f49328c57f 629 }
taknokolat 13:d8f49328c57f 630 if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
taknokolat 13:d8f49328c57f 631 else{ *Matchspeed = matchspeed;
taknokolat 13:d8f49328c57f 632 SDerrorcount++;
taknokolat 13:d8f49328c57f 633 }
taknokolat 13:d8f49328c57f 634 if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
taknokolat 13:d8f49328c57f 635 else{ *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 13:d8f49328c57f 636 SDerrorcount++;
taknokolat 13:d8f49328c57f 637 }
taknokolat 13:d8f49328c57f 638
taknokolat 13:d8f49328c57f 639 fclose(fp);
taknokolat 13:d8f49328c57f 640
taknokolat 13:d8f49328c57f 641 }else{ //ファイルがなかったら
taknokolat 13:d8f49328c57f 642 pc.printf("fp was null.\r\n");
taknokolat 13:d8f49328c57f 643
taknokolat 13:d8f49328c57f 644 *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 13:d8f49328c57f 645 *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 13:d8f49328c57f 646 *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 13:d8f49328c57f 647 *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 13:d8f49328c57f 648 *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 13:d8f49328c57f 649 *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 13:d8f49328c57f 650 *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 13:d8f49328c57f 651 *Matchspeed = matchspeed;
taknokolat 13:d8f49328c57f 652 *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 13:d8f49328c57f 653 SDerrorcount = -1;
taknokolat 13:d8f49328c57f 654 }
taknokolat 13:d8f49328c57f 655 pc.printf("SDsetup finished.\r\n");
taknokolat 13:d8f49328c57f 656 if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
taknokolat 13:d8f49328c57f 657 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
taknokolat 13:d8f49328c57f 658 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
taknokolat 13:d8f49328c57f 659 /*
taknokolat 13:d8f49328c57f 660 pc.printf("SERVO_NEUTRAL_R = %f, SERVO_NEUTRAL_L = %f\r\n", *g_kpRUD, *g_kiRUD);
taknokolat 13:d8f49328c57f 661 pc.printf("SERVO_FORWARD_R = %f, SERVO_FORWARD_L = %f, kdRUD = %f\r\n", *servo_FORWARD_R, *servo_FORWARD_L);
taknokolat 13:d8f49328c57f 662 pc.printf("SERVO_BACK_R = %f, SERVO_BACK_L = %f\r\n", *servo_back_R, *servo_back_L);
taknokolat 13:d8f49328c57f 663 pc.printf("SERVO_SLOW_FORWARD_R = %f, SERVO_SLOW_FORWARD_L = %f\r\n", *servo_slow_FORWARD_R, *servo_slow_FORWARD_L);
taknokolat 13:d8f49328c57f 664 pc.printf("TURNTABLE_NEUTRAL = %f, MATCH_SPEED = %f\, MINFOCUS = %fr\n", *TurnTable_NEUTRAL, *matchspeed,*minFocus);
taknokolat 13:d8f49328c57f 665 */
taknokolat 13:d8f49328c57f 666 return SDerrorcount;
taknokolat 13:d8f49328c57f 667 }
taknokolat 13:d8f49328c57f 668
taknokolat 13:d8f49328c57f 669 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
taknokolat 13:d8f49328c57f 670 int i=0, j=0;
taknokolat 13:d8f49328c57f 671 int strmax = 200;
taknokolat 13:d8f49328c57f 672 char str[strmax];
taknokolat 13:d8f49328c57f 673
taknokolat 13:d8f49328c57f 674 rewind(fp); //ファイル位置を先頭に
taknokolat 13:d8f49328c57f 675 while(1){
taknokolat 13:d8f49328c57f 676 if (fgets(str, strmax, fp) == NULL) {
taknokolat 13:d8f49328c57f 677 return 0;
taknokolat 13:d8f49328c57f 678 }
taknokolat 13:d8f49328c57f 679 if (!strncmp(str, paramName, strlen(paramName))) {
taknokolat 13:d8f49328c57f 680 while (str[i++] != '=') {}
taknokolat 13:d8f49328c57f 681 while (str[i] != '\n') {
taknokolat 13:d8f49328c57f 682 parameter[j++] = str[i++];
taknokolat 13:d8f49328c57f 683 }
taknokolat 13:d8f49328c57f 684 parameter[j] = '\0';
taknokolat 13:d8f49328c57f 685 return 1;
taknokolat 13:d8f49328c57f 686 }
taknokolat 13:d8f49328c57f 687 }
taknokolat 13:d8f49328c57f 688 }
taknokolat 12:a2c6207cbce3 689
taknokolat 6:166746820555 690
taknokolat 4:67f705d42f1e 691 void DebugPrint(){
taknokolat 4:67f705d42f1e 692 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
taknokolat 7:8989a4b84695 693 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 7:8989a4b84695 694 //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 7:8989a4b84695 695 //pc.printf("\r\n");
taknokolat 6:166746820555 696 }