NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@16:1d9bc2849c79, 2020-02-29 (annotated)

Committer:
LVRhase01
Date:
Sat Feb 29 09:23:37 2020 +0000
Revision:
16:1d9bc2849c79
Parent:
14:03e27cf2c6b3 
gakuseikikaku mode add ; change motor pin out

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LVRhase01 1:3d8552eac4fa 1 #define BEAT 140
LVRhase01 0:b3c48e055e7b 2 #include "mbed.h"
LVRhase01 1:3d8552eac4fa 3 #include "math.h"
LVRhase01 4:07dc5c86e702 4 #include "motorsmlap.h"
LVRhase01 1:3d8552eac4fa 5
LVRhase01 12:8d5e9965e8f2 6 Serial pc(USBTX, USBRX, 115200);
LVRhase01 4:07dc5c86e702 7
LVRhase01 1:3d8552eac4fa 8 PwmOut beep(PA_0);
LVRhase01 1:3d8552eac4fa 9 DigitalOut led1(PA_11);
LVRhase01 1:3d8552eac4fa 10 DigitalOut led2(LED2);
LVRhase01 1:3d8552eac4fa 11
LVRhase01 4:07dc5c86e702 12 motorSmLap motor[] = {
LVRhase01 4:07dc5c86e702 13 motorSmLap(PC_6, PC_9, PC_8),
LVRhase01 16:1d9bc2849c79 14 motorSmLap(PA_9,PB_6, PC_7),
LVRhase01 16:1d9bc2849c79 15 motorSmLap(PB_4, PA_8, PB_7)
LVRhase01 4:07dc5c86e702 16 };
LVRhase01 5:3eed67b60cd2 17
LVRhase01 1:3d8552eac4fa 18 void setup(){
LVRhase01 1:3d8552eac4fa 19
LVRhase01 1:3d8552eac4fa 20 beep.period(1.0/3136);
LVRhase01 1:3d8552eac4fa 21 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 22 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 23
LVRhase01 1:3d8552eac4fa 24 beep.period(1.0/2960);
LVRhase01 1:3d8552eac4fa 25 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 26 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 27
LVRhase01 1:3d8552eac4fa 28 beep.period(1.0/2489);
LVRhase01 1:3d8552eac4fa 29 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 30 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 31
LVRhase01 1:3d8552eac4fa 32 beep.period(1.0/1760);
LVRhase01 1:3d8552eac4fa 33 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 34 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 35
LVRhase01 1:3d8552eac4fa 36 beep.period(1.0/1661);
LVRhase01 1:3d8552eac4fa 37 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 38 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 39
LVRhase01 1:3d8552eac4fa 40 beep.period(1.0/2637);
LVRhase01 1:3d8552eac4fa 41 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 42 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 43
LVRhase01 1:3d8552eac4fa 44 beep.period(1.0/3322);
LVRhase01 1:3d8552eac4fa 45 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 46 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 47
LVRhase01 1:3d8552eac4fa 48 beep.period(1.0/4186);
LVRhase01 1:3d8552eac4fa 49 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 50 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 51
LVRhase01 1:3d8552eac4fa 52 beep.write(0.0f);
LVRhase01 1:3d8552eac4fa 53 wait_ms(BEAT);
LVRhase01 0:b3c48e055e7b 54 }
LVRhase01 0:b3c48e055e7b 55
LVRhase01 1:3d8552eac4fa 56 int main() {
LVRhase01 1:3d8552eac4fa 57 setup();
LVRhase01 12:8d5e9965e8f2 58 float x, y;
LVRhase01 13:0810457baab8 59 double speed[3]={0};
LVRhase01 1:3d8552eac4fa 60 while (true) {
LVRhase01 5:3eed67b60cd2 61 led2=1;
LVRhase01 11:85568f4e4eaf 62 int c = pc.getc();
LVRhase01 11:85568f4e4eaf 63 if(c == 'w' ){
LVRhase01 11:85568f4e4eaf 64 y=0.5;
LVRhase01 11:85568f4e4eaf 65 }
LVRhase01 11:85568f4e4eaf 66 if(c == 's' ){
LVRhase01 11:85568f4e4eaf 67 y=-0.5;
LVRhase01 11:85568f4e4eaf 68 }
LVRhase01 11:85568f4e4eaf 69 if(c == 'd' ){
LVRhase01 11:85568f4e4eaf 70 x=0.5;
LVRhase01 11:85568f4e4eaf 71 }
LVRhase01 11:85568f4e4eaf 72 if(c == 'a' ){
LVRhase01 11:85568f4e4eaf 73 x=-0.5;
LVRhase01 11:85568f4e4eaf 74 }
LVRhase01 11:85568f4e4eaf 75 if(c=='q'){
LVRhase01 11:85568f4e4eaf 76 x=0;
LVRhase01 11:85568f4e4eaf 77 y=0;
LVRhase01 11:85568f4e4eaf 78 }
LVRhase01 13:0810457baab8 79
LVRhase01 2:5a55f555e885 80 for(int i=0; i<3; i++){
LVRhase01 13:0810457baab8 81 speed[i] = sin(atan2(y,x) - PI * (i*4 + 3) / 6);
LVRhase01 13:0810457baab8 82 motor[i].setMotorSpeed(speed[i]*hypot(x,y));
LVRhase01 2:5a55f555e885 83 }
LVRhase01 1:3d8552eac4fa 84 }
LVRhase01 5:3eed67b60cd2 85 }