NagaokaRoboticsClub_mbedTeam / Mbed OS haseomni_main

ハセオムニのプログラム

Dependencies:   vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3

main.cpp@12:8d5e9965e8f2, 2020-01-24 (annotated)

Committer:
LVRhase01
Date:
Fri Jan 24 08:58:19 2020 +0000
Revision:
12:8d5e9965e8f2
Parent:
11:85568f4e4eaf
Child:
13:0810457baab8
wasdcontroller not PID

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 1:3d8552eac4fa 4 #include "omni_wheel.h"
LVRhase01 4:07dc5c86e702 5 #include "motorsmlap.h"
LVRhase01 4:07dc5c86e702 6 #include "SerialMultiByte.h"
LVRhase01 1:3d8552eac4fa 7
LVRhase01 12:8d5e9965e8f2 8 Serial pc(USBTX, USBRX, 115200);
LVRhase01 4:07dc5c86e702 9
LVRhase01 1:3d8552eac4fa 10 PwmOut beep(PA_0);
LVRhase01 1:3d8552eac4fa 11 DigitalOut led1(PA_11);
LVRhase01 1:3d8552eac4fa 12 DigitalOut led2(LED2);
LVRhase01 1:3d8552eac4fa 13
LVRhase01 4:07dc5c86e702 14 motorSmLap motor[] = {
LVRhase01 4:07dc5c86e702 15 motorSmLap(PC_6, PC_9, PC_8),
LVRhase01 4:07dc5c86e702 16 motorSmLap(PB_6, PA_9, PC_7),
LVRhase01 4:07dc5c86e702 17 motorSmLap(PA_8, PB_4, PB_7)
LVRhase01 4:07dc5c86e702 18 };
LVRhase01 5:3eed67b60cd2 19
LVRhase01 1:3d8552eac4fa 20 const double PII = 3.14159265358979;
LVRhase01 7:4e77a1ae70d1 21 OmniWheel omni(3);
LVRhase01 0:b3c48e055e7b 22
LVRhase01 1:3d8552eac4fa 23 void setup(){
LVRhase01 1:3d8552eac4fa 24 omni.wheel[0].setRadian(PII/2.0);
LVRhase01 3:9e20624d3d0d 25 omni.wheel[1].setRadian(7.0/6.0*PII);
LVRhase01 3:9e20624d3d0d 26 omni.wheel[2].setRadian(11.0/6.0*PII);
LVRhase01 1:3d8552eac4fa 27
LVRhase01 1:3d8552eac4fa 28 beep.period(1.0/3136);
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/2960);
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/2489);
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/1760);
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/1661);
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/2637);
LVRhase01 1:3d8552eac4fa 49 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 50 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 51
LVRhase01 1:3d8552eac4fa 52 beep.period(1.0/3322);
LVRhase01 1:3d8552eac4fa 53 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 54 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 55
LVRhase01 1:3d8552eac4fa 56 beep.period(1.0/4186);
LVRhase01 1:3d8552eac4fa 57 beep.write(0.4f);
LVRhase01 1:3d8552eac4fa 58 wait_ms(BEAT);
LVRhase01 1:3d8552eac4fa 59
LVRhase01 1:3d8552eac4fa 60 beep.write(0.0f);
LVRhase01 1:3d8552eac4fa 61 wait_ms(BEAT);
LVRhase01 0:b3c48e055e7b 62 }
LVRhase01 0:b3c48e055e7b 63
LVRhase01 1:3d8552eac4fa 64 int main() {
LVRhase01 1:3d8552eac4fa 65 setup();
LVRhase01 12:8d5e9965e8f2 66 float x, y;
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 1:3d8552eac4fa 86 omni.computeXY(
LVRhase01 1:3d8552eac4fa 87 x,
LVRhase01 1:3d8552eac4fa 88 y,
LVRhase01 1:3d8552eac4fa 89 0,
LVRhase01 1:3d8552eac4fa 90 0,
LVRhase01 12:8d5e9965e8f2 91 0
LVRhase01 7:4e77a1ae70d1 92
LVRhase01 1:3d8552eac4fa 93 );
LVRhase01 2:5a55f555e885 94 for(int i=0; i<3; i++){
LVRhase01 4:07dc5c86e702 95 motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
LVRhase01 2:5a55f555e885 96 }
LVRhase01 12:8d5e9965e8f2 97 pc.printf("X = <%f>,Y = <%f>\r\n",x,y);
LVRhase01 1:3d8552eac4fa 98 }
LVRhase01 5:3eed67b60cd2 99 }