Miya Miyagawa / Mbed 2 deprecated OREWOTAOSITAI

aaaaa

Dependencies:   mbed Servo

System/Process/Process.cpp@9:ec30ae33cc9e, 2019-02-18 (annotated)

Committer:
niimurasyou
Date:
Mon Feb 18 06:01:10 2019 +0000
Revision:
9:ec30ae33cc9e
Parent:
8:cb53beff4bb2
Child:
10:a765515594bd
kkkkkkkkkk; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 8:cb53beff4bb2 1 //2018/02/24のやつ
t_yamamoto 5:a802ab60df1b 2
t_yamamoto 0:562021ed1ba9 3 #include "Process.h"
t_yamamoto 0:562021ed1ba9 4
t_yamamoto 0:562021ed1ba9 5 #include "mbed.h"
t_yamamoto 0:562021ed1ba9 6 #include "../../Communication/XBee/XBee.h"
t_yamamoto 0:562021ed1ba9 7 #include "../../Input/Switch/Switch.h"
t_yamamoto 0:562021ed1ba9 8 #include "../../Output/Motor/Motor.h"
t_yamamoto 8:cb53beff4bb2 9 #include "../../Output/Servo/Servo.h"
t_yamamoto 8:cb53beff4bb2 10
t_yamamoto 0:562021ed1ba9 11 //_____________________
t_yamamoto 0:562021ed1ba9 12 /*---------------- HOW TO WRITE ----------------/
t_yamamoto 0:562021ed1ba9 13
t_yamamoto 0:562021ed1ba9 14 ・motor の割り当てを決める
t_yamamoto 0:562021ed1ba9 15 #define TIRE_L 1
t_yamamoto 0:562021ed1ba9 16
t_yamamoto 0:562021ed1ba9 17 ・リミットスイッチの割り当てを決める
t_yamamoto 0:562021ed1ba9 18 #define ARM_L 1
t_yamamoto 0:562021ed1ba9 19
t_yamamoto 0:562021ed1ba9 20 ・他にも自由に定義してもいいです (pwmとか)
t_yamamoto 0:562021ed1ba9 21
t_yamamoto 0:562021ed1ba9 22 /---------------- HOW TO WRITE ----------------*/
t_yamamoto 0:562021ed1ba9 23 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
t_yamamoto 0:562021ed1ba9 24
niimurasyou 9:ec30ae33cc9e 25 #define a 0
niimurasyou 9:ec30ae33cc9e 26 #define b 1
niimurasyou 9:ec30ae33cc9e 27 #define c 2
niimurasyou 9:ec30ae33cc9e 28
niimurasyou 9:ec30ae33cc9e 29 //DigitalOut aira(PA_7);
niimurasyou 9:ec30ae33cc9e 30 //DigitalOut airb(PA_6);
niimurasyou 9:ec30ae33cc9e 31
niimurasyou 9:ec30ae33cc9e 32 int a_array[15][15] = {
niimurasyou 9:ec30ae33cc9e 33 { -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 } ,
niimurasyou 9:ec30ae33cc9e 34 { -40 , -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 , 40 } ,
niimurasyou 9:ec30ae33cc9e 35 { -50 , -40 , -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 , 40 , 50 } ,
niimurasyou 9:ec30ae33cc9e 36 { -60 , -50 , -40 , -30 , -20 , -10 , 0 , 0 , 0 , 10 , 20 , 30 , 40 , 50 , 60 } ,
niimurasyou 9:ec30ae33cc9e 37 { -70 , -60 , -50 , -40 , -30 , -20 , -10 , 0 , 10 , 20 , 30 , 40 , 50 , 60 , 70 } ,
niimurasyou 9:ec30ae33cc9e 38 { -80 , -70 , -60 , -50 , -40 , -30 , -20 , 0 , 20 , 30 , 40 , 50 , 60 , 70 , 80 } ,
niimurasyou 9:ec30ae33cc9e 39 { -90 , -80 , -70 , -60 , -50 , -40 , -30 , 0 , 30 , 40 , 50 , 60 , 70 , 80 , 90 } ,
niimurasyou 9:ec30ae33cc9e 40 { -100, -90 , -80 , -70 , -60 , -50 , -40 , 0 , 40 , 50 , 60 , 70 , 80 , 90 , 100 } ,
niimurasyou 9:ec30ae33cc9e 41 { -90 , -80 , -70 , -60 , -50 , -40 , -30 , 0 , 30 , 40 , 50 , 60 , 70 , 80 , 90 } ,
niimurasyou 9:ec30ae33cc9e 42 { -80 , -70 , -60 , -50 , -40 , -30 , -20 , 0 , 20 , 30 , 40 , 50 , 60 , 70 , 80 } ,
niimurasyou 9:ec30ae33cc9e 43 { -70 , -60 , -50 , -40 , -30 , -20 , -10 , 0 , 10 , 20 , 30 , 40 , 50 , 60 , 70 } ,
niimurasyou 9:ec30ae33cc9e 44 { -60 , -50 , -40 , -30 , -20 , -10 , 0 , 0 , 0 , 10 , 20 , 30 , 40 , 50 , 60 } ,
niimurasyou 9:ec30ae33cc9e 45 { -50 , -40 , -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 , 40 , 50 } ,
niimurasyou 9:ec30ae33cc9e 46 { -40 , -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 , 40 } ,
niimurasyou 9:ec30ae33cc9e 47 { -30 , -20 , -10 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 10 , 20 , 30 }
niimurasyou 9:ec30ae33cc9e 48 };
niimurasyou 9:ec30ae33cc9e 49
niimurasyou 9:ec30ae33cc9e 50 int b_array[15][15] = {
niimurasyou 9:ec30ae33cc9e 51 { 100 , 92 , 84 , 76 , 68 , 60 , 52 , 100 , 36 , 28 , 20 , 12 , 4 , 0 , 0 } ,
niimurasyou 9:ec30ae33cc9e 52 { 92 , 84 , 76 , 68 , 60 , 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 } ,
niimurasyou 9:ec30ae33cc9e 53 { 84 , 76 , 68 , 60 , 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 } ,
niimurasyou 9:ec30ae33cc9e 54 { 76 , 68 , 60 , 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 } ,
niimurasyou 9:ec30ae33cc9e 55 { 68 , 60 , 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 } ,
niimurasyou 9:ec30ae33cc9e 56 { 60 , 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 } ,
niimurasyou 9:ec30ae33cc9e 57 { 52 , 44 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 } ,
niimurasyou 9:ec30ae33cc9e 58 { 100 , 36 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 } ,
niimurasyou 9:ec30ae33cc9e 59 { 85 , 28 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 } ,
niimurasyou 9:ec30ae33cc9e 60 { 70 , 20 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 , -60 } ,
niimurasyou 9:ec30ae33cc9e 61 { 55 , 12 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 , -60 , -68 } ,
niimurasyou 9:ec30ae33cc9e 62 { 40 , 4 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 , -60 , -68 , -76 } ,
niimurasyou 9:ec30ae33cc9e 63 { 25 , 0 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 , -60 , -68 , -76 , -84 } ,
niimurasyou 9:ec30ae33cc9e 64 { 10 , 0 , 0 , -4 , -12 , -20 , -28 , -36 , -44 , -52 , -60 , -68 , -76 , -84 , -92 } ,
niimurasyou 9:ec30ae33cc9e 65 { 0 , 0 , -4 , -12 , -20 , -28 , -36 , -100 , -52 , -60 , -68 , -76 , -84 , -92 , -100 }
niimurasyou 9:ec30ae33cc9e 66 };
t_yamamoto 0:562021ed1ba9 67
t_yamamoto 0:562021ed1ba9 68
niimurasyou 9:ec30ae33cc9e 69 int c_array[15][15] = {
niimurasyou 9:ec30ae33cc9e 70 { 0 , 0 , 4 , 12 , 20 , 28 , 36 , 100 , 52 , 60 , 68 , 76 , 84 , 92 , 100 } ,
niimurasyou 9:ec30ae33cc9e 71 { 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 , 60 , 68 , 76 , 84 , 92 } ,
niimurasyou 9:ec30ae33cc9e 72 { -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 , 60 , 68 , 76 , 84 } ,
niimurasyou 9:ec30ae33cc9e 73 { -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 , 60 , 68 , 76 } ,
niimurasyou 9:ec30ae33cc9e 74 { -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 , 60 , 68 } ,
niimurasyou 9:ec30ae33cc9e 75 { -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 , 60 } ,
niimurasyou 9:ec30ae33cc9e 76 { -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 , 52 } ,
niimurasyou 9:ec30ae33cc9e 77 { -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 , 44 } ,
niimurasyou 9:ec30ae33cc9e 78 { -52 , -44 , -36 , -28 , -20 , -12 , 4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 , 36 } ,
niimurasyou 9:ec30ae33cc9e 79 { -60 , -52 , -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 , 28 } ,
niimurasyou 9:ec30ae33cc9e 80 { -68 , -60 , -52 , -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 , 20 } ,
niimurasyou 9:ec30ae33cc9e 81 { -76 , -68 , -60 , -52 , -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 , 12 } ,
niimurasyou 9:ec30ae33cc9e 82 { -84 , -76 , -68 , -60 , -52 , -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 , 4 } ,
niimurasyou 9:ec30ae33cc9e 83 { -92 , -84 , -76 , -68 , -60 , -52 , -44 , -36 , -28 , -20 , -12 , -4 , 0 , 0 , 0 } ,
niimurasyou 9:ec30ae33cc9e 84 { -100 , -92 , -84 , -76 , -76 , -60 , -100 , -100 , -36 , -28 , -20 , -12 , -4 , 0 , 0 }
niimurasyou 9:ec30ae33cc9e 85 };
t_yamamoto 0:562021ed1ba9 86
niimurasyou 9:ec30ae33cc9e 87 int pwm_array[15] = { 30 , 25 , 20 , 15 , 10 , 5 , 0 , 0 , 0 , -5 , -10 , -15 , -20 , -25 , -30 };
niimurasyou 9:ec30ae33cc9e 88
niimurasyou 9:ec30ae33cc9e 89 #define usiro 0
niimurasyou 9:ec30ae33cc9e 90 #define mae 0
niimurasyou 9:ec30ae33cc9e 91 uint8_t motorData[5];
niimurasyou 9:ec30ae33cc9e 92 uint8_t pwmData[5];
niimurasyou 9:ec30ae33cc9e 93
niimurasyou 9:ec30ae33cc9e 94 int conlx;
niimurasyou 9:ec30ae33cc9e 95 int conly;
niimurasyou 9:ec30ae33cc9e 96 int conrx;
niimurasyou 9:ec30ae33cc9e 97 int conry;
niimurasyou 9:ec30ae33cc9e 98 int conba;
niimurasyou 9:ec30ae33cc9e 99 int conbb;
niimurasyou 9:ec30ae33cc9e 100 int conbx;
niimurasyou 9:ec30ae33cc9e 101 int conby;
t_yamamoto 0:562021ed1ba9 102
t_yamamoto 0:562021ed1ba9 103 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
t_yamamoto 0:562021ed1ba9 104 //_____________________
t_yamamoto 0:562021ed1ba9 105
t_yamamoto 8:cb53beff4bb2 106 //#define USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 107 #ifdef USE_USB_SERIAL
t_yamamoto 0:562021ed1ba9 108 Serial pc(SERIAL_TX, SERIAL_RX);
t_yamamoto 0:562021ed1ba9 109 #endif
t_yamamoto 0:562021ed1ba9 110 XBEE::ControllerData *controller;
t_yamamoto 0:562021ed1ba9 111 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:562021ed1ba9 112
t_yamamoto 0:562021ed1ba9 113 using namespace SWITCH;
t_yamamoto 0:562021ed1ba9 114
t_yamamoto 8:cb53beff4bb2 115 void SystemProcess(void) {
t_yamamoto 0:562021ed1ba9 116 while(true) {
t_yamamoto 0:562021ed1ba9 117 controller = XBEE::Controller::GetData();
t_yamamoto 0:562021ed1ba9 118 //____________________________
t_yamamoto 0:562021ed1ba9 119 /*------------------------ HOW TO WRITE ------------------------/
t_yamamoto 0:562021ed1ba9 120
t_yamamoto 0:562021ed1ba9 121 ここにメインのプログラムを書く
t_yamamoto 0:562021ed1ba9 122
t_yamamoto 0:562021ed1ba9 123 ・コントローラから受け取ったデータをもとに動作のプログラムを書く
t_yamamoto 0:562021ed1ba9 124 (コントローラのデータは controller-> で取る)
t_yamamoto 0:562021ed1ba9 125
t_yamamoto 0:562021ed1ba9 126 if(controller->Button.RIGHT) {
t_yamamoto 0:562021ed1ba9 127 motor[TIRE_L].dir = FOR;
t_yamamoto 0:562021ed1ba9 128 motor[TIRE_R].dir = BACK;
t_yamamoto 7:f4252ddc480d 129 motor[TIRE_L].pwm = 12.3;
t_yamamoto 7:f4252ddc480d 130 motor[TIRE_R].pwm = 12.3;
t_yamamoto 0:562021ed1ba9 131 }
t_yamamoto 0:562021ed1ba9 132
t_yamamoto 6:c294736ba99d 133 motor[0].dirは FOR (正転)
t_yamamoto 0:562021ed1ba9 134 BACK (逆転)
t_yamamoto 0:562021ed1ba9 135 BRAKE (ブレーキ)
t_yamamoto 0:562021ed1ba9 136 FREE (フリー)
t_yamamoto 0:562021ed1ba9 137
t_yamamoto 0:562021ed1ba9 138 motor[0].pwmは 0.0(%) ~ 100.0(%)
t_yamamoto 0:562021ed1ba9 139
t_yamamoto 0:562021ed1ba9 140 controllerは XBee.hの構造体の中身
t_yamamoto 0:562021ed1ba9 141
t_yamamoto 0:562021ed1ba9 142 (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
t_yamamoto 0:562021ed1ba9 143
t_yamamoto 0:562021ed1ba9 144 ・リミットスイッチの値をもとに動作のプログラムを書く
t_yamamoto 0:562021ed1ba9 145
t_yamamoto 0:562021ed1ba9 146 if(Switch::CheckPushed(ARM_L))
t_yamamoto 0:562021ed1ba9 147 {
t_yamamoto 0:562021ed1ba9 148 if(controller->Button.L)
t_yamamoto 0:562021ed1ba9 149 {
t_yamamoto 6:c294736ba99d 150 motor[ARM].dir = FOR;
t_yamamoto 0:562021ed1ba9 151 motor[ARM].pwm = 80.0;
t_yamamoto 0:562021ed1ba9 152 }
t_yamamoto 0:562021ed1ba9 153 if(motor[ARM].dir == BACK)
t_yamamoto 0:562021ed1ba9 154 {
t_yamamoto 0:562021ed1ba9 155 motor[ARM].dir = BRAKE;
t_yamamoto 0:562021ed1ba9 156 }
t_yamamoto 0:562021ed1ba9 157 }
t_yamamoto 0:562021ed1ba9 158
t_yamamoto 0:562021ed1ba9 159 →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
t_yamamoto 0:562021ed1ba9 160
t_yamamoto 0:562021ed1ba9 161 ・他にもやりたいことがあったら自由にどうぞ
t_yamamoto 0:562021ed1ba9 162
t_yamamoto 0:562021ed1ba9 163 ps.わからないことがあったら聞いてください
t_yamamoto 0:562021ed1ba9 164
t_yamamoto 0:562021ed1ba9 165 /------------------------ HOW TO WRITE ------------------------*/
t_yamamoto 0:562021ed1ba9 166 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
t_yamamoto 0:562021ed1ba9 167
niimurasyou 9:ec30ae33cc9e 168 conlx = controller->AnalogL.X;
niimurasyou 9:ec30ae33cc9e 169 conly = controller->AnalogL.Y;
niimurasyou 9:ec30ae33cc9e 170 conrx = controller->AnalogR.X;
niimurasyou 9:ec30ae33cc9e 171 conry = controller->AnalogR.Y;
niimurasyou 9:ec30ae33cc9e 172 //conba = controller->Button.A;
niimurasyou 9:ec30ae33cc9e 173 //conbb = controller->Button.B;
niimurasyou 9:ec30ae33cc9e 174 //conby = controller->Button.Y;
niimurasyou 9:ec30ae33cc9e 175 //conbx = controller->Button.X;
t_yamamoto 0:562021ed1ba9 176
niimurasyou 9:ec30ae33cc9e 177 if(((conlx >6) && (conly <8)) ||((conrx >6)&&(conry <8))){
niimurasyou 9:ec30ae33cc9e 178 motor[a].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 179 motor[b].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 180 motor[c].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 181 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 182 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 183 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 184 }
t_yamamoto 0:562021ed1ba9 185
niimurasyou 9:ec30ae33cc9e 186 if(a_array[conly][conlx]<0){
niimurasyou 9:ec30ae33cc9e 187 motor[a].pwm = -1*a_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 188 motor[a].dir = FOR;
niimurasyou 9:ec30ae33cc9e 189 }else if(a_array[conly][conlx] > 0){
niimurasyou 9:ec30ae33cc9e 190 motor[a].pwm = a_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 191 motor[a].dir = BACK;
niimurasyou 9:ec30ae33cc9e 192 }else if(a_array[conly][conlx] == 0){
niimurasyou 9:ec30ae33cc9e 193 if(conry < 6){
niimurasyou 9:ec30ae33cc9e 194 motor[a].dir = BACK;
niimurasyou 9:ec30ae33cc9e 195 motor[b].dir = BACK;
niimurasyou 9:ec30ae33cc9e 196 motor[c].dir = BACK;
niimurasyou 9:ec30ae33cc9e 197 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 198 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 199 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 200 }else if(conry > 8){
niimurasyou 9:ec30ae33cc9e 201 motor[a].dir = FOR;
niimurasyou 9:ec30ae33cc9e 202 motor[b].dir = FOR;
niimurasyou 9:ec30ae33cc9e 203 motor[c].dir = FOR;
niimurasyou 9:ec30ae33cc9e 204 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 205 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 206 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 207 }else{
niimurasyou 9:ec30ae33cc9e 208 motor[a].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 209 motor[b].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 210 motor[c].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 211 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 212 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 213 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 214 }
niimurasyou 9:ec30ae33cc9e 215 }
niimurasyou 9:ec30ae33cc9e 216
niimurasyou 9:ec30ae33cc9e 217 if(b_array[conly][conlx] <0 ){
niimurasyou 9:ec30ae33cc9e 218 motor[b].pwm = -1*b_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 219 motor[b].dir = FOR;
niimurasyou 9:ec30ae33cc9e 220 }else if(b_array[conly][conlx] > 0){
niimurasyou 9:ec30ae33cc9e 221 motor[b].pwm = b_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 222 motor[b].dir = BACK;
niimurasyou 9:ec30ae33cc9e 223 }else if(a_array[conly][conlx] == 0 ){
niimurasyou 9:ec30ae33cc9e 224 if(conrx < 6 ) {
niimurasyou 9:ec30ae33cc9e 225 motor[a].dir = BACK;
niimurasyou 9:ec30ae33cc9e 226 motor[b].dir = BACK;
niimurasyou 9:ec30ae33cc9e 227 motor[c].dir = BACK;
niimurasyou 9:ec30ae33cc9e 228 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 229 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 230 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 231 }else if(conrx > 8){
niimurasyou 9:ec30ae33cc9e 232 motor[a].dir = FOR;
niimurasyou 9:ec30ae33cc9e 233 motor[b].dir = FOR;
niimurasyou 9:ec30ae33cc9e 234 motor[c].dir = FOR;
niimurasyou 9:ec30ae33cc9e 235 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 236 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 237 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 238 }else{
niimurasyou 9:ec30ae33cc9e 239 motor[a].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 240 motor[b].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 241 motor[c].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 242 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 243 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 244 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 245 }
niimurasyou 9:ec30ae33cc9e 246 }
niimurasyou 9:ec30ae33cc9e 247
niimurasyou 9:ec30ae33cc9e 248
niimurasyou 9:ec30ae33cc9e 249 if(c_array[conly][conlx] <0 ){
niimurasyou 9:ec30ae33cc9e 250 motor[c].pwm =-1*c_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 251 motor[c].dir = BACK;
niimurasyou 9:ec30ae33cc9e 252 }else if(c_array[conly][conlx] > 0){
niimurasyou 9:ec30ae33cc9e 253 motor[c].pwm = c_array[conly][conlx];
niimurasyou 9:ec30ae33cc9e 254 motor[c].dir = FOR;
niimurasyou 9:ec30ae33cc9e 255 }else if(a_array[conly][conlx] == 0 ){
niimurasyou 9:ec30ae33cc9e 256 if(conrx < 6 ) {
niimurasyou 9:ec30ae33cc9e 257 motor[a].dir = BACK;
niimurasyou 9:ec30ae33cc9e 258 motor[b].dir = BACK;
niimurasyou 9:ec30ae33cc9e 259 motor[c].dir = BACK;
niimurasyou 9:ec30ae33cc9e 260 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 261 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 262 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 263 }else if(conrx > 8){
niimurasyou 9:ec30ae33cc9e 264 motor[a].dir = FOR;
niimurasyou 9:ec30ae33cc9e 265 motor[b].dir = FOR;
niimurasyou 9:ec30ae33cc9e 266 motor[c].dir = FOR;
niimurasyou 9:ec30ae33cc9e 267 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 268 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 269 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 270 }else{
niimurasyou 9:ec30ae33cc9e 271 motor[a].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 272 motor[b].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 273 motor[c].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 274 motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 275 motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 276 motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 277 }
niimurasyou 9:ec30ae33cc9e 278
niimurasyou 9:ec30ae33cc9e 279 //
niimurasyou 9:ec30ae33cc9e 280 //if (a_array[conlx>6][conly<8]){
niimurasyou 9:ec30ae33cc9e 281 // motor[a].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 282 // motor[a].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 283 // }
niimurasyou 9:ec30ae33cc9e 284 //if (b_array[conlx>6][conly<8]){
niimurasyou 9:ec30ae33cc9e 285 // motor[b].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 286 // motor[b].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 287 // }
niimurasyou 9:ec30ae33cc9e 288 //if (c_array[conlx>6][conly<8]){
niimurasyou 9:ec30ae33cc9e 289 // motor[c].dir = BRAKE;
niimurasyou 9:ec30ae33cc9e 290 // motor[c].pwm = 100.0;
niimurasyou 9:ec30ae33cc9e 291 // }
niimurasyou 9:ec30ae33cc9e 292 //
niimurasyou 9:ec30ae33cc9e 293 //
niimurasyou 9:ec30ae33cc9e 294 //(conba)
niimurasyou 9:ec30ae33cc9e 295 //{aira=0;}
niimurasyou 9:ec30ae33cc9e 296 // if(conbb)
niimurasyou 9:ec30ae33cc9e 297 // {airb=0;}
niimurasyou 9:ec30ae33cc9e 298
niimurasyou 9:ec30ae33cc9e 299
niimurasyou 9:ec30ae33cc9e 300
niimurasyou 9:ec30ae33cc9e 301
niimurasyou 9:ec30ae33cc9e 302
niimurasyou 9:ec30ae33cc9e 303
niimurasyou 9:ec30ae33cc9e 304
niimurasyou 9:ec30ae33cc9e 305
niimurasyou 9:ec30ae33cc9e 306
niimurasyou 9:ec30ae33cc9e 307 }
niimurasyou 9:ec30ae33cc9e 308
niimurasyou 9:ec30ae33cc9e 309
niimurasyou 9:ec30ae33cc9e 310
niimurasyou 9:ec30ae33cc9e 311
niimurasyou 9:ec30ae33cc9e 312 //
niimurasyou 9:ec30ae33cc9e 313 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
niimurasyou 9:ec30ae33cc9e 314 //___________________________
t_yamamoto 0:562021ed1ba9 315
t_yamamoto 0:562021ed1ba9 316
niimurasyou 9:ec30ae33cc9e 317
t_yamamoto 0:562021ed1ba9 318 MOTOR::Motor::Update(motor);
t_yamamoto 0:562021ed1ba9 319 }
niimurasyou 9:ec30ae33cc9e 320 }