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@2:8dd5298ca3a0, 2020-01-04 (annotated)

Committer:
takedayuya
Date:
Sat Jan 04 04:24:19 2020 +0000
Revision:
2:8dd5298ca3a0
Parent:
1:9c7744271d72 
roboconAsyudou

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 2:8dd5298ca3a0 47 int control::CheckButton(int status,int button)
takedayuya 2:8dd5298ca3a0 48 {
takedayuya 2:8dd5298ca3a0 49 if(status == 1) {
takedayuya 2:8dd5298ca3a0 50 pre_button[button] = 1;
takedayuya 2:8dd5298ca3a0 51 if(pre_button[button] == 0) {
takedayuya 2:8dd5298ca3a0 52 return 1;
takedayuya 2:8dd5298ca3a0 53 }
takedayuya 2:8dd5298ca3a0 54 } else {
takedayuya 2:8dd5298ca3a0 55 pre_button[button] = 0;
takedayuya 2:8dd5298ca3a0 56 }
takedayuya 2:8dd5298ca3a0 57 return 0;
takedayuya 2:8dd5298ca3a0 58 }
takedayuya 0:ed8bf5304fbb 59 void control::SolenoidControl()
takedayuya 1:9c7744271d72 60 {
takedayuya 2:8dd5298ca3a0 61 if(CheckButton(sb.PS3_CIRCLE_bit(),CIRCLE)) {
takedayuya 2:8dd5298ca3a0 62 sol.write(0,1);
takedayuya 2:8dd5298ca3a0 63 } else {
takedayuya 2:8dd5298ca3a0 64 sol.write(0,0);
takedayuya 2:8dd5298ca3a0 65 }
takedayuya 2:8dd5298ca3a0 66 if(CheckButton(sb.PS3_SQUARE_bit(),SQUARE)) {
takedayuya 2:8dd5298ca3a0 67 sol.write(1,1);
takedayuya 2:8dd5298ca3a0 68 } else {
takedayuya 2:8dd5298ca3a0 69 sol.write(1,0);
takedayuya 2:8dd5298ca3a0 70 }
takedayuya 2:8dd5298ca3a0 71 if(CheckButton(sb.PS3_CROSS_bit(),CROSS)) {
takedayuya 2:8dd5298ca3a0 72 sol.write(2,1);
takedayuya 2:8dd5298ca3a0 73 } else {
takedayuya 2:8dd5298ca3a0 74 sol.write(2,0);
takedayuya 2:8dd5298ca3a0 75 }
takedayuya 2:8dd5298ca3a0 76 if(CheckButton(sb.PS3_TRIANGLE_bit(),TRIANGLE)) {
takedayuya 2:8dd5298ca3a0 77 sol.write(3,1);
takedayuya 2:8dd5298ca3a0 78 } else {
takedayuya 2:8dd5298ca3a0 79 sol.write(3,0);
takedayuya 0:ed8bf5304fbb 80 }
takedayuya 0:ed8bf5304fbb 81 }