keiichi shimazu / Mbed 2 deprecated NITKK2019_WinterB-Manual_madebyshimazu

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@4:90b840185921, 2019-12-28 (annotated)

Committer:
shimazukeiichi
Date:
Sat Dec 28 07:38:40 2019 +0000
Revision:
4:90b840185921
Parent:
3:42a8da1837f7 
NITKK2019_WinterroboconB

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OishiHinata 0:9d5b8d0fb901 1 #include "mbed.h"
OishiHinata 0:9d5b8d0fb901 2 #include "Control.h"
OishiHinata 0:9d5b8d0fb901 3 #include "pinmap.h"
OishiHinata 3:42a8da1837f7 4 #include "SBDBT.h"
OishiHinata 0:9d5b8d0fb901 5 void control::setup(){
OishiHinata 0:9d5b8d0fb901 6 sb.setup(0);
OishiHinata 0:9d5b8d0fb901 7 }
OishiHinata 0:9d5b8d0fb901 8
OishiHinata 3:42a8da1837f7 9 void control::run()
OishiHinata 3:42a8da1837f7 10 {
OishiHinata 3:42a8da1837f7 11 sb.PS3_JOYSTICK();
OishiHinata 3:42a8da1837f7 12 OmniControl();
OishiHinata 3:42a8da1837f7 13 ModuleControl();
OishiHinata 3:42a8da1837f7 14 }
OishiHinata 3:42a8da1837f7 15 void control::OmniControl()
OishiHinata 3:42a8da1837f7 16 {
OishiHinata 1:39ada37dfdbe 17 Vx = sb.JoyStick_dt[0] * -1;
OishiHinata 1:39ada37dfdbe 18 Vy = sb.JoyStick_dt[1];
OishiHinata 1:39ada37dfdbe 19 wl = sb.JoyStick_dt[2];
OishiHinata 2:43e1336c80e4 20
OishiHinata 0:9d5b8d0fb901 21
OishiHinata 0:9d5b8d0fb901 22 pwm[0]= (Vx - Vy)*bias[0] + wl;
OishiHinata 0:9d5b8d0fb901 23 pwm[1]=(-1)*(Vx + Vy)*bias[1] + wl;
OishiHinata 0:9d5b8d0fb901 24 pwm[2]=(-1)*(Vx - Vy)*bias[2] + wl;
OishiHinata 0:9d5b8d0fb901 25 pwm[3]= (Vx + Vy)*bias[3] + wl;
OishiHinata 0:9d5b8d0fb901 26
OishiHinata 2:43e1336c80e4 27
OishiHinata 0:9d5b8d0fb901 28 for(int i = 0; i < 4; i++){
OishiHinata 0:9d5b8d0fb901 29 if(pwm[i] > 127){
OishiHinata 0:9d5b8d0fb901 30 pwm[i] =127;
OishiHinata 0:9d5b8d0fb901 31 }else if(pwm[i] < -127){
OishiHinata 0:9d5b8d0fb901 32 pwm[i] = -127;
OishiHinata 0:9d5b8d0fb901 33 }
OishiHinata 0:9d5b8d0fb901 34 }
OishiHinata 0:9d5b8d0fb901 35
OishiHinata 0:9d5b8d0fb901 36 mt1.move(pwm[0]/130.0f);
OishiHinata 0:9d5b8d0fb901 37 mt2.move(pwm[1]/130.0f);
OishiHinata 0:9d5b8d0fb901 38 mt3.move(pwm[2]/130.0f);
OishiHinata 0:9d5b8d0fb901 39 mt4.move(pwm[3]/130.0f);
OishiHinata 3:42a8da1837f7 40 mt5.move(pwm[4]/130.0f);
OishiHinata 3:42a8da1837f7 41 mt6.move(pwm[5]/130.0f);
OishiHinata 1:39ada37dfdbe 42 }
OishiHinata 1:39ada37dfdbe 43
OishiHinata 3:42a8da1837f7 44
OishiHinata 3:42a8da1837f7 45 int control::CheckButton(int status,int button)
OishiHinata 3:42a8da1837f7 46 {
OishiHinata 3:42a8da1837f7 47 if(status == 1){
OishiHinata 3:42a8da1837f7 48 pre_button[button] = 1;
OishiHinata 3:42a8da1837f7 49 if(pre_button[button] == 0){
OishiHinata 3:42a8da1837f7 50 return 1;
OishiHinata 3:42a8da1837f7 51 }
OishiHinata 3:42a8da1837f7 52 }else{
OishiHinata 3:42a8da1837f7 53 pre_button[button] = 0;
OishiHinata 3:42a8da1837f7 54 }
OishiHinata 3:42a8da1837f7 55 return 0;
OishiHinata 3:42a8da1837f7 56 }
OishiHinata 3:42a8da1837f7 57
OishiHinata 3:42a8da1837f7 58 void control::ModuleControl()
OishiHinata 3:42a8da1837f7 59 {
OishiHinata 3:42a8da1837f7 60 if(CheckButton(sb.PS3_TRIANGLE_bit(),TRIANGLE) && s_time > 100){
OishiHinata 3:42a8da1837f7 61 pwm[4] = 0.40f;
OishiHinata 3:42a8da1837f7 62 }else{
OishiHinata 3:42a8da1837f7 63 pwm[4] = 0.0f;
OishiHinata 3:42a8da1837f7 64 }
OishiHinata 3:42a8da1837f7 65
OishiHinata 3:42a8da1837f7 66 if(CheckButton(sb.PS3_CROSS_bit(),CROSS) && s_time > 100){
OishiHinata 3:42a8da1837f7 67 pwm[4] = 0.40f;
OishiHinata 3:42a8da1837f7 68 }else{
OishiHinata 3:42a8da1837f7 69 pwm[4] = 0.0f;
OishiHinata 3:42a8da1837f7 70 }
OishiHinata 3:42a8da1837f7 71
OishiHinata 3:42a8da1837f7 72 if(CheckButton(sb.PS3_CIRCLE_bit(),CIRCLE)){
OishiHinata 3:42a8da1837f7 73 pwm[5] = 0.40f;
OishiHinata 3:42a8da1837f7 74 }else{
OishiHinata 3:42a8da1837f7 75 pwm[5] = 0.0f;
OishiHinata 3:42a8da1837f7 76 }
shimazukeiichi 4:90b840185921 77 if(CheckButton(sb.PS3_SQUARE_bit(),s_time > 500)){
shimazukeiichi 4:90b840185921 78 if(ls){
shimazukeiichi 4:90b840185921 79 pwm[6] = 0.40f;
shimazukeiichi 4:90b840185921 80 pwm[7] = 0.40f;
shimazukeiichi 4:90b840185921 81 }else{
shimazukeiichi 4:90b840185921 82 pwm[6] = 0.0f;
shimazukeiichi 4:90b840185921 83 pwm[7] = 0.0f;
shimazukeiichi 4:90b840185921 84 }
shimazukeiichi 4:90b840185921 85 }else{
shimazukeiichi 4:90b840185921 86 pwm[6] = 0.0f;
shimazukeiichi 4:90b840185921 87 pwm[7] = 0.0f;
shimazukeiichi 4:90b840185921 88 }
shimazukeiichi 4:90b840185921 89 if(CheckButton(sb.PS3_R1_bit(),R1)&&CheckButton(sb.PS3_L1_bit(),L1)){
shimazukeiichi 4:90b840185921 90 pwm[8] = 0.60f;
shimazukeiichi 4:90b840185921 91 }else{
shimazukeiichi 4:90b840185921 92 pwm[8] = 0.0f;
shimazukeiichi 4:90b840185921 93 }
shimazukeiichi 4:90b840185921 94
shimazukeiichi 4:90b840185921 95
shimazukeiichi 4:90b840185921 96
OishiHinata 3:42a8da1837f7 97 }
OishiHinata 3:42a8da1837f7 98
OishiHinata 1:39ada37dfdbe 99