ハセオムニのプログラム
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?
User | Revision | Line number | New 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 | } |