imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@21:ca8f35e24e66, 2019-02-16 (annotated)

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