imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@31:210cc32d3175, 2019-02-27 (annotated)

Committer:
taknokolat
Date:
Wed Feb 27 02:39:44 2019 +0000
Revision:
31:210cc32d3175
Parent:
30:90252dc48c1a
Child:
32:ab030832d180
g;

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