Ryu Anzawa / Mbed OS kouroboA_2019

kourobo2019 teamA

Dependencies:   Controller ikarashiMDC

main.cpp@4:94f943530940, 2019-03-30 (annotated)

Committer:
piroro4560
Date:
Sat Mar 30 10:34:08 2019 +0000
Revision:
4:94f943530940
Parent:
2:6cce570ce61b 
kourobo Ateam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 1:e208233598b5 1 #include "mbed.h"
piroro4560 0:93be607fd77a 2 #include "controller.h"
piroro4560 0:93be607fd77a 3 #include "ikarashiMDC.h"
piroro4560 0:93be607fd77a 4
piroro4560 4:94f943530940 5 Controller pad(PA_0, PA_1, );
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 2:6cce570ce61b 20 double value[4], rad[2], dis[2];
piroro4560 2:6cce570ce61b 21 double spV = 0.3;/////////speed Vertical
piroro4560 2:6cce570ce61b 22 double spS = 0.3;/////////speed Side
piroro4560 4:94f943530940 23 double spT = 0.15;////////speed Turn
piroro4560 4:94f943530940 24 double Fm = 1.0;//////////Front magnification
piroro4560 4:94f943530940 25 double Bm = 1.0;//////////Back magnification
piroro4560 4:94f943530940 26 double Sm = 1.0///////////Side magnification
piroro4560 4:94f943530940 27 double acc = 25;//////////acceleration
piroro4560 4:94f943530940 28 double dec = 5;///////////deceleration
piroro4560 0:93be607fd77a 29 ikarashi[0].braking = true;
piroro4560 0:93be607fd77a 30 while (1) {
piroro4560 2:6cce570ce61b 31 //button stick trigger//
piroro4560 2:6cce570ce61b 32 if(pad.receiveState()){
piroro4560 0:93be607fd77a 33 for(int i = 0; i < 13; i++){
piroro4560 0:93be607fd77a 34 b[i] = pad.getButton1(i);
piroro4560 1:e208233598b5 35 pc.printf("%d ", [i]);
piroro4560 0:93be607fd77a 36 }
piroro4560 1:e208233598b5 37 for(int i = 0; i < 2; i++){
piroro4560 1:e208233598b5 38 rad[i] = pad.getRadian(i);
piroro4560 1:e208233598b5 39 dis[i] = pad.getNorm(i);
piroro4560 2:6cce570ce61b 40 PC.printf("%f %f\t", rad[i], dis[i]);
piroro4560 1:e208233598b5 41 }
piroro4560 0:93be607fd77a 42 }
piroro4560 2:6cce570ce61b 43 /***************************
piroro4560 2:6cce570ce61b 44 ***************************/
piroro4560 2:6cce570ce61b 45 ////acceleration////
piroro4560 1:e208233598b5 46 if (b[2] == 0 && b[3] + b[4] + b[5] == 3 && value[0] < spS && value[1] < spS * Sm) {
piroro4560 0:93be607fd77a 47 value[0] += spS / acc;
piroro4560 0:93be607fd77a 48 value[1] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 49 value[2] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 50 value[3] += spS / acc;
piroro4560 0:93be607fd77a 51 }
piroro4560 1:e208233598b5 52 if (b[3] == 0 && b[2] + b[4] + b[5] == 3 && value[0] < spV && value[2] < spV * Fm) {
piroro4560 0:93be607fd77a 53 value[0] += spV / acc;
piroro4560 0:93be607fd77a 54 value[1] += spV / acc;
piroro4560 0:93be607fd77a 55 value[2] -= spV / acc * Fm;
piroro4560 0:93be607fd77a 56 value[3] -= spV / acc * Fm;
piroro4560 0:93be607fd77a 57 }
piroro4560 1:e208233598b5 58 if (b[4] == 0 && b[2] + b[3] + b[5] == 3 && value[1] < spS && value[0] < spS * Sm) {
piroro4560 0:93be607fd77a 59 value[0] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 60 value[1] += spS / acc;
piroro4560 0:93be607fd77a 61 value[2] += spS / acc;
piroro4560 0:93be607fd77a 62 value[3] -= spS / acc * Sm;
piroro4560 0:93be607fd77a 63 }
piroro4560 1:e208233598b5 64 if (b[5] == 0 && b[2] + b[3] + b[4] == 3 && value[2] < spV && value[0] < spV * Bm) {
piroro4560 0:93be607fd77a 65 value[0] -= spV / acc * Bm;
piroro4560 0:93be607fd77a 66 value[1] -= spV / acc * Bm;
piroro4560 0:93be607fd77a 67 value[2] += spV / acc;
piroro4560 0:93be607fd77a 68 value[3] += spV / acc;
piroro4560 0:93be607fd77a 69 }
piroro4560 2:6cce570ce61b 70 /********/
piroro4560 1:e208233598b5 71 ////turn////
piroro4560 0:93be607fd77a 72 if (b[0] + b[1] + b[2] + b[3] == 4) {
piroro4560 0:93be607fd77a 73 if (b[8] == 0 && b[10] == 1) {
piroro4560 0:93be607fd77a 74 for (int i = 0;i < 4; i++) {
piroro4560 2:6cce570ce61b 75 value[i] = spT;
piroro4560 0:93be607fd77a 76 }
piroro4560 0:93be607fd77a 77 }
piroro4560 0:93be607fd77a 78 if (b[10] == 0 && b[8] == 1) {
piroro4560 0:93be607fd77a 79 for (int i = 0; i < 4; i++) {
piroro4560 2:6cce570ce61b 80 value[i] = -spT;
piroro4560 0:93be607fd77a 81 }
piroro4560 0:93be607fd77a 82 }
piroro4560 0:93be607fd77a 83 }
piroro4560 2:6cce570ce61b 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 1:e208233598b5 87 value[i] -= value[i] / dec;
piroro4560 0:93be607fd77a 88 }
piroro4560 0:93be607fd77a 89 }
piroro4560 1:e208233598b5 90 /***************************
piroro4560 1:e208233598b5 91 ***************************/
piroro4560 0:93be607fd77a 92 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 93 PC.printf("%.2f,", value[i]);
piroro4560 0:93be607fd77a 94 }
piroro4560 0:93be607fd77a 95 PC.printf("||");
piroro4560 0:93be607fd77a 96 ikarashi[0].setSpeed(value[0]);
piroro4560 0:93be607fd77a 97 ikarashi[1].setSpeed(value[1]);
piroro4560 0:93be607fd77a 98 ikarashi[2].setSpeed(value[2]);
piroro4560 0:93be607fd77a 99 ikarashi[3].setSpeed(value[3]);
piroro4560 0:93be607fd77a 100
piroro4560 2:6cce570ce61b 101 for (int i = 0; i < 4; i++) {
piroro4560 0:93be607fd77a 102 PC.printf("%.2f,", value[i]);
piroro4560 0:93be607fd77a 103 }
piroro4560 0:93be607fd77a 104 PC.printf("\n\r");
piroro4560 0:93be607fd77a 105 }
piroro4560 0:93be607fd77a 106 }