航空研究会 / Mbed 2 deprecated hassi_10_usb

usb実装中

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp@1:290e621741fd, 2018-12-22 (annotated)

Committer:
taknokolat
Date:
Sat Dec 22 10:41:25 2018 +0000
Revision:
1:290e621741fd
Parent:
0:84ddd6d354e1
Child:
2:f30666d7838b
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HARUKIDELTA 0:84ddd6d354e1 1 #include "mbed.h"
HARUKIDELTA 0:84ddd6d354e1 2
HARUKIDELTA 0:84ddd6d354e1 3 #define servo_NEUTRAL_R 1460
HARUKIDELTA 0:84ddd6d354e1 4 #define servo_NEUTRAL_L 1460
HARUKIDELTA 0:84ddd6d354e1 5 #define servo_FORWARD_R 1860
HARUKIDELTA 0:84ddd6d354e1 6 #define servo_FORWARD_L 1860
HARUKIDELTA 0:84ddd6d354e1 7 #define servo_back_R 1060
HARUKIDELTA 0:84ddd6d354e1 8 #define servo_back_L 1060
HARUKIDELTA 0:84ddd6d354e1 9 #define servo_slow_FORWARD_R 1560
HARUKIDELTA 0:84ddd6d354e1 10 #define servo_slow_FORWARD_L 1560
HARUKIDELTA 0:84ddd6d354e1 11 #define servo_slow_back_R 1360
HARUKIDELTA 0:84ddd6d354e1 12 #define servo_slow_back_L 1360
HARUKIDELTA 0:84ddd6d354e1 13
HARUKIDELTA 0:84ddd6d354e1 14 #define MOVE_NEUTRAL 0
HARUKIDELTA 0:84ddd6d354e1 15 #define MOVE_FORWARD 1
HARUKIDELTA 0:84ddd6d354e1 16 #define MOVE_LEFT 2
HARUKIDELTA 0:84ddd6d354e1 17 #define MOVE_RIGHT 3
HARUKIDELTA 0:84ddd6d354e1 18 #define MOVE_BACK 4
HARUKIDELTA 0:84ddd6d354e1 19 #define GOAL_FORWARD 5 //ゴール付近 ゆっくり
HARUKIDELTA 0:84ddd6d354e1 20 #define GOAL_LEFT 6
HARUKIDELTA 0:84ddd6d354e1 21 #define GOAL_RIGHT 7
HARUKIDELTA 0:84ddd6d354e1 22 #define MAX_FORWARD 8 //はやい 姿勢修正用
HARUKIDELTA 0:84ddd6d354e1 23 #define MAX_BACK 9
HARUKIDELTA 0:84ddd6d354e1 24
taknokolat 1:290e621741fd 25 void getSF_Serial_jevois();
taknokolat 1:290e621741fd 26 void getSF_Serial_pi();
taknokolat 1:290e621741fd 27
HARUKIDELTA 0:84ddd6d354e1 28 PwmOut servoR(PC_6);
HARUKIDELTA 0:84ddd6d354e1 29 PwmOut servoL(PC_7);
HARUKIDELTA 0:84ddd6d354e1 30
taknokolat 1:290e621741fd 31 RawSerial pc(PA_2,PA_3,115200);
taknokolat 1:290e621741fd 32 RawSerial pc2(PB_6,PB_7,115200);
taknokolat 1:290e621741fd 33
taknokolat 1:290e621741fd 34 char g_landingcommand='X';
taknokolat 1:290e621741fd 35
taknokolat 1:290e621741fd 36 void MoveCansat(char g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 37
HARUKIDELTA 0:84ddd6d354e1 38 int main() {
taknokolat 1:290e621741fd 39
taknokolat 1:290e621741fd 40 // シリアル通信受信の割り込みイベント登録
taknokolat 1:290e621741fd 41 pc.attach(getSF_Serial_jevois, Serial::RxIrq);
taknokolat 1:290e621741fd 42 pc2.attach(getSF_Serial_pi, Serial::RxIrq);
taknokolat 1:290e621741fd 43
taknokolat 1:290e621741fd 44 NVIC_SetPriority(USART1_IRQn,0);//割り込み優先度
taknokolat 1:290e621741fd 45 NVIC_SetPriority(USART2_IRQn,1);
taknokolat 1:290e621741fd 46
HARUKIDELTA 0:84ddd6d354e1 47 while(1) {
taknokolat 1:290e621741fd 48 //pc.printf("Hello World!!");
taknokolat 1:290e621741fd 49 MoveCansat(g_landingcommand);
HARUKIDELTA 0:84ddd6d354e1 50 }
HARUKIDELTA 0:84ddd6d354e1 51 }
HARUKIDELTA 0:84ddd6d354e1 52
taknokolat 1:290e621741fd 53 void MoveCansat(char g_landingcommand)
HARUKIDELTA 0:84ddd6d354e1 54 {
taknokolat 1:290e621741fd 55 NVIC_DisableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 56 NVIC_DisableIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 57 switch(g_landingcommand){
taknokolat 1:290e621741fd 58 case 'N': //MOVE_NEUTRAL
taknokolat 1:290e621741fd 59 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 60 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 61 servoR.pulsewidth_us(servo_NEUTRAL_R);
HARUKIDELTA 0:84ddd6d354e1 62 servoL.pulsewidth_us(servo_NEUTRAL_L);
HARUKIDELTA 0:84ddd6d354e1 63 break;
taknokolat 1:290e621741fd 64
taknokolat 1:290e621741fd 65 case 'F': //MOVE_FORWARD
taknokolat 1:290e621741fd 66 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 67 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 68 servoR.pulsewidth_us(servo_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 69 servoL.pulsewidth_us(servo_FORWARD_L);
HARUKIDELTA 0:84ddd6d354e1 70 break;
taknokolat 1:290e621741fd 71
taknokolat 1:290e621741fd 72 case 'L': //MOVE_LEFT
taknokolat 1:290e621741fd 73 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 74 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 75 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 76 servoL.pulsewidth_us(servo_slow_back_L);
taknokolat 1:290e621741fd 77
taknokolat 1:290e621741fd 78 case 'R': //MOVE_RIGHT
taknokolat 1:290e621741fd 79 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 80 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 81 servoR.pulsewidth_us(servo_slow_back_R);
HARUKIDELTA 0:84ddd6d354e1 82 servoL.pulsewidth_us(servo_slow_FORWARD_L);
HARUKIDELTA 0:84ddd6d354e1 83 break;
taknokolat 1:290e621741fd 84
taknokolat 1:290e621741fd 85 case 'B': //MOVE_BACK
taknokolat 1:290e621741fd 86 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 87 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 88 servoR.pulsewidth_us(servo_back_R);
HARUKIDELTA 0:84ddd6d354e1 89 servoL.pulsewidth_us(servo_back_L);
HARUKIDELTA 0:84ddd6d354e1 90 break;
taknokolat 1:290e621741fd 91
taknokolat 1:290e621741fd 92 case 'G': //GOAL_FORWARD
taknokolat 1:290e621741fd 93 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 94 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 95 servoR.pulsewidth_us(servo_slow_FORWARD_R);
HARUKIDELTA 0:84ddd6d354e1 96 servoL.pulsewidth_us(servo_slow_FORWARD_L);
HARUKIDELTA 0:84ddd6d354e1 97 break;
taknokolat 1:290e621741fd 98
taknokolat 1:290e621741fd 99 default :
taknokolat 1:290e621741fd 100 NVIC_EnableIRQ(USART1_IRQn);
taknokolat 1:290e621741fd 101 NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA 0:84ddd6d354e1 102 break;
taknokolat 1:290e621741fd 103
HARUKIDELTA 0:84ddd6d354e1 104 }
HARUKIDELTA 0:84ddd6d354e1 105 return;
taknokolat 1:290e621741fd 106 }
taknokolat 1:290e621741fd 107
taknokolat 1:290e621741fd 108 void getSF_Serial_jevois(){
taknokolat 1:290e621741fd 109
taknokolat 1:290e621741fd 110
taknokolat 1:290e621741fd 111 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 112
taknokolat 1:290e621741fd 113 static int bufcounter=0;
taknokolat 1:290e621741fd 114
taknokolat 1:290e621741fd 115
taknokolat 1:290e621741fd 116
taknokolat 1:290e621741fd 117 if(pc.readable()) { // 受信確認
taknokolat 1:290e621741fd 118
taknokolat 1:290e621741fd 119 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 120 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 121 //pc.printf("x");
taknokolat 1:290e621741fd 122 return;
taknokolat 1:290e621741fd 123 }
taknokolat 1:290e621741fd 124
taknokolat 1:290e621741fd 125
taknokolat 1:290e621741fd 126
taknokolat 1:290e621741fd 127 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 128
taknokolat 1:290e621741fd 129 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 130
taknokolat 1:290e621741fd 131 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 132
taknokolat 1:290e621741fd 133 g_landingcommand = SFbuf[1];
taknokolat 1:290e621741fd 134 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 135 //wait_ms(20);
taknokolat 1:290e621741fd 136 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 137 bufcounter = 0;
taknokolat 1:290e621741fd 138 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 139 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 140 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 141 }
taknokolat 1:290e621741fd 142
taknokolat 1:290e621741fd 143 else if(bufcounter>=5){
taknokolat 1:290e621741fd 144 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 145 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 146 bufcounter = 0;
taknokolat 1:290e621741fd 147 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 148 }
taknokolat 1:290e621741fd 149 }
taknokolat 1:290e621741fd 150
taknokolat 1:290e621741fd 151
taknokolat 1:290e621741fd 152 }
taknokolat 1:290e621741fd 153
taknokolat 1:290e621741fd 154
taknokolat 1:290e621741fd 155 void getSF_Serial_pi(){
taknokolat 1:290e621741fd 156
taknokolat 1:290e621741fd 157
taknokolat 1:290e621741fd 158 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
taknokolat 1:290e621741fd 159
taknokolat 1:290e621741fd 160 static int bufcounter=0;
taknokolat 1:290e621741fd 161
taknokolat 1:290e621741fd 162
taknokolat 1:290e621741fd 163
taknokolat 1:290e621741fd 164 if(pc2.readable()) { // 受信確認
taknokolat 1:290e621741fd 165
taknokolat 1:290e621741fd 166 SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
taknokolat 1:290e621741fd 167 if(SFbuf[0]!='S'){
taknokolat 1:290e621741fd 168 //pc.printf("x");
taknokolat 1:290e621741fd 169 return;
taknokolat 1:290e621741fd 170 }
taknokolat 1:290e621741fd 171
taknokolat 1:290e621741fd 172
taknokolat 1:290e621741fd 173
taknokolat 1:290e621741fd 174 //pc.printf("%c",SFbuf[bufcounter]);
taknokolat 1:290e621741fd 175
taknokolat 1:290e621741fd 176 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
taknokolat 1:290e621741fd 177
taknokolat 1:290e621741fd 178 if(bufcounter==5 && SFbuf[4]=='F'){
taknokolat 1:290e621741fd 179
taknokolat 1:290e621741fd 180 g_landingcommand = SFbuf[1];
taknokolat 1:290e621741fd 181 //pc.printf("%c",g_landingcommand);
taknokolat 1:290e621741fd 182 //wait_ms(20);
taknokolat 1:290e621741fd 183 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
taknokolat 1:290e621741fd 184 bufcounter = 0;
taknokolat 1:290e621741fd 185 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 186 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 187 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
taknokolat 1:290e621741fd 188 }
taknokolat 1:290e621741fd 189
taknokolat 1:290e621741fd 190 else if(bufcounter>=5){
taknokolat 1:290e621741fd 191 //pc.printf("Communication Falsed.\r\n");
taknokolat 1:290e621741fd 192 memset(SFbuf, 0, sizeof(SFbuf));
taknokolat 1:290e621741fd 193 bufcounter = 0;
taknokolat 1:290e621741fd 194 NVIC_ClearPendingIRQ(USART2_IRQn);
taknokolat 1:290e621741fd 195 }
taknokolat 1:290e621741fd 196 }
taknokolat 1:290e621741fd 197
taknokolat 1:290e621741fd 198
taknokolat 1:290e621741fd 199 }
taknokolat 1:290e621741fd 200