Ryu Anzawa / Mbed OS kouroboA_2019

kourobo2019 teamA

Dependencies:   Controller ikarashiMDC

main.cpp@0:93be607fd77a, 2019-03-18 (annotated)

Committer:
piroro4560
Date:
Mon Mar 18 07:41:34 2019 +0000
Revision:
0:93be607fd77a
Child:
1:e208233598b5
ryu_anzawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:93be607fd77a 1 #include "mbed.h" //交流ロボコン用
piroro4560 0:93be607fd77a 2 #include "controller.h"
piroro4560 0:93be607fd77a 3 #include "ikarashiMDC.h"
piroro4560 0:93be607fd77a 4
piroro4560 0:93be607fd77a 5 Controller pad(PA_0, PA_1, 208);
piroro4560 0:93be607fd77a 6 Serial serial(PC_10, PC_11, 115200);
piroro4560 0:93be607fd77a 7 Serial PC(USBTX,USBRX,115200);
piroro4560 0:93be607fd77a 8 DigitalOut serialcontrol(D2);
piroro4560 0:93be607fd77a 9
piroro4560 0:93be607fd77a 10 ikarashiMDC ikarashi[] {
piroro4560 0:93be607fd77a 11 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
piroro4560 0:93be607fd77a 12 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
piroro4560 0:93be607fd77a 13 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
piroro4560 0:93be607fd77a 14 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
piroro4560 0:93be607fd77a 15 };
piroro4560 0:93be607fd77a 16 PwmOut pwm(LED1);
piroro4560 0:93be607fd77a 17
piroro4560 0:93be607fd77a 18 int main() {
piroro4560 0:93be607fd77a 19 int b[11];
piroro4560 0:93be607fd77a 20 double value[4], spV, spS, Fm, Bm, Sm, acc;
piroro4560 0:93be607fd77a 21 spV = 0.3;/////////速度（前後）
piroro4560 0:93be607fd77a 22 spS = 0.3;/////////速度（左右）
piroro4560 0:93be607fd77a 23 Fm = 1.25;/////////倍率（前）
piroro4560 0:93be607fd77a 24 Bm = 0.9;//////////倍率（後ろ）
piroro4560 0:93be607fd77a 25 Sm = 1.13;/////////倍率（左右）
piroro4560 0:93be607fd77a 26 acc = 22;//////////加速度
piroro4560 0:93be607fd77a 27 ikarashi[0].braking = true;
piroro4560 0:93be607fd77a 28 while (1) {
piroro4560 0:93be607fd77a 29 //仏恩 すてぃっく とりがあ//
piroro4560 0:93be607fd77a 30 if(pad.receiveState()){
piroro4560 0:93be607fd77a 31 for(int i = 0; i < 13; i++){
piroro4560 0:93be607fd77a 32 b[i] = pad.getButton1(i);
piroro4560 0:93be607fd77a 33 }
piroro4560 0:93be607fd77a 34 }
piroro4560 0:93be607fd77a 35 for (int i = 2; i < 6; i++) {
piroro4560 0:93be607fd77a 36 PC.printf("%1d,",b[i]);
piroro4560 0:93be607fd77a 37 }
piroro4560 0:93be607fd77a 38 // PC.printf("%1d,",b[8]);
piroro4560 0:93be607fd77a 39 // PC.printf("%1d,",b[10]);
piroro4560 0:93be607fd77a 40 ////////////////////////////
piroro4560 0:93be607fd77a 41 ////////////////////////////
piroro4560 0:93be607fd77a 42 ////減速////
piroro4560 0:93be607fd77a 43 if (b[2] + b[3] + b[4] + b[5] == 4 && value[0] != 0) {
piroro4560 0:93be607fd77a 44 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 45 value[i] -= value[i] / 4;
piroro4560 0:93be607fd77a 46 }
piroro4560 0:93be607fd77a 47 }
piroro4560 0:93be607fd77a 48 if (b[2] == 0 && b[3] + b[4] + b[5] == 3 && value[0] < spS && value[1] < spS * Sm) {/////////左
piroro4560 0:93be607fd77a 49 value[0] += spS / acc;
piroro4560 0:93be607fd77a 50 value[1] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 51 value[2] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 52 value[3] += spS / acc;
piroro4560 0:93be607fd77a 53 }
piroro4560 0:93be607fd77a 54 if (b[3] == 0 && b[2] + b[4] + b[5] == 3 && value[0] < spV && value[2] < spV * Fm) {//前
piroro4560 0:93be607fd77a 55 value[0] += spV / acc;
piroro4560 0:93be607fd77a 56 value[1] += spV / acc;
piroro4560 0:93be607fd77a 57 value[2] -= spV / acc * Fm;
piroro4560 0:93be607fd77a 58 value[3] -= spV / acc * Fm;
piroro4560 0:93be607fd77a 59 }
piroro4560 0:93be607fd77a 60 if (b[4] == 0 && b[2] + b[3] + b[5] == 3 && value[1] < spS && value[0] < spS * Sm) {//右
piroro4560 0:93be607fd77a 61 value[0] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 62 value[1] += spS / acc;
piroro4560 0:93be607fd77a 63 value[2] += spS / acc;
piroro4560 0:93be607fd77a 64 value[3] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 65 }
piroro4560 0:93be607fd77a 66 if (b[5] == 0 && b[2] + b[3] + b[4] == 3 && value[2] < spV && value[0] < spV * Bm) {//後ろ
piroro4560 0:93be607fd77a 67 value[0] -= spV / acc * Bm;
piroro4560 0:93be607fd77a 68 value[1] -= spV / acc * Bm;
piroro4560 0:93be607fd77a 69 value[2] += spV / acc;
piroro4560 0:93be607fd77a 70 value[3] += spV / acc;
piroro4560 0:93be607fd77a 71 }
piroro4560 0:93be607fd77a 72 ////旋回////
piroro4560 0:93be607fd77a 73 if (b[0] + b[1] + b[2] + b[3] == 4) {
piroro4560 0:93be607fd77a 74 if (b[8] == 0 && b[10] == 1) {
piroro4560 0:93be607fd77a 75 for (int i = 0;i < 4; i++) {
piroro4560 0:93be607fd77a 76 value[i] = 0.20;
piroro4560 0:93be607fd77a 77 }
piroro4560 0:93be607fd77a 78 }
piroro4560 0:93be607fd77a 79 if (b[10] == 0 && b[8] == 1) {
piroro4560 0:93be607fd77a 80 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 81 value[i] = -0.20;
piroro4560 0:93be607fd77a 82 }
piroro4560 0:93be607fd77a 83 }
piroro4560 0:93be607fd77a 84 }
piroro4560 0:93be607fd77a 85 if (b[2] + b[3] + b[4] + b[5] <= 2 || b[2] + b[3] + b[4] + b[5] == 4 && b[8] + b[10] == 2) {
piroro4560 0:93be607fd77a 86 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 87 value[i] -= value[i] / 4;
piroro4560 0:93be607fd77a 88 }
piroro4560 0:93be607fd77a 89 }
piroro4560 0:93be607fd77a 90 ////////////////////////////
piroro4560 0:93be607fd77a 91 ////////////////////////////
piroro4560 0:93be607fd77a 92 if (b[2] + b[3] + b[4] + b[5] <= 2 || b[2] + b[3] + b[4] + b[5] == 4) {
piroro4560 0:93be607fd77a 93 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 94 value[i] -= value[i] / 4;
piroro4560 0:93be607fd77a 95 }
piroro4560 0:93be607fd77a 96 }
piroro4560 0:93be607fd77a 97 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 98 PC.printf("%.2f,", value[i]);
piroro4560 0:93be607fd77a 99 }
piroro4560 0:93be607fd77a 100 PC.printf("||");
piroro4560 0:93be607fd77a 101 ikarashi[0].setSpeed(value[0]);
piroro4560 0:93be607fd77a 102 ikarashi[1].setSpeed(value[1]);
piroro4560 0:93be607fd77a 103 ikarashi[2].setSpeed(value[2]);
piroro4560 0:93be607fd77a 104 ikarashi[3].setSpeed(value[3]);
piroro4560 0:93be607fd77a 105
piroro4560 0:93be607fd77a 106 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 107 PC.printf("%.2f,", value[i]);
piroro4560 0:93be607fd77a 108 }
piroro4560 0:93be607fd77a 109 PC.printf("\n\r");
piroro4560 0:93be607fd77a 110 }
piroro4560 0:93be607fd77a 111 }