航空研究会 / Mbed 2 deprecated sahaquiel_brushless_tset

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@27:f67efcee6509, 2019-02-19 (annotated)

Committer:
HARUKIDELTA
Date:
Tue Feb 19 11:21:00 2019 +0000
Revision:
27:f67efcee6509
Parent:
26:ea86e69563b5
Child:
28:887df143fa9c
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 12:083662bca47d 4 #include "SDFileSystem.h"
taknokolat 12:083662bca47d 5 #include "SkipperSv2.h"
taknokolat 12:083662bca47d 6 #include "falfalla.h"
Skykon 3:c18342e4fddd 7
Skykon 3:c18342e4fddd 8 //MPU_check用
Skykon 3:c18342e4fddd 9 #define PI 3.14159265358979
HARUKIDELTA 0:84ddd6d354e1 10
HARUKIDELTA 17:83db05cab3d1 11 #define servo_NEUTRAL_R 1614
HARUKIDELTA 17:83db05cab3d1 12 #define servo_NEUTRAL_L 1621
HARUKIDELTA 0:84ddd6d354e1 13 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 14 #define servo_slow_FORWARD_L 1560
taknokolat 12:083662bca47d 15 #define servo_high_FORWARD_R 1860
taknokolat 12:083662bca47d 16 #define servo_high_FORWARD_L 1860
HARUKIDELTA 17:83db05cab3d1 17 #define turntable_NEUTRAL 1180 //カメラ台座のサーボ
HARUKIDELTA 17:83db05cab3d1 18 #define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
HARUKIDELTA 17:83db05cab3d1 19 #define focus_NEUTRAL 1455 //焦点合わせ用サーボ
HARUKIDELTA 17:83db05cab3d1 20 #define camera_deg_A 1400 //カメラ角度調整
HARUKIDELTA 17:83db05cab3d1 21 #define camera_deg_B 1800
HARUKIDELTA 17:83db05cab3d1 22 #define pint_speed 25
taknokolat 20:011977a37394 23 #define pint_wait 3
HARUKIDELTA 17:83db05cab3d1 24 #define turntable_speed 1800
taknokolat 24:0ad1725c7849 25 #define time360 110
taknokolat 24:0ad1725c7849 26 #define match_wid 5
taknokolat 20:011977a37394 27 #define camera_board_wait 100
HARUKIDELTA 0:84ddd6d354e1 28
HARUKIDELTA 0:84ddd6d354e1 29 #define MOVE_NEUTRAL 0
taknokolat 19:949bab1e9451 30 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 31 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 32 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 33 #define MOVE_BACK 4
Skykon 3:c18342e4fddd 34 #define GOAL_FORWARD 5 //ゴール付近_ゆっくり
HARUKIDELTA 0:84ddd6d354e1 35 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 36 #define GOAL_RIGHT 7
Skykon 3:c18342e4fddd 37 #define MAX_FORWARD 8 //はやい_姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 38 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 39
taknokolat 1:290e621741fd 40 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 41 void getSF_Serial_pi();
taknokolat 1:290e621741fd 42
Skykon 3:c18342e4fddd 43 //MPU_check用
Skykon 3:c18342e4fddd 44 void SensingMPU();
taknokolat 7:8989a4b84695 45
taknokolat 7:8989a4b84695 46 void MoveCansat(char g_landingcommand);
Skykon 3:c18342e4fddd 47 void setup();
Skykon 3:c18342e4fddd 48 void Init_sensors();
Skykon 3:c18342e4fddd 49 void DisplayClock();
taknokolat 4:67f705d42f1e 50 void DebugPrint();
taknokolat 6:166746820555 51 void SensingHMC();
taknokolat 7:8989a4b84695 52 void MoveCameraBoard();
taknokolat 7:8989a4b84695 53 void MatchPosition();
taknokolat 7:8989a4b84695 54 void FocusAdjust();
Skykon 3:c18342e4fddd 55
taknokolat 12:083662bca47d 56 //sd設定
taknokolat 12:083662bca47d 57 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
taknokolat 12:083662bca47d 58
taknokolat 13:b088f0db7158 59 int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 60 int *Servo_high_FORWARD_R,int *Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 61 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 62 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 63 int *Camera_deg_A, int *Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 64 int *Pint_speed, float *Pint_wait,
taknokolat 19:949bab1e9451 65 int *Turntable_speed,
taknokolat 24:0ad1725c7849 66 int *Time360, int *Match_wid,
taknokolat 20:011977a37394 67 int *Camera_board_wait
taknokolat 12:083662bca47d 68 );
taknokolat 12:083662bca47d 69
taknokolat 6:166746820555 70 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
taknokolat 6:166746820555 71 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
Skykon 3:c18342e4fddd 72
taknokolat 7:8989a4b84695 73 bool setupFlag = false;
taknokolat 7:8989a4b84695 74 bool CameraDegFlag = false;
taknokolat 7:8989a4b84695 75 bool jevoisFlag = true;
taknokolat 4:67f705d42f1e 76
taknokolat 24:0ad1725c7849 77 int g_CameraDegCounter = 0; //カメラの回転数をカウント
taknokolat 24:0ad1725c7849 78
Skykon 3:c18342e4fddd 79 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
Skykon 3:c18342e4fddd 80
Skykon 3:c18342e4fddd 81 Timer t;
HARUKIDELTA 26:ea86e69563b5 82 Timer t2;
Skykon 3:c18342e4fddd 83
HARUKIDELTA 25:1f938342d5f9 84 FILE *g_fp;
HARUKIDELTA 25:1f938342d5f9 85
taknokolat 14:90e9b44eb819 86 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
taknokolat 14:90e9b44eb819 87
taknokolat 11:d44d137831b9 88 RawSerial pc(PA_2,PA_3,115200); //uart2
taknokolat 11:d44d137831b9 89 //pa2:UART2_TX,pa3:UART2_RX
taknokolat 12:083662bca47d 90 RawSerial pc2(PA_11,PA_12,115200); //uart1
taknokolat 11:d44d137831b9 91 //pb6:UART1_TX,pb7:UART1_RX
taknokolat 11:d44d137831b9 92
HARUKIDELTA 5:8bfe95431ec0 93 //PWM pin宣言
taknokolat 6:166746820555 94 PwmOut servoR(PC_6); //TIM3_CH1 車輪右
taknokolat 6:166746820555 95 PwmOut servoL(PC_7); //TIM3_CH2 車輪左
HARUKIDELTA 5:8bfe95431ec0 96 PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
HARUKIDELTA 5:8bfe95431ec0 97 PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
HARUKIDELTA 5:8bfe95431ec0 98 PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
HARUKIDELTA 5:8bfe95431ec0 99 PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
HARUKIDELTA 5:8bfe95431ec0 100 PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
Skykon 3:c18342e4fddd 101
HARUKIDELTA 5:8bfe95431ec0 102 /*通信用のpinは
HARUKIDELTA 5:8bfe95431ec0 103 PA_3(UART2_Rx) skipperウラ
HARUKIDELTA 5:8bfe95431ec0 104 PA_12(UART6_Rx) skipperオモテ USB端子より
HARUKIDELTA 5:8bfe95431ec0 105 PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
HARUKIDELTA 5:8bfe95431ec0 106 */
HARUKIDELTA 5:8bfe95431ec0 107
HARUKIDELTA 5:8bfe95431ec0 108 /*超音波はRaspberryPiに積む*/
HARUKIDELTA 5:8bfe95431ec0 109
HARUKIDELTA 23:29b2722bd753 110 DigitalOut led1(PA_0); //tanakaOK kimuraは動作不良
HARUKIDELTA 23:29b2722bd753 111 DigitalOut led2(PA_1); //tanakaOK kimuraOK
HARUKIDELTA 23:29b2722bd753 112 DigitalOut led3(PB_5); //使ってないよ
HARUKIDELTA 23:29b2722bd753 113 DigitalOut led4(PB_4); //使ってないよ
HARUKIDELTA 17:83db05cab3d1 114
HARUKIDELTA 17:83db05cab3d1 115
HARUKIDELTA 5:8bfe95431ec0 116 //外付けコンパス
taknokolat 24:0ad1725c7849 117 //HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
HARUKIDELTA 0:84ddd6d354e1 118
taknokolat 2:f30666d7838b 119 char g_landingcommand='N';
taknokolat 1:290e621741fd 120
Skykon 3:c18342e4fddd 121 //MPU_check用
Skykon 3:c18342e4fddd 122 MPU6050DMP6 mpu6050(PC_0,&pc);
Skykon 3:c18342e4fddd 123
Skykon 3:c18342e4fddd 124
HARUKIDELTA 0:84ddd6d354e1 125 int main() {
taknokolat 1:290e621741fd 126
Skykon 3:c18342e4fddd 127 //MPU_check
Skykon 3:c18342e4fddd 128 setup();
taknokolat 19:949bab1e9451 129 //コンパスチェック用
taknokolat 19:949bab1e9451 130 /*while(1){
taknokolat 22:a19be3b505b7 131 //SensingMPU();
taknokolat 19:949bab1e9451 132 SensingHMC();
taknokolat 22:a19be3b505b7 133 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 19:949bab1e9451 134 pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 19:949bab1e9451 135 pc.printf("\r\n");
taknokolat 19:949bab1e9451 136 wait_ms(30);
taknokolat 19:949bab1e9451 137 }*/
taknokolat 19:949bab1e9451 138
Skykon 3:c18342e4fddd 139
HARUKIDELTA 17:83db05cab3d1 140 servoCameradeg.pulsewidth_us(Camera_deg_A);
taknokolat 11:d44d137831b9 141
taknokolat 1:290e621741fd 142 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 143 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 12:083662bca47d 144 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
HARUKIDELTA 27:f67efcee6509 145 t2.start();
taknokolat 1:290e621741fd 146
HARUKIDELTA 0:84ddd6d354e1 147 while(1) {
taknokolat 7:8989a4b84695 148 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 149 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 20:011977a37394 150 pc.printf("Move Camera Board\r\n");
taknokolat 10:63fe920595a7 151 MoveCameraBoard();
taknokolat 10:63fe920595a7 152
taknokolat 20:011977a37394 153 pc.printf("Position\r\n");
taknokolat 22:a19be3b505b7 154 pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
HARUKIDELTA 25:1f938342d5f9 155
taknokolat 22:a19be3b505b7 156 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 157 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 158 if(g_landingcommand!='N') MatchPosition();
taknokolat 22:a19be3b505b7 159 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 160 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 161
taknokolat 8:d11a59d2a2f1 162 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 8:d11a59d2a2f1 163 else NVIC_DisableIRQ(USART2_IRQn);
taknokolat 8:d11a59d2a2f1 164
taknokolat 20:011977a37394 165 pc.printf("Move Cansat\r\n");
taknokolat 7:8989a4b84695 166 MoveCansat(g_landingcommand);
taknokolat 22:a19be3b505b7 167 if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 168 else NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 169 wait_ms(23);
taknokolat 8:d11a59d2a2f1 170
taknokolat 20:011977a37394 171 pc.printf("finish\r\n");
taknokolat 7:8989a4b84695 172 if(jevoisFlag==true) NVIC_EnableIRQ(USART1_IRQn);
taknokolat 7:8989a4b84695 173 else NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 174 }
HARUKIDELTA 0:84ddd6d354e1 175 }
HARUKIDELTA 0:84ddd6d354e1 176
Skykon 3:c18342e4fddd 177
taknokolat 22:a19be3b505b7 178 void MoveCansat(char a)
HARUKIDELTA 0:84ddd6d354e1 179 {
taknokolat 22:a19be3b505b7 180 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 181 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 22:a19be3b505b7 182 switch(a){
taknokolat 2:f30666d7838b 183 case 'N': //MOVE_NEUTRAL
HARUKIDELTA 17:83db05cab3d1 184 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 185 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 22:a19be3b505b7 186 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 187 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 26:ea86e69563b5 188 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 27:f67efcee6509 189 fprintf(g_fp, "Time = %d min %d sec : Goal is not found.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 26:ea86e69563b5 190 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 191 break;
taknokolat 1:290e621741fd 192
taknokolat 2:f30666d7838b 193 case 'Y': //MOVE_FORWARD
HARUKIDELTA 17:83db05cab3d1 194 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 195 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 22:a19be3b505b7 196 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 197 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 198 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 27:f67efcee6509 199 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward .\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 200 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 201 break;
taknokolat 1:290e621741fd 202
taknokolat 12:083662bca47d 203 case 'l': //MOVE_LEFT Low Speed
HARUKIDELTA 17:83db05cab3d1 204 servoR.pulsewidth_us(Servo_slow_FORWARD_R);
taknokolat 22:a19be3b505b7 205 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 22:a19be3b505b7 206 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 207 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 208 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 209 fprintf(g_fp, "Time = %d min %d sec : Cansat move left low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 210 fclose(g_fp);
HARUKIDELTA 27:f67efcee6509 211 break;
HARUKIDELTA 27:f67efcee6509 212
taknokolat 12:083662bca47d 213 case 'L': //MOVE_LEFT High Speed
HARUKIDELTA 17:83db05cab3d1 214 servoR.pulsewidth_us(Servo_high_FORWARD_R);
taknokolat 22:a19be3b505b7 215 servoL.pulsewidth_us(Servo_NEUTRAL_L);
taknokolat 22:a19be3b505b7 216 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 217 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 218 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 219 fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 220 fclose(g_fp);
HARUKIDELTA 27:f67efcee6509 221 break;
taknokolat 1:290e621741fd 222
taknokolat 12:083662bca47d 223 case 'r': //MOVE_RIGHT Low Speed
taknokolat 22:a19be3b505b7 224 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 225 servoL.pulsewidth_us(Servo_slow_FORWARD_L);
taknokolat 22:a19be3b505b7 226 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 227 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 228 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 229 fprintf(g_fp, "Time = %d min %d sec : Cansat move right low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 230 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 231 break;
taknokolat 12:083662bca47d 232
taknokolat 12:083662bca47d 233 case 'R': //MOVE_RIGHT High Speed
taknokolat 22:a19be3b505b7 234 servoR.pulsewidth_us(Servo_NEUTRAL_R);
HARUKIDELTA 17:83db05cab3d1 235 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 22:a19be3b505b7 236 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 237 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 238 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 239 fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 240 fclose(g_fp);
HARUKIDELTA 0:84ddd6d354e1 241 break;
taknokolat 1:290e621741fd 242
taknokolat 1:290e621741fd 243 case 'G': //GOAL_FORWARD
HARUKIDELTA 17:83db05cab3d1 244 servoR.pulsewidth_us(Servo_slow_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 245 servoL.pulsewidth_us(Servo_slow_FORWARD_L);
taknokolat 22:a19be3b505b7 246 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 247 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 248 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 249 fprintf(g_fp, "Time = %d min %d sec : Cansat move goal forward mode.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 250 fclose(g_fp);
taknokolat 7:8989a4b84695 251 break;
taknokolat 12:083662bca47d 252
taknokolat 12:083662bca47d 253 case '1': //MOVE_FORWARD Speed Level 1
HARUKIDELTA 17:83db05cab3d1 254 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 255 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 256 wait(1);
taknokolat 12:083662bca47d 257 do{
taknokolat 22:a19be3b505b7 258 SensingMPU();
taknokolat 22:a19be3b505b7 259 wait_ms(30);
taknokolat 22:a19be3b505b7 260 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 22:a19be3b505b7 261 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 262 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 263 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 264 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 1sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 265 fclose(g_fp);
taknokolat 12:083662bca47d 266 break;
taknokolat 12:083662bca47d 267
taknokolat 12:083662bca47d 268 case '2': //MOVE_FORWARD Speed Level 2
HARUKIDELTA 17:83db05cab3d1 269 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 270 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 271 wait(2);
taknokolat 12:083662bca47d 272 do{
taknokolat 22:a19be3b505b7 273 SensingMPU();
taknokolat 22:a19be3b505b7 274 wait_ms(30);
taknokolat 22:a19be3b505b7 275 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 22:a19be3b505b7 276 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 277 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 278 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 279 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 2sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 280 fclose(g_fp);
taknokolat 12:083662bca47d 281 break;
taknokolat 12:083662bca47d 282
taknokolat 12:083662bca47d 283 case '3': //MOVE_FORWARD Speed Level 3
HARUKIDELTA 17:83db05cab3d1 284 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 285 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 286 wait(3);
taknokolat 12:083662bca47d 287 do{
taknokolat 22:a19be3b505b7 288 SensingMPU();
taknokolat 22:a19be3b505b7 289 wait_ms(30);
taknokolat 22:a19be3b505b7 290 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 22:a19be3b505b7 291 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 292 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 293 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 294 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 295 fclose(g_fp);
taknokolat 12:083662bca47d 296 break;
taknokolat 12:083662bca47d 297
taknokolat 12:083662bca47d 298 case '4': //MOVE_FORWARD Speed Level 4
HARUKIDELTA 17:83db05cab3d1 299 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 300 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 12:083662bca47d 301 wait(4);
taknokolat 12:083662bca47d 302 do{
taknokolat 22:a19be3b505b7 303 SensingMPU();
taknokolat 22:a19be3b505b7 304 wait_ms(30);
taknokolat 22:a19be3b505b7 305 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)||(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 22:a19be3b505b7 306 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 307 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 308 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 309 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 310 fclose(g_fp);
taknokolat 12:083662bca47d 311 break;
taknokolat 7:8989a4b84695 312
taknokolat 12:083662bca47d 313 case '5': //MOVE_FORWARD Speed Level 5
HARUKIDELTA 17:83db05cab3d1 314 servoR.pulsewidth_us(Servo_high_FORWARD_R);
HARUKIDELTA 17:83db05cab3d1 315 servoL.pulsewidth_us(Servo_high_FORWARD_L);
taknokolat 7:8989a4b84695 316 wait(5);
taknokolat 10:63fe920595a7 317 do{
taknokolat 22:a19be3b505b7 318 SensingMPU();
taknokolat 22:a19be3b505b7 319 wait_ms(30);
taknokolat 22:a19be3b505b7 320 }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20));
taknokolat 22:a19be3b505b7 321 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 322 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 27:f67efcee6509 323 g_fp = fopen( "/sd/Datalog01.txt","a" );
HARUKIDELTA 27:f67efcee6509 324 fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 5sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
HARUKIDELTA 27:f67efcee6509 325 fclose(g_fp);
taknokolat 7:8989a4b84695 326 break;
taknokolat 7:8989a4b84695 327
taknokolat 7:8989a4b84695 328 case 'M': //MatchPosition
HARUKIDELTA 17:83db05cab3d1 329 servoR.pulsewidth_us(Matchspeed);
taknokolat 22:a19be3b505b7 330 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 331 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 7:8989a4b84695 332 break;
taknokolat 7:8989a4b84695 333
taknokolat 12:083662bca47d 334 case 'P': //jevoisからraspberry piへの切り替え
taknokolat 7:8989a4b84695 335 jevoisFlag = false;
taknokolat 22:a19be3b505b7 336 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 337 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 338 break;
taknokolat 12:083662bca47d 339
taknokolat 12:083662bca47d 340 case 'S':
taknokolat 12:083662bca47d 341 /*RasPiからの超音波判定(プログラムスタート部)*/
taknokolat 22:a19be3b505b7 342 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 343 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 12:083662bca47d 344 break;
taknokolat 1:290e621741fd 345
taknokolat 1:290e621741fd 346 default :
taknokolat 22:a19be3b505b7 347 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 22:a19be3b505b7 348 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 349 break;
taknokolat 1:290e621741fd 350
HARUKIDELTA 0:84ddd6d354e1 351 }
taknokolat 7:8989a4b84695 352
HARUKIDELTA 0:84ddd6d354e1 353 return;
taknokolat 1:290e621741fd 354 }
taknokolat 1:290e621741fd 355
taknokolat 7:8989a4b84695 356 void MoveCameraBoard(){
taknokolat 22:a19be3b505b7 357 //pc.printf("start\r\n");
taknokolat 7:8989a4b84695 358 MoveCansat('N');
taknokolat 22:a19be3b505b7 359 //pc.printf("ok\r\n");
taknokolat 10:63fe920595a7 360 g_landingcommand='N';
taknokolat 24:0ad1725c7849 361 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
taknokolat 20:011977a37394 362 wait_ms(Camera_board_wait);
taknokolat 24:0ad1725c7849 363 g_CameraDegCounter++;
HARUKIDELTA 17:83db05cab3d1 364 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 22:a19be3b505b7 365 //pc.printf("zoom1\r\n");
taknokolat 7:8989a4b84695 366 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 367 else wait(1);
taknokolat 7:8989a4b84695 368 if(g_landingcommand!='N') return;
taknokolat 7:8989a4b84695 369 if(!CameraDegFlag){
taknokolat 18:4566d1f5fc60 370 for(int i=30; i<Camera_deg_B-Camera_deg_A;i=i+30){
taknokolat 18:4566d1f5fc60 371 servoCameradeg.pulsewidth_us(Camera_deg_A+i);
taknokolat 22:a19be3b505b7 372 //pc.printf("%d\r\n",i);
taknokolat 15:1519fccfcae3 373 wait_ms(20);
taknokolat 15:1519fccfcae3 374 }
taknokolat 7:8989a4b84695 375 CameraDegFlag=!CameraDegFlag;
taknokolat 7:8989a4b84695 376 }else{
taknokolat 18:4566d1f5fc60 377 for(int i=30; i<Camera_deg_B-Camera_deg_A;i=i+30){
taknokolat 18:4566d1f5fc60 378 servoCameradeg.pulsewidth_us(Camera_deg_B-i);
taknokolat 22:a19be3b505b7 379 //pc.printf("%d\r\n",i);
taknokolat 18:4566d1f5fc60 380 wait_ms(20);
taknokolat 15:1519fccfcae3 381 }
taknokolat 7:8989a4b84695 382 CameraDegFlag = !CameraDegFlag;
taknokolat 7:8989a4b84695 383 }
taknokolat 22:a19be3b505b7 384 if(g_landingcommand!='N') return;
taknokolat 22:a19be3b505b7 385 //pc.printf("zoom2\r\n");
taknokolat 7:8989a4b84695 386 if(jevoisFlag == true) FocusAdjust();
taknokolat 7:8989a4b84695 387 else wait(1);
taknokolat 7:8989a4b84695 388
taknokolat 20:011977a37394 389
taknokolat 22:a19be3b505b7 390 //pc.printf("Move Board Finish\r\n");
taknokolat 7:8989a4b84695 391 return;
taknokolat 7:8989a4b84695 392 }
taknokolat 7:8989a4b84695 393
taknokolat 7:8989a4b84695 394 void MatchPosition(){
taknokolat 22:a19be3b505b7 395 pc.printf("MatchPosition\r\n");
taknokolat 24:0ad1725c7849 396
taknokolat 24:0ad1725c7849 397 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
taknokolat 24:0ad1725c7849 398 wait_ms(Camera_board_wait*g_CameraDegCounter);
taknokolat 24:0ad1725c7849 399 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
taknokolat 24:0ad1725c7849 400
taknokolat 22:a19be3b505b7 401 int SetLoop=0;
taknokolat 22:a19be3b505b7 402
taknokolat 22:a19be3b505b7 403 while(SetLoop<30){
taknokolat 7:8989a4b84695 404 SensingMPU();
taknokolat 22:a19be3b505b7 405 wait_ms(20);
taknokolat 24:0ad1725c7849 406 //SensingHMC();
taknokolat 24:0ad1725c7849 407 //wait_ms(20);
taknokolat 22:a19be3b505b7 408 //DebugPrint();
taknokolat 22:a19be3b505b7 409 SetLoop++;
taknokolat 22:a19be3b505b7 410 //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
taknokolat 22:a19be3b505b7 411
taknokolat 22:a19be3b505b7 412 }
taknokolat 7:8989a4b84695 413
taknokolat 24:0ad1725c7849 414 static float TargetDeg = 0;
taknokolat 24:0ad1725c7849 415
taknokolat 24:0ad1725c7849 416 TargetDeg = 360*(float)Camera_board_wait*(float)g_CameraDegCounter/(float)Time360;
taknokolat 24:0ad1725c7849 417
taknokolat 24:0ad1725c7849 418 float HighTargetYaw = TargetDeg + Match_wid;
taknokolat 24:0ad1725c7849 419 float LowTargetYaw = TargetDeg - Match_wid;
taknokolat 21:ca8f35e24e66 420
taknokolat 22:a19be3b505b7 421
taknokolat 21:ca8f35e24e66 422 if(HighTargetYaw >= 360.0) HighTargetYaw = HighTargetYaw - 360.0;
taknokolat 22:a19be3b505b7 423
taknokolat 21:ca8f35e24e66 424 if(LowTargetYaw < 0) LowTargetYaw = LowTargetYaw + 360.0;
taknokolat 21:ca8f35e24e66 425
taknokolat 22:a19be3b505b7 426 pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
taknokolat 22:a19be3b505b7 427
taknokolat 24:0ad1725c7849 428 MoveCansat('r');
taknokolat 22:a19be3b505b7 429
taknokolat 22:a19be3b505b7 430 if(HighTargetYaw-LowTargetYaw<0){
taknokolat 22:a19be3b505b7 431 while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
taknokolat 22:a19be3b505b7 432 //MoveCansat('r');
taknokolat 22:a19be3b505b7 433 SensingMPU();
taknokolat 22:a19be3b505b7 434 wait_ms(30);
taknokolat 22:a19be3b505b7 435 pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
taknokolat 22:a19be3b505b7 436 }
taknokolat 22:a19be3b505b7 437 }else{
taknokolat 22:a19be3b505b7 438 while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
taknokolat 22:a19be3b505b7 439 //MoveCansat('r');
taknokolat 22:a19be3b505b7 440 SensingMPU();
taknokolat 22:a19be3b505b7 441 wait_ms(30);
taknokolat 22:a19be3b505b7 442 pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
taknokolat 22:a19be3b505b7 443 }
taknokolat 7:8989a4b84695 444 }
taknokolat 24:0ad1725c7849 445 g_CameraDegCounter = 0;
taknokolat 7:8989a4b84695 446 return;
taknokolat 7:8989a4b84695 447 }
taknokolat 7:8989a4b84695 448
taknokolat 7:8989a4b84695 449 void FocusAdjust(){
HARUKIDELTA 17:83db05cab3d1 450 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
taknokolat 20:011977a37394 451 pc.printf("set\r\n");
taknokolat 11:d44d137831b9 452 wait(1);
taknokolat 20:011977a37394 453 pc.printf("set2\r\n");
HARUKIDELTA 17:83db05cab3d1 454 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL+Pint_speed);
taknokolat 20:011977a37394 455 pc.printf("check\r\n");
taknokolat 20:011977a37394 456 wait(Pint_wait);
HARUKIDELTA 17:83db05cab3d1 457 servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
taknokolat 11:d44d137831b9 458
taknokolat 7:8989a4b84695 459 return;
taknokolat 15:1519fccfcae3 460 }
taknokolat 7:8989a4b84695 461
taknokolat 1:290e621741fd 462 void getSF_Serial_jevois(){
taknokolat 22:a19be3b505b7 463
taknokolat 22:a19be3b505b7 464 //pc.printf("jevois\r\n");
taknokolat 1:290e621741fd 465
taknokolat 1:290e621741fd 466
taknokolat 1:290e621741fd 467 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 468
taknokolat 1:290e621741fd 469 static int bufcounter=0;
taknokolat 1:290e621741fd 470
taknokolat 1:290e621741fd 471
taknokolat 1:290e621741fd 472
taknokolat 1:290e621741fd 473 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 474
taknokolat 1:290e621741fd 475 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 476 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 477 //pc.printf("x");
taknokolat 1:290e621741fd 478 return;
taknokolat 1:290e621741fd 479 }
taknokolat 1:290e621741fd 480
taknokolat 1:290e621741fd 481
taknokolat 1:290e621741fd 482
taknokolat 22:a19be3b505b7 483 pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 484
Skykon 3:c18342e4fddd 485 if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 486
taknokolat 1:290e621741fd 487 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 488
taknokolat 1:290e621741fd 489 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 490 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 491 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 492 //wait_ms(20);
taknokolat 1:290e621741fd 493 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 494 bufcounter = 0;
taknokolat 1:290e621741fd 495 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 496 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 497 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 498 }
taknokolat 1:290e621741fd 499
taknokolat 1:290e621741fd 500 else if(bufcounter>=5){
taknokolat 1:290e621741fd 501 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 502 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 503 bufcounter = 0;
taknokolat 1:290e621741fd 504 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 505 }
taknokolat 1:290e621741fd 506 }
taknokolat 1:290e621741fd 507
taknokolat 1:290e621741fd 508
taknokolat 1:290e621741fd 509 }
taknokolat 1:290e621741fd 510
taknokolat 1:290e621741fd 511
taknokolat 12:083662bca47d 512 void getSF_Serial_pi(){
taknokolat 2:f30666d7838b 513
taknokolat 7:8989a4b84695 514 //NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 515
taknokolat 1:290e621741fd 516 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 517
taknokolat 1:290e621741fd 518 static int bufcounter=0;
taknokolat 1:290e621741fd 519
taknokolat 1:290e621741fd 520
taknokolat 1:290e621741fd 521
taknokolat 1:290e621741fd 522 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 523
taknokolat 1:290e621741fd 524 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 525 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 526 //pc.printf("x");
taknokolat 1:290e621741fd 527 return;
taknokolat 1:290e621741fd 528 }
taknokolat 1:290e621741fd 529
taknokolat 1:290e621741fd 530
taknokolat 1:290e621741fd 531
taknokolat 1:290e621741fd 532 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 533
taknokolat 1:290e621741fd 534 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 535
taknokolat 1:290e621741fd 536 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 537
taknokolat 1:290e621741fd 538 g_landingcommand = SFbuf[1];
taknokolat 2:f30666d7838b 539 wait_ms(31);//信号が速すぎることによる割り込み防止
taknokolat 1:290e621741fd 540 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 541 //wait_ms(20);
taknokolat 1:290e621741fd 542 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 543 bufcounter = 0;
taknokolat 1:290e621741fd 544 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 545 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 546 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 547 }
taknokolat 1:290e621741fd 548
taknokolat 1:290e621741fd 549 else if(bufcounter>=5){
taknokolat 1:290e621741fd 550 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 551 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 552 bufcounter = 0;
taknokolat 1:290e621741fd 553 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 554 }
taknokolat 1:290e621741fd 555 }
taknokolat 2:f30666d7838b 556
taknokolat 7:8989a4b84695 557 //NVIC_EnableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 558
taknokolat 12:083662bca47d 559 }
taknokolat 1:290e621741fd 560
Skykon 3:c18342e4fddd 561
Skykon 3:c18342e4fddd 562 void setup(){
HARUKIDELTA 23:29b2722bd753 563
HARUKIDELTA 23:29b2722bd753 564 led1 = 1;
HARUKIDELTA 23:29b2722bd753 565 led2 = 1;
HARUKIDELTA 23:29b2722bd753 566 led3 = 1;
HARUKIDELTA 23:29b2722bd753 567 led4 = 1;
HARUKIDELTA 26:ea86e69563b5 568
HARUKIDELTA 26:ea86e69563b5 569 servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
HARUKIDELTA 26:ea86e69563b5 570 servoL.pulsewidth_us(Servo_NEUTRAL_L);
HARUKIDELTA 26:ea86e69563b5 571 servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
HARUKIDELTA 26:ea86e69563b5 572 servoCameradeg.pulsewidth_us(Camera_deg_A);
HARUKIDELTA 26:ea86e69563b5 573 servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
Skykon 3:c18342e4fddd 574
taknokolat 13:b088f0db7158 575 SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 576 &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 577 &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 578 &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 579 &Camera_deg_A, &Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 580 &Pint_speed,&Pint_wait,
taknokolat 19:949bab1e9451 581 &Turntable_speed,
taknokolat 24:0ad1725c7849 582 &Time360, &Match_wid,
taknokolat 20:011977a37394 583 &Camera_board_wait
taknokolat 13:b088f0db7158 584 );
taknokolat 12:083662bca47d 585
Skykon 3:c18342e4fddd 586 Init_sensors();
Skykon 3:c18342e4fddd 587 //switch2.rise(ResetTrim);
Skykon 3:c18342e4fddd 588
taknokolat 7:8989a4b84695 589 //NVIC_SetPriority(USART1_IRQn,0);
taknokolat 7:8989a4b84695 590 //NVIC_SetPriority(EXTI0_IRQn,1);
taknokolat 7:8989a4b84695 591 //NVIC_SetPriority(TIM5_IRQn,2);
taknokolat 7:8989a4b84695 592 //NVIC_SetPriority(EXTI9_5_IRQn,3);
taknokolat 7:8989a4b84695 593 //NVIC_SetPriority(USART2_IRQn,4);
taknokolat 7:8989a4b84695 594
taknokolat 7:8989a4b84695 595 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 7:8989a4b84695 596 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 7:8989a4b84695 597
Skykon 3:c18342e4fddd 598 DisplayClock();
Skykon 3:c18342e4fddd 599 t.start();
Skykon 3:c18342e4fddd 600
Skykon 3:c18342e4fddd 601 pc.printf("MPU calibration start\r\n");
taknokolat 24:0ad1725c7849 602 //pc.printf("HMC calibration start\r\n");
Skykon 3:c18342e4fddd 603
Skykon 3:c18342e4fddd 604 float offsetstart = t.read();
Skykon 3:c18342e4fddd 605 while(t.read() - offsetstart < 26){
Skykon 3:c18342e4fddd 606 SensingMPU();
taknokolat 24:0ad1725c7849 607 //SensingHMC();
Skykon 3:c18342e4fddd 608 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
taknokolat 24:0ad1725c7849 609 //pc.printf("\t%3.2f\t",nowAngle_HMC);
Skykon 3:c18342e4fddd 610 pc.printf("\r\n");
HARUKIDELTA 23:29b2722bd753 611 led1 = !led1;
HARUKIDELTA 23:29b2722bd753 612 led2 = !led2;
HARUKIDELTA 23:29b2722bd753 613 led3 = !led3;
HARUKIDELTA 23:29b2722bd753 614 led4 = !led4;
Skykon 3:c18342e4fddd 615 }
Skykon 3:c18342e4fddd 616
Skykon 3:c18342e4fddd 617 FirstROLL = nowAngle[ROLL];
Skykon 3:c18342e4fddd 618 FirstPITCH = nowAngle[PITCH];
Skykon 3:c18342e4fddd 619 nowAngle[ROLL] -=FirstROLL;
Skykon 3:c18342e4fddd 620 nowAngle[PITCH] -=FirstPITCH;
taknokolat 6:166746820555 621 FirstYAW = nowAngle[YAW];
taknokolat 6:166746820555 622 nowAngle[YAW] -= FirstYAW;
taknokolat 6:166746820555 623
taknokolat 24:0ad1725c7849 624 //g_FirstYAW_HMC = nowAngle_HMC;
taknokolat 24:0ad1725c7849 625 //nowAngle_HMC -=g_FirstYAW_HMC;
taknokolat 24:0ad1725c7849 626 //if(nowAngle_HMC<0)nowAngle_HMC+=360;
Skykon 3:c18342e4fddd 627
HARUKIDELTA 23:29b2722bd753 628 led1 = 0;
HARUKIDELTA 23:29b2722bd753 629 led2 = 0;
HARUKIDELTA 23:29b2722bd753 630 led3 = 0;
HARUKIDELTA 23:29b2722bd753 631 led4 = 0;
HARUKIDELTA 23:29b2722bd753 632
Skykon 3:c18342e4fddd 633 wait(0.2);
Skykon 3:c18342e4fddd 634
HARUKIDELTA 26:ea86e69563b5 635
HARUKIDELTA 26:ea86e69563b5 636 g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
HARUKIDELTA 26:ea86e69563b5 637 if( g_fp == NULL ) {
HARUKIDELTA 26:ea86e69563b5 638 pc.printf( "ファイルオープンエラー\r\n" );
HARUKIDELTA 26:ea86e69563b5 639 }
HARUKIDELTA 27:f67efcee6509 640 fprintf(g_fp,"\r\n-------------------------\r\n");
HARUKIDELTA 26:ea86e69563b5 641 fprintf(g_fp,"All initialized.\r\n");
HARUKIDELTA 26:ea86e69563b5 642 fclose( g_fp );
HARUKIDELTA 26:ea86e69563b5 643
HARUKIDELTA 26:ea86e69563b5 644
Skykon 3:c18342e4fddd 645 pc.printf("All initialized\r\n");
taknokolat 4:67f705d42f1e 646 setupFlag=true;
taknokolat 1:290e621741fd 647 }
taknokolat 1:290e621741fd 648
Skykon 3:c18342e4fddd 649
Skykon 3:c18342e4fddd 650 void SensingMPU(){
Skykon 3:c18342e4fddd 651 //static int16_t deltaT = 0, t_start = 0;
Skykon 3:c18342e4fddd 652 //t_start = t.read_us();
Skykon 3:c18342e4fddd 653
taknokolat 7:8989a4b84695 654 float rpy[3] = {0};
Skykon 3:c18342e4fddd 655 static uint16_t count_changeRPY = 0;
Skykon 3:c18342e4fddd 656 static bool flg_checkoutlier = false;
Skykon 3:c18342e4fddd 657 NVIC_DisableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 658 NVIC_DisableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 659 NVIC_DisableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 660 NVIC_DisableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 661 NVIC_DisableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 662
Skykon 3:c18342e4fddd 663 mpu6050.getRollPitchYaw_Skipper(rpy);
Skykon 3:c18342e4fddd 664
Skykon 3:c18342e4fddd 665 NVIC_EnableIRQ(USART1_IRQn);
Skykon 3:c18342e4fddd 666 NVIC_EnableIRQ(USART2_IRQn);
Skykon 3:c18342e4fddd 667 NVIC_EnableIRQ(TIM5_IRQn);
Skykon 3:c18342e4fddd 668 NVIC_EnableIRQ(EXTI0_IRQn);
Skykon 3:c18342e4fddd 669 NVIC_EnableIRQ(EXTI9_5_IRQn);
Skykon 3:c18342e4fddd 670
Skykon 3:c18342e4fddd 671
Skykon 3:c18342e4fddd 672 //外れ値対策
Skykon 3:c18342e4fddd 673 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
Skykon 3:c18342e4fddd 674 rpy[ROLL] -= FirstROLL;
Skykon 3:c18342e4fddd 675 rpy[PITCH] -= FirstPITCH;
taknokolat 4:67f705d42f1e 676 if(!setupFlag){
taknokolat 4:67f705d42f1e 677 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 678 }else{
taknokolat 4:67f705d42f1e 679 if(rpy[YAW] >= FirstYAW){
taknokolat 4:67f705d42f1e 680 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 681 }else{
taknokolat 4:67f705d42f1e 682 rpy[YAW] += 360.0f;
taknokolat 4:67f705d42f1e 683 rpy[YAW] -= FirstYAW;
taknokolat 4:67f705d42f1e 684 }
taknokolat 4:67f705d42f1e 685 }
Skykon 3:c18342e4fddd 686
Skykon 3:c18342e4fddd 687 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 688 if(!flg_checkoutlier || count_changeRPY >= 2){
Skykon 3:c18342e4fddd 689 for(uint8_t i=0; i<3; i++){
Skykon 3:c18342e4fddd 690 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
Skykon 3:c18342e4fddd 691 }
Skykon 3:c18342e4fddd 692 count_changeRPY = 0;
Skykon 3:c18342e4fddd 693 }else count_changeRPY++;
Skykon 3:c18342e4fddd 694 flg_checkoutlier = false;
Skykon 3:c18342e4fddd 695
Skykon 3:c18342e4fddd 696 }
Skykon 3:c18342e4fddd 697
Skykon 3:c18342e4fddd 698
Skykon 3:c18342e4fddd 699 void Init_sensors(){
Skykon 3:c18342e4fddd 700 if(mpu6050.setup() == -1){
Skykon 3:c18342e4fddd 701 pc.printf("failed initialize\r\n");
Skykon 3:c18342e4fddd 702 }
Skykon 3:c18342e4fddd 703 }
Skykon 3:c18342e4fddd 704
Skykon 3:c18342e4fddd 705
Skykon 3:c18342e4fddd 706 void DisplayClock(){
Skykon 3:c18342e4fddd 707 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
Skykon 3:c18342e4fddd 708 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
Skykon 3:c18342e4fddd 709 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
Skykon 3:c18342e4fddd 710 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
Skykon 3:c18342e4fddd 711 pc.printf("\r\n");
taknokolat 4:67f705d42f1e 712 }
taknokolat 6:166746820555 713
taknokolat 24:0ad1725c7849 714 /*void SensingHMC(){
taknokolat 6:166746820555 715 //static int16_t deltaT = 0, t_start = 0;
taknokolat 6:166746820555 716 //t_start = t.read_us();
taknokolat 6:166746820555 717
taknokolat 7:8989a4b84695 718 float rpy=0;
taknokolat 6:166746820555 719 static uint16_t count_changeRPY = 0;
taknokolat 6:166746820555 720 static bool flg_checkoutlier = false;
taknokolat 6:166746820555 721 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 6:166746820555 722 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 19:949bab1e9451 723 //NVIC_DisableIRQ(TIM5_IRQn);
taknokolat 19:949bab1e9451 724 //NVIC_DisableIRQ(EXTI0_IRQn);
taknokolat 19:949bab1e9451 725 //NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat 4:67f705d42f1e 726
taknokolat 24:0ad1725c7849 727 rpy= compass.getHeadingXYDeg(Time360,Match_wid);
taknokolat 6:166746820555 728
taknokolat 6:166746820555 729 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 6:166746820555 730 NVIC_EnableIRQ(USART2_IRQn);
taknokolat 19:949bab1e9451 731 //NVIC_EnableIRQ(TIM5_IRQn);
taknokolat 19:949bab1e9451 732 //NVIC_EnableIRQ(EXTI0_IRQn);
taknokolat 19:949bab1e9451 733 //NVIC_EnableIRQ(EXTI9_5_IRQn);
taknokolat 6:166746820555 734
taknokolat 6:166746820555 735
taknokolat 6:166746820555 736 //外れ値対策
taknokolat 6:166746820555 737 //rpy*= 180.0f/PI;
taknokolat 6:166746820555 738 if(!setupFlag){
taknokolat 6:166746820555 739 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 740 }else{
taknokolat 6:166746820555 741 if(rpy >= g_FirstYAW_HMC){
taknokolat 6:166746820555 742 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 743 }else{
taknokolat 6:166746820555 744 rpy += 360.0f;
taknokolat 6:166746820555 745 rpy -= g_FirstYAW_HMC;
taknokolat 6:166746820555 746 }
taknokolat 6:166746820555 747 }
taknokolat 6:166746820555 748
taknokolat 6:166746820555 749 if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
taknokolat 6:166746820555 750 if(!flg_checkoutlier || count_changeRPY >= 2){
taknokolat 6:166746820555 751
taknokolat 6:166746820555 752 nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
taknokolat 6:166746820555 753
taknokolat 6:166746820555 754 count_changeRPY = 0;
taknokolat 6:166746820555 755 }else count_changeRPY++;
taknokolat 6:166746820555 756 flg_checkoutlier = false;
taknokolat 6:166746820555 757
taknokolat 24:0ad1725c7849 758 }*/
taknokolat 12:083662bca47d 759
taknokolat 12:083662bca47d 760 int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
taknokolat 12:083662bca47d 761 int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
taknokolat 12:083662bca47d 762 int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
taknokolat 12:083662bca47d 763 int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
taknokolat 16:26cee2aaf61d 764 int *Camera_deg_A, int *Camera_deg_B,
HARUKIDELTA 17:83db05cab3d1 765 int *Pint_speed, float *Pint_wait,
taknokolat 19:949bab1e9451 766 int *Turntable_speed,
taknokolat 24:0ad1725c7849 767 int *Time360, int *Match_wid,
taknokolat 20:011977a37394 768 int *Camera_board_wait
taknokolat 12:083662bca47d 769 ){
taknokolat 12:083662bca47d 770
taknokolat 12:083662bca47d 771 pc.printf("SDsetup start.\r\n");
taknokolat 12:083662bca47d 772
taknokolat 12:083662bca47d 773 FILE *fp;
taknokolat 12:083662bca47d 774 char parameter[20]; //文字列渡す用の配列
taknokolat 12:083662bca47d 775 int SDerrorcount = 0; //取得できなかった数を返す
taknokolat 12:083662bca47d 776 const char *paramNames[] = {
taknokolat 12:083662bca47d 777 "SERVO_NEUTRAL_R",
taknokolat 12:083662bca47d 778 "SERVO_NEUTRAL_L",
taknokolat 12:083662bca47d 779 "SERVO_HIGH_FORWARD_R",
taknokolat 12:083662bca47d 780 "SERVO_HIGH_FORWARD_L",
taknokolat 12:083662bca47d 781 "SERVO_SLOW_FORWARD_R",
taknokolat 12:083662bca47d 782 "SERVO_SLOW_FORWARD_L",
taknokolat 12:083662bca47d 783 "TURNTABLE_NEUTRAL",
taknokolat 12:083662bca47d 784 "MATCH_SPEED",
taknokolat 12:083662bca47d 785 "FOCUS_NEUTRAL",
taknokolat 12:083662bca47d 786 "CAMERA_DEG_A",
taknokolat 16:26cee2aaf61d 787 "CAMERA_DEG_B",
taknokolat 16:26cee2aaf61d 788 "PINT_SPEED",
HARUKIDELTA 17:83db05cab3d1 789 "PINT_WAIT",
taknokolat 19:949bab1e9451 790 "TURNTABLE_SPEED",
taknokolat 24:0ad1725c7849 791 "TIME360",
taknokolat 24:0ad1725c7849 792 "MATCH_WID",
taknokolat 20:011977a37394 793 "CAMERA_BOARD_WAIT"
taknokolat 12:083662bca47d 794 };
taknokolat 12:083662bca47d 795
taknokolat 12:083662bca47d 796 fp = fopen("/sd/option.txt","r");
taknokolat 12:083662bca47d 797
taknokolat 12:083662bca47d 798 if(fp != NULL){ //開けたら
taknokolat 12:083662bca47d 799 pc.printf("File was openned.\r\n");
taknokolat 12:083662bca47d 800 if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
taknokolat 12:083662bca47d 801 else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 802 SDerrorcount++;
taknokolat 12:083662bca47d 803 }
taknokolat 12:083662bca47d 804 if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
taknokolat 12:083662bca47d 805 else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 806 SDerrorcount++;
taknokolat 12:083662bca47d 807 }
taknokolat 12:083662bca47d 808 if(GetParameter(fp,paramNames[2],parameter)) *Servo_high_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 809 else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 810 SDerrorcount++;
taknokolat 12:083662bca47d 811 }
taknokolat 12:083662bca47d 812 if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 813 else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 814 SDerrorcount++;
taknokolat 12:083662bca47d 815 }
taknokolat 12:083662bca47d 816 if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
taknokolat 12:083662bca47d 817 else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 818 SDerrorcount++;
taknokolat 12:083662bca47d 819 }
taknokolat 12:083662bca47d 820 if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
taknokolat 12:083662bca47d 821 else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 822 SDerrorcount++;
taknokolat 12:083662bca47d 823 }
taknokolat 12:083662bca47d 824
taknokolat 12:083662bca47d 825 if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 826 else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 827 SDerrorcount++;
taknokolat 12:083662bca47d 828 }
taknokolat 12:083662bca47d 829 if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
taknokolat 12:083662bca47d 830 else{ *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 831 SDerrorcount++;
taknokolat 12:083662bca47d 832 }
taknokolat 12:083662bca47d 833 if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
taknokolat 12:083662bca47d 834 else{ *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 835 SDerrorcount++;
taknokolat 12:083662bca47d 836 }
taknokolat 19:949bab1e9451 837 if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
taknokolat 12:083662bca47d 838 else{ *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 839 SDerrorcount++;
taknokolat 12:083662bca47d 840 }
taknokolat 19:949bab1e9451 841 if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
taknokolat 12:083662bca47d 842 else{ *Camera_deg_B = camera_deg_B;
taknokolat 12:083662bca47d 843 SDerrorcount++;
taknokolat 12:083662bca47d 844 }
taknokolat 19:949bab1e9451 845 if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
taknokolat 16:26cee2aaf61d 846 else{ *Pint_speed = pint_speed;
taknokolat 16:26cee2aaf61d 847 SDerrorcount++;
taknokolat 16:26cee2aaf61d 848 }
taknokolat 20:011977a37394 849 if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
taknokolat 20:011977a37394 850 else{ *Pint_wait = pint_wait;
taknokolat 16:26cee2aaf61d 851 SDerrorcount++;
taknokolat 16:26cee2aaf61d 852 }
taknokolat 19:949bab1e9451 853 if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
HARUKIDELTA 17:83db05cab3d1 854 else{ *Turntable_speed = turntable_speed;
HARUKIDELTA 17:83db05cab3d1 855 SDerrorcount++;
HARUKIDELTA 17:83db05cab3d1 856 }
taknokolat 24:0ad1725c7849 857 if(GetParameter(fp,paramNames[14],parameter)) *Time360 = atof(parameter);
taknokolat 24:0ad1725c7849 858 else{ *Time360 = time360;
taknokolat 19:949bab1e9451 859 SDerrorcount++;
taknokolat 19:949bab1e9451 860 }
taknokolat 24:0ad1725c7849 861 if(GetParameter(fp,paramNames[15],parameter)) *Match_wid = atof(parameter);
taknokolat 24:0ad1725c7849 862 else{ *Match_wid = match_wid;
taknokolat 19:949bab1e9451 863 SDerrorcount++;
taknokolat 19:949bab1e9451 864 }
taknokolat 20:011977a37394 865 if(GetParameter(fp,paramNames[16],parameter)) *Camera_board_wait = atof(parameter);
taknokolat 20:011977a37394 866 else{ *Camera_board_wait = camera_board_wait;
taknokolat 20:011977a37394 867 SDerrorcount++;
taknokolat 20:011977a37394 868 }
taknokolat 12:083662bca47d 869 fclose(fp);
taknokolat 12:083662bca47d 870
taknokolat 12:083662bca47d 871 }else{ //ファイルがなかったら
taknokolat 12:083662bca47d 872 pc.printf("fp was null.\r\n");
taknokolat 12:083662bca47d 873
taknokolat 12:083662bca47d 874 *Servo_NEUTRAL_R = servo_NEUTRAL_R;
taknokolat 12:083662bca47d 875 *Servo_NEUTRAL_L = servo_NEUTRAL_L;
taknokolat 12:083662bca47d 876 *Servo_high_FORWARD_R = servo_high_FORWARD_R;
taknokolat 12:083662bca47d 877 *Servo_high_FORWARD_L = servo_high_FORWARD_L;
taknokolat 12:083662bca47d 878 *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
taknokolat 12:083662bca47d 879 *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
taknokolat 12:083662bca47d 880 *Turntable_NEUTRAL = turntable_NEUTRAL;
taknokolat 12:083662bca47d 881 *Matchspeed = matchspeed;
taknokolat 12:083662bca47d 882 *Focus_NEUTRAL = focus_NEUTRAL;
taknokolat 12:083662bca47d 883 *Camera_deg_A = camera_deg_A;
taknokolat 12:083662bca47d 884 *Camera_deg_B = camera_deg_B;
taknokolat 16:26cee2aaf61d 885 *Pint_speed = pint_speed;
taknokolat 16:26cee2aaf61d 886 *Pint_wait = pint_wait;
HARUKIDELTA 17:83db05cab3d1 887 *Turntable_speed = turntable_speed;
taknokolat 24:0ad1725c7849 888 *Time360 = time360;
taknokolat 24:0ad1725c7849 889 *Match_wid = match_wid;
taknokolat 22:a19be3b505b7 890 *Camera_board_wait = camera_board_wait;
taknokolat 12:083662bca47d 891 SDerrorcount = -1;
taknokolat 12:083662bca47d 892 }
taknokolat 12:083662bca47d 893 pc.printf("SDsetup finished.\r\n");
taknokolat 12:083662bca47d 894 if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
taknokolat 12:083662bca47d 895 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
taknokolat 12:083662bca47d 896 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
taknokolat 12:083662bca47d 897
taknokolat 12:083662bca47d 898 pc.printf("Servo_NEUTRAL_R = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
taknokolat 13:b088f0db7158 899 pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_L);
taknokolat 13:b088f0db7158 900 pc.printf("Servo_slow_FORWARD_R = %d, Servo_slow_FORWARD_L = %d\r\n", *Servo_slow_FORWARD_R, *Servo_slow_FORWARD_L);
taknokolat 13:b088f0db7158 901 pc.printf("Turntable_NEUTRAL = %d, Matchspeed = %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
taknokolat 13:b088f0db7158 902 pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
taknokolat 13:b088f0db7158 903 pc.printf("Camera_deg_A = %d, Camera_deg_B = %d\r\n", *Camera_deg_A, *Camera_deg_B);
taknokolat 16:26cee2aaf61d 904 pc.printf("Pint_speed = %d, Pint_wait = %f\r\n", *Pint_speed, *Pint_wait);
HARUKIDELTA 17:83db05cab3d1 905 pc.printf("Turntable_speed = %d\r\n",*Turntable_speed);
taknokolat 24:0ad1725c7849 906 pc.printf("Time360 = %d, Match_wid = %d\r\n", *Time360, *Match_wid);
taknokolat 20:011977a37394 907 pc.printf("Camera_board_wait = %d\r\n", *Camera_board_wait);
taknokolat 12:083662bca47d 908
taknokolat 12:083662bca47d 909 return SDerrorcount;
taknokolat 12:083662bca47d 910 }
taknokolat 12:083662bca47d 911 int GetParameter(FILE *fp, const char *paramName,char parameter[]){
taknokolat 12:083662bca47d 912 int i=0, j=0;
taknokolat 12:083662bca47d 913 int strmax = 200;
taknokolat 12:083662bca47d 914 char str[strmax];
taknokolat 12:083662bca47d 915
taknokolat 12:083662bca47d 916 rewind(fp); //ファイル位置を先頭に
taknokolat 12:083662bca47d 917 while(1){
taknokolat 12:083662bca47d 918 if (fgets(str, strmax, fp) == NULL) {
taknokolat 12:083662bca47d 919 return 0;
taknokolat 12:083662bca47d 920 }
taknokolat 12:083662bca47d 921 if (!strncmp(str, paramName, strlen(paramName))) {
taknokolat 12:083662bca47d 922 while (str[i++] != '=') {}
taknokolat 12:083662bca47d 923 while (str[i] != '\n') {
taknokolat 12:083662bca47d 924 parameter[j++] = str[i++];
taknokolat 12:083662bca47d 925 }
taknokolat 12:083662bca47d 926 parameter[j] = '\0';
taknokolat 12:083662bca47d 927 return 1;
taknokolat 12:083662bca47d 928 }
taknokolat 12:083662bca47d 929 }
taknokolat 12:083662bca47d 930 }
taknokolat 12:083662bca47d 931
taknokolat 12:083662bca47d 932
taknokolat 6:166746820555 933
taknokolat 4:67f705d42f1e 934 void DebugPrint(){
taknokolat 4:67f705d42f1e 935 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
taknokolat 7:8989a4b84695 936 //pc.printf("%3.2f\t",nowAngle[2]);
taknokolat 7:8989a4b84695 937 //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
taknokolat 7:8989a4b84695 938 //pc.printf("\r\n");
taknokolat 6:166746820555 939 }