takeda yuya / Mbed 2 deprecated NITKK2019WinterA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mdc_read

Control.cpp@1:9c7744271d72, 2019-12-28 (annotated)

Committer:
takedayuya
Date:
Sat Dec 28 06:18:27 2019 +0000
Revision:
1:9c7744271d72
Parent:
0:ed8bf5304fbb
Child:
2:8dd5298ca3a0
kumamotoAteamsyudou;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takedayuya 0:ed8bf5304fbb 1 #include "mbed.h"
takedayuya 0:ed8bf5304fbb 2 #include "Control.h"
takedayuya 0:ed8bf5304fbb 3 #include "SBDBT.h"
takedayuya 0:ed8bf5304fbb 4 #include "pinmap.h"
takedayuya 0:ed8bf5304fbb 5
takedayuya 1:9c7744271d72 6 void control::setup()
takedayuya 1:9c7744271d72 7 {
takedayuya 1:9c7744271d72 8 for(int i = 0; i <= 4; i++) {
takedayuya 1:9c7744271d72 9 bias[i]=0.0f;
takedayuya 0:ed8bf5304fbb 10 }
takedayuya 0:ed8bf5304fbb 11
takedayuya 0:ed8bf5304fbb 12 }
takedayuya 0:ed8bf5304fbb 13
takedayuya 1:9c7744271d72 14 void control::run()
takedayuya 1:9c7744271d72 15 {
takedayuya 1:9c7744271d72 16
takedayuya 0:ed8bf5304fbb 17 sb.PS3_JOYSTICK();
takedayuya 0:ed8bf5304fbb 18 OmniControl();
takedayuya 0:ed8bf5304fbb 19
takedayuya 0:ed8bf5304fbb 20 }
takedayuya 0:ed8bf5304fbb 21
takedayuya 0:ed8bf5304fbb 22 void control::OmniControl()
takedayuya 0:ed8bf5304fbb 23 {
takedayuya 0:ed8bf5304fbb 24 Vx = sb.JoyStick_dt[0] * -1;
takedayuya 0:ed8bf5304fbb 25 Vy = sb.JoyStick_dt[1];
takedayuya 0:ed8bf5304fbb 26 wl = sb.JoyStick_dt[2];
takedayuya 1:9c7744271d72 27
takedayuya 0:ed8bf5304fbb 28 pwm[0]=(Vx -Vy) * bias[0] + wl;
takedayuya 0:ed8bf5304fbb 29 pwm[1]=(-1)*(Vx -Vy) * bias[1] + wl;
takedayuya 0:ed8bf5304fbb 30 pwm[2]=(-1)*(Vx -Vy) * bias[2] + wl;
takedayuya 0:ed8bf5304fbb 31 pwm[3]=(Vx -Vy) * bias[3] + wl;
takedayuya 1:9c7744271d72 32
takedayuya 1:9c7744271d72 33 for(int i = 0; i < 4; i++) {
takedayuya 1:9c7744271d72 34 if(pwm[i] > 127) {
takedayuya 0:ed8bf5304fbb 35 pwm[i] = 127;
takedayuya 1:9c7744271d72 36 } else if(pwm[i] < -127) {
takedayuya 0:ed8bf5304fbb 37 pwm[i] = -127;
takedayuya 0:ed8bf5304fbb 38 }
takedayuya 1:9c7744271d72 39 }
takedayuya 0:ed8bf5304fbb 40
takedayuya 0:ed8bf5304fbb 41 mt1.move(pwm[0]/130.0f);
takedayuya 0:ed8bf5304fbb 42 mt2.move(pwm[1]/130.0f);
takedayuya 0:ed8bf5304fbb 43 mt3.move(pwm[2]/130.0f);
takedayuya 0:ed8bf5304fbb 44 mt4.move(pwm[3]/130.0f);
takedayuya 0:ed8bf5304fbb 45 }
takedayuya 0:ed8bf5304fbb 46
takedayuya 0:ed8bf5304fbb 47 void control::SolenoidControl()
takedayuya 1:9c7744271d72 48 {
takedayuya 1:9c7744271d72 49 if(sb.PS3_R1_bit()==1) {
takedayuya 1:9c7744271d72 50 if(sb.PS3_CIRCLE_bit() == 1&&pre_PS3_R1_bit==0) {
takedayuya 1:9c7744271d72 51 if(pre_button_circle[0]==0/*&&sb.PS3_R1_bit()==1*/) {
takedayuya 1:9c7744271d72 52 sol.write(0,1);
takedayuya 1:9c7744271d72 53 } else {
takedayuya 1:9c7744271d72 54 pre_button_circle[0]=1;//sb.PS3_R1_bit()==0;
takedayuya 1:9c7744271d72 55 sol.write(0,0);
takedayuya 1:9c7744271d72 56 }
takedayuya 0:ed8bf5304fbb 57 }
takedayuya 1:9c7744271d72 58 if(sb.PS3_TRIANGLE_bit() == 1&&pre_PS3_R1_bit==0) {
takedayuya 1:9c7744271d72 59 if(pre_button_triangle[0]==0/*&&sb.PS3_R1_bit()==1*/) {
takedayuya 1:9c7744271d72 60 sol.write(1,1);
takedayuya 1:9c7744271d72 61 } else {
takedayuya 1:9c7744271d72 62 pre_button_triangle[0]=1;//sb.PS3_R1_bit()==0;
takedayuya 1:9c7744271d72 63 sol.write(1,0);
takedayuya 1:9c7744271d72 64 }
takedayuya 1:9c7744271d72 65 if(sb.PS3_SQUARE_bit() == 1&&pre_PS3_R1_bit==0) {
takedayuya 1:9c7744271d72 66 if(pre_button_square[0]==0/*&&sb.PS3_R1_bit()==1*/) {
takedayuya 1:9c7744271d72 67 sol.write(2,1);
takedayuya 1:9c7744271d72 68 } else {
takedayuya 1:9c7744271d72 69 pre_button_square[0]=1;//sb.PS3_R1_bit()==0;
takedayuya 1:9c7744271d72 70 sol.write(2,0);
takedayuya 1:9c7744271d72 71
takedayuya 1:9c7744271d72 72 }
takedayuya 1:9c7744271d72 73 if(sb.PS3_CROSS_bit() == 1&&pre_PS3_R1_bit==0) {
takedayuya 1:9c7744271d72 74 if(pre_button_cross[0]==0) {
takedayuya 1:9c7744271d72 75 sol.write(3,1);
takedayuya 1:9c7744271d72 76 } else {
takedayuya 1:9c7744271d72 77 pre_button_cross[0]=1;//;sb.PS3_R1_bit()==0;
takedayuya 1:9c7744271d72 78 sol.write(3,0);
takedayuya 1:9c7744271d72 79 }
takedayuya 1:9c7744271d72 80 }
takedayuya 1:9c7744271d72 81 pre_PS3_R1_bit=1;
takedayuya 1:9c7744271d72 82 }else{
takedayuya 1:9c7744271d72 83 pre_PS3_R1_bit=0;
takedayuya 1:9c7744271d72 84 }
takedayuya 0:ed8bf5304fbb 85 }
takedayuya 0:ed8bf5304fbb 86 }
takedayuya 0:ed8bf5304fbb 87 }