NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@13:0810457baab8, 2020-01-24 (annotated)

Committer:
LVRhase01
Date:
Fri Jan 24 09:32:07 2020 +0000
Revision:
13:0810457baab8
Parent:
12:8d5e9965e8f2
Child:
14:03e27cf2c6b3
omniwheel.h not use mode by wasdcontroller

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