imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

main.cpp@18:4566d1f5fc60, 2019-02-15 (annotated)

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