Soma Takahashi / Mbed OS kourobo2019

kourobo

Dependencies:   Controller ikarashiMDC

main.cpp@3:e8f08f2be311, 2019-03-13 (annotated)

Committer:
ec30109b
Date:
Wed Mar 13 05:26:53 2019 +0000
Revision:
3:e8f08f2be311
Parent:
2:a583ad08d3d6 
kourobo2019_nagaokaBteam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:9beba3a67526 1 #include "mbed.h"
ec30109b 0:9beba3a67526 2 #include "ikarashiMDC.h"
ec30109b 1:58103ad56096 3 #include "controller.h"
ec30109b 3:e8f08f2be311 4 #define PI 3.141592653589
ec30109b 0:9beba3a67526 5
ec30109b 0:9beba3a67526 6 Serial serial(PC_6, PC_7);
ec30109b 0:9beba3a67526 7 DigitalOut serialcontrol(D2);
ec30109b 0:9beba3a67526 8 DigitalOut led1(LED1);
ec30109b 0:9beba3a67526 9 Serial pc(USBTX, USBRX,115200);
ec30109b 1:58103ad56096 10 AnalogIn analog_value(A0);
ec30109b 1:58103ad56096 11 DigitalOut led(LED1);
ec30109b 2:a583ad08d3d6 12 Controller pad(PC_12,PD_2,198);
ec30109b 1:58103ad56096 13
ec30109b 0:9beba3a67526 14
ec30109b 0:9beba3a67526 15 ikarashiMDC ikarashi[] {
ec30109b 0:9beba3a67526 16 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
ec30109b 0:9beba3a67526 17 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
ec30109b 0:9beba3a67526 18 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
ec30109b 1:58103ad56096 19 ikarashiMDC(&serialcontrol,2,3,SM,&serial),
ec30109b 2:a583ad08d3d6 20 ikarashiMDC(&serialcontrol,3,0,SM,&serial)
ec30109b 0:9beba3a67526 21 };
ec30109b 0:9beba3a67526 22 PwmOut pwm(LED1);
ec30109b 0:9beba3a67526 23
ec30109b 2:a583ad08d3d6 24 float change(float);
ec30109b 2:a583ad08d3d6 25
ec30109b 0:9beba3a67526 26 int main()
ec30109b 0:9beba3a67526 27 {
ec30109b 0:9beba3a67526 28 serial.baud(115200);
ec30109b 0:9beba3a67526 29 float i = -1;
ec30109b 0:9beba3a67526 30 ikarashi[0].braking = true;
ec30109b 0:9beba3a67526 31 float pwm = 0;
ec30109b 1:58103ad56096 32 float meas;
ec30109b 1:58103ad56096 33
ec30109b 1:58103ad56096 34 printf("\nAnalogIn example\n");
ec30109b 1:58103ad56096 35
ec30109b 1:58103ad56096 36 int button[11];
ec30109b 1:58103ad56096 37 double SPe[4];
ec30109b 1:58103ad56096 38 float stickRadian[2], stickNorn[2];
ec30109b 1:58103ad56096 39
ec30109b 1:58103ad56096 40 serial.baud(115200);
ec30109b 1:58103ad56096 41 ikarashi[0].braking = true;
ec30109b 0:9beba3a67526 42
ec30109b 1:58103ad56096 43 while(1) {
ec30109b 1:58103ad56096 44 if(pad.receiveState()){
ec30109b 1:58103ad56096 45 for(int i = 0; i < 13; i++){
ec30109b 1:58103ad56096 46 button[i] = pad.getButton1(i);
ec30109b 3:e8f08f2be311 47 // pc.printf("%d ", button[i]);
ec30109b 1:58103ad56096 48 }
ec30109b 1:58103ad56096 49 for(int i = 0; i < 2; i++){
ec30109b 1:58103ad56096 50 stickRadian[i] = pad.getRadian(i);
ec30109b 1:58103ad56096 51 stickNorn[i] = pad.getNorm(i);
ec30109b 3:e8f08f2be311 52 //pc.printf("%f %f\t", stickRadian[i], stickNorn[i]);
ec30109b 1:58103ad56096 53 }
ec30109b 0:9beba3a67526 54
ec30109b 1:58103ad56096 55 //足回り
ec30109b 1:58103ad56096 56 if(stickNorn[0] != 0){
ec30109b 3:e8f08f2be311 57 SPe[0] = sin(change(stickRadian[0]) - PI * 1/4);
ec30109b 3:e8f08f2be311 58 SPe[1] = sin(change(stickRadian[0]) - PI * 7/4);
ec30109b 3:e8f08f2be311 59 SPe[2] = sin(change(stickRadian[0]) - PI * 5/4);
ec30109b 3:e8f08f2be311 60 SPe[3] = sin(change(stickRadian[0]) - PI * 3/4);
ec30109b 3:e8f08f2be311 61
ec30109b 0:9beba3a67526 62 }
ec30109b 0:9beba3a67526 63
ec30109b 1:58103ad56096 64 //微調整
ec30109b 3:e8f08f2be311 65 if((button[0] == 1 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1) && stickNorn[0] == 0){
ec30109b 2:a583ad08d3d6 66 SPe[0] = 0;
ec30109b 2:a583ad08d3d6 67 SPe[1] = 0;
ec30109b 2:a583ad08d3d6 68 SPe[2] = 0;
ec30109b 2:a583ad08d3d6 69 SPe[3] = 0;
ec30109b 2:a583ad08d3d6 70 }
ec30109b 3:e8f08f2be311 71 if(button[3] == 0 && button[0] == 1 && button[1] == 1 && button[2] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){ //前
ec30109b 1:58103ad56096 72 SPe[0] = 0.4;
ec30109b 1:58103ad56096 73 SPe[1] = 0.4;
ec30109b 1:58103ad56096 74 SPe[2] = -0.4;
ec30109b 1:58103ad56096 75 SPe[3] = -0.4;
ec30109b 0:9beba3a67526 76 }
ec30109b 3:e8f08f2be311 77 if(button[5] == 0 && button[0] == 1 == button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[7] == 1 && button[8] == 1){ //後ろ
ec30109b 1:58103ad56096 78 SPe[0] = -0.4;
ec30109b 1:58103ad56096 79 SPe[1] = -0.4;
ec30109b 1:58103ad56096 80 SPe[2] = 0.4;
ec30109b 1:58103ad56096 81 SPe[3] = 0.4;
ec30109b 0:9beba3a67526 82 }
ec30109b 3:e8f08f2be311 83 if(button[4] == 0 && button[0] == 1 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){ //右
ec30109b 3:e8f08f2be311 84 SPe[0] = -0.5;
ec30109b 3:e8f08f2be311 85 SPe[1] = 0.3;
ec30109b 3:e8f08f2be311 86 SPe[2] = 0.3;
ec30109b 3:e8f08f2be311 87 SPe[3] = -0.4;
ec30109b 0:9beba3a67526 88 }
ec30109b 3:e8f08f2be311 89 if(button[2] == 0 && button[0] == 1 && button[1] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){ //左
ec30109b 3:e8f08f2be311 90 SPe[0] = 0.5;
ec30109b 3:e8f08f2be311 91 SPe[1] = -0.3;
ec30109b 3:e8f08f2be311 92 SPe[2] = -0.3;
ec30109b 3:e8f08f2be311 93 SPe[3] = 0.4;
ec30109b 0:9beba3a67526 94 }
ec30109b 1:58103ad56096 95
ec30109b 1:58103ad56096 96 //右回転
ec30109b 3:e8f08f2be311 97 if(button[7] == 0 && button[0] == 1 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[8] == 1){
ec30109b 1:58103ad56096 98 SPe[0] = -0.4;
ec30109b 1:58103ad56096 99 SPe[1] = -0.4;
ec30109b 1:58103ad56096 100 SPe[2] = -0.4;
ec30109b 1:58103ad56096 101 SPe[3] = -0.4;
ec30109b 1:58103ad56096 102 }
ec30109b 1:58103ad56096 103 //左回転
ec30109b 3:e8f08f2be311 104 if(button[8] == 0 && button[0] == 1 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1){
ec30109b 0:9beba3a67526 105 SPe[0] = 0.4;
ec30109b 0:9beba3a67526 106 SPe[1] = 0.4;
ec30109b 0:9beba3a67526 107 SPe[2] = 0.4;
ec30109b 0:9beba3a67526 108 SPe[3] = 0.4;
ec30109b 0:9beba3a67526 109 }
ec30109b 1:58103ad56096 110
ec30109b 1:58103ad56096 111 //機構
ec30109b 3:e8f08f2be311 112 if(button[0] == 1 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){
ec30109b 2:a583ad08d3d6 113 ikarashi[4].setSpeed(0);
ec30109b 1:58103ad56096 114 }
ec30109b 1:58103ad56096 115
ec30109b 3:e8f08f2be311 116 else if(button[1] == 0 && button[0] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){ //閉じる
ec30109b 3:e8f08f2be311 117 ikarashi[4].setSpeed(-0.17);
ec30109b 2:a583ad08d3d6 118 }
ec30109b 2:a583ad08d3d6 119
ec30109b 3:e8f08f2be311 120 else if(button[0] == 0 && button[1] == 1 && button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 1 && button[7] == 1 && button[8] == 1){ //開く
ec30109b 3:e8f08f2be311 121 ikarashi[4].setSpeed(0.17);
ec30109b 0:9beba3a67526 122 }
ec30109b 3:e8f08f2be311 123 else{
ec30109b 3:e8f08f2be311 124 ikarashi[4].setSpeed(0);
ec30109b 3:e8f08f2be311 125 }
ec30109b 3:e8f08f2be311 126
ec30109b 1:58103ad56096 127
ec30109b 1:58103ad56096 128
ec30109b 1:58103ad56096 129 ikarashi[0].setSpeed(SPe[0]);
ec30109b 1:58103ad56096 130 ikarashi[1].setSpeed(SPe[1]);
ec30109b 1:58103ad56096 131 ikarashi[2].setSpeed(SPe[2]);
ec30109b 1:58103ad56096 132 ikarashi[3].setSpeed(SPe[3]);
ec30109b 1:58103ad56096 133
ec30109b 3:e8f08f2be311 134 //pc.printf("%.2f ",SPe[0]);
ec30109b 3:e8f08f2be311 135 // pc.printf("%.2f ",SPe[1]);
ec30109b 3:e8f08f2be311 136 // pc.printf("%.2f ",SPe[2]);
ec30109b 3:e8f08f2be311 137 // pc.printf("%.2f ",SPe[3]);
ec30109b 2:a583ad08d3d6 138
ec30109b 3:e8f08f2be311 139 //pc.printf("%.3f ",change(stickRadian[0]));
ec30109b 0:9beba3a67526 140
ec30109b 3:e8f08f2be311 141 // pc.printf("\r\n");
ec30109b 1:58103ad56096 142
ec30109b 1:58103ad56096 143 }else{
ec30109b 1:58103ad56096 144 pc.printf("error\n\r");
ec30109b 0:9beba3a67526 145
ec30109b 1:58103ad56096 146 ikarashi[0].setSpeed(0);
ec30109b 1:58103ad56096 147 ikarashi[1].setSpeed(0);
ec30109b 1:58103ad56096 148 ikarashi[2].setSpeed(0);
ec30109b 1:58103ad56096 149 ikarashi[3].setSpeed(0);
ec30109b 2:a583ad08d3d6 150
ec30109b 1:58103ad56096 151 }
ec30109b 0:9beba3a67526 152 }
ec30109b 1:58103ad56096 153 }
ec30109b 1:58103ad56096 154
ec30109b 2:a583ad08d3d6 155 float change(float stiRad){
ec30109b 2:a583ad08d3d6 156 float RAD;
ec30109b 2:a583ad08d3d6 157 if(stiRad >= 0){
ec30109b 2:a583ad08d3d6 158 RAD = PI - stiRad;
ec30109b 2:a583ad08d3d6 159 }
ec30109b 2:a583ad08d3d6 160 if(stiRad < 0){
ec30109b 2:a583ad08d3d6 161 RAD = -(PI) -(stiRad);
ec30109b 2:a583ad08d3d6 162 }
ec30109b 2:a583ad08d3d6 163 return RAD;
ec30109b 2:a583ad08d3d6 164 }
ec30109b 1:58103ad56096 165
ec30109b 2:a583ad08d3d6 166
ec30109b 2:a583ad08d3d6 167
ec30109b 2:a583ad08d3d6 168
ec30109b 2:a583ad08d3d6 169
ec30109b 2:a583ad08d3d6 170
ec30109b 2:a583ad08d3d6 171
ec30109b 2:a583ad08d3d6 172
ec30109b 1:58103ad56096 173