miyake_sotsuken / Mbed 2 deprecated sotsuken_mecha

sotsuken

Dependencies:   IncEncoder mbed

main.cpp@0:a1f453dd2bb5, 23 months ago (annotated)

Committer:
koki_konishi
Date:
Mon Dec 12 05:50:18 2022 +0000
Revision:
0:a1f453dd2bb5
mecha

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koki_konishi 0:a1f453dd2bb5 1 #include "mbed.h"
koki_konishi 0:a1f453dd2bb5 2 #include "IncEncoder.h"
koki_konishi 0:a1f453dd2bb5 3 #include <math.h>
koki_konishi 0:a1f453dd2bb5 4
koki_konishi 0:a1f453dd2bb5 5 #define PI 3.14159265358979
koki_konishi 0:a1f453dd2bb5 6
koki_konishi 0:a1f453dd2bb5 7 #define BELT_RPP 512
koki_konishi 0:a1f453dd2bb5 8 #define ANG_RPP 512
koki_konishi 0:a1f453dd2bb5 9 #define LIFT_RPP 256
koki_konishi 0:a1f453dd2bb5 10
koki_konishi 0:a1f453dd2bb5 11 #define BELT_PER 1050
koki_konishi 0:a1f453dd2bb5 12 #define BELT_Rratio 5
koki_konishi 0:a1f453dd2bb5 13
koki_konishi 0:a1f453dd2bb5 14 #define LIFT_PCD 54
koki_konishi 0:a1f453dd2bb5 15
koki_konishi 0:a1f453dd2bb5 16 #define BELT_UC 30 //モータ出力オフセット
koki_konishi 0:a1f453dd2bb5 17 #define ANG_UC 25
koki_konishi 0:a1f453dd2bb5 18 #define LIFT_UC 36
koki_konishi 0:a1f453dd2bb5 19
koki_konishi 0:a1f453dd2bb5 20 #define VELO_dt 0.010
koki_konishi 0:a1f453dd2bb5 21 #define BELT_dt 0.010
koki_konishi 0:a1f453dd2bb5 22 #define ANG_dt 0.010
koki_konishi 0:a1f453dd2bb5 23 #define LIFT_dt 0.010
koki_konishi 0:a1f453dd2bb5 24
koki_konishi 0:a1f453dd2bb5 25 #define VELO_Ke 1
koki_konishi 0:a1f453dd2bb5 26 #define VELO_Kp 0.005
koki_konishi 0:a1f453dd2bb5 27 #define VELO_Ki 0.000012
koki_konishi 0:a1f453dd2bb5 28 #define VELO_Kd 0.0000
koki_konishi 0:a1f453dd2bb5 29
koki_konishi 0:a1f453dd2bb5 30 #define BELT_Ke 1
koki_konishi 0:a1f453dd2bb5 31 #define BELT_Kp 1.2
koki_konishi 0:a1f453dd2bb5 32 #define BELT_Ki 0.005
koki_konishi 0:a1f453dd2bb5 33 #define BELT_Kd 0.03
koki_konishi 0:a1f453dd2bb5 34
koki_konishi 0:a1f453dd2bb5 35 #define ANG_Ke 4
koki_konishi 0:a1f453dd2bb5 36 #define ANG_Kp 1.2
koki_konishi 0:a1f453dd2bb5 37 #define ANG_Ki 0.005
koki_konishi 0:a1f453dd2bb5 38 #define ANG_Kd 0.03
koki_konishi 0:a1f453dd2bb5 39
koki_konishi 0:a1f453dd2bb5 40 #define LIFT_Ke 2
koki_konishi 0:a1f453dd2bb5 41 #define LIFT_Kp 1.8
koki_konishi 0:a1f453dd2bb5 42 #define LIFT_Ki 0.013
koki_konishi 0:a1f453dd2bb5 43 #define LIFT_Kd 0.03
koki_konishi 0:a1f453dd2bb5 44
koki_konishi 0:a1f453dd2bb5 45 #define ARM_SERVO_OFFSET 7500
koki_konishi 0:a1f453dd2bb5 46 #define ARM_SERVO_VPA 29.6296296
koki_konishi 0:a1f453dd2bb5 47
koki_konishi 0:a1f453dd2bb5 48 //モータードライバのアドレス
koki_konishi 0:a1f453dd2bb5 49 #define beltR_MOTOR 0x02
koki_konishi 0:a1f453dd2bb5 50 #define angR_MOTOR 0x04
koki_konishi 0:a1f453dd2bb5 51
koki_konishi 0:a1f453dd2bb5 52 //モータードライバへの命令
koki_konishi 0:a1f453dd2bb5 53 #define STOP 0
koki_konishi 0:a1f453dd2bb5 54 #define CW 1
koki_konishi 0:a1f453dd2bb5 55 #define CCW -1
koki_konishi 0:a1f453dd2bb5 56 #define bSTOP 10
koki_konishi 0:a1f453dd2bb5 57 #define bCW 11
koki_konishi 0:a1f453dd2bb5 58 #define bCCW -11
koki_konishi 0:a1f453dd2bb5 59
koki_konishi 0:a1f453dd2bb5 60 #define sCW 21
koki_konishi 0:a1f453dd2bb5 61 #define sCCW -21
koki_konishi 0:a1f453dd2bb5 62 #define sSTOP 20
koki_konishi 0:a1f453dd2bb5 63 #define shCW 31
koki_konishi 0:a1f453dd2bb5 64 #define shCCW -31
koki_konishi 0:a1f453dd2bb5 65
koki_konishi 0:a1f453dd2bb5 66 #define lCW 51
koki_konishi 0:a1f453dd2bb5 67 #define lCCW -51
koki_konishi 0:a1f453dd2bb5 68 #define lSTOP 50
koki_konishi 0:a1f453dd2bb5 69 #define lsCW 61
koki_konishi 0:a1f453dd2bb5 70 #define lsCCW -61
koki_konishi 0:a1f453dd2bb5 71 #define lsSTOP 60
koki_konishi 0:a1f453dd2bb5 72
koki_konishi 0:a1f453dd2bb5 73 #define rON 41
koki_konishi 0:a1f453dd2bb5 74 #define rOFF -41
koki_konishi 0:a1f453dd2bb5 75
koki_konishi 0:a1f453dd2bb5 76 //通信初期化
koki_konishi 0:a1f453dd2bb5 77 Serial mecha_ins(USBTX,USBRX,115200);
koki_konishi 0:a1f453dd2bb5 78 I2C md_i2c_tx(PD_13,PD_12);
koki_konishi 0:a1f453dd2bb5 79
koki_konishi 0:a1f453dd2bb5 80 //GPIO初期化
koki_konishi 0:a1f453dd2bb5 81 InterruptIn beltR_i_(PC_1);
koki_konishi 0:a1f453dd2bb5 82
koki_konishi 0:a1f453dd2bb5 83 DigitalIn angR_zero(PE_15);
koki_konishi 0:a1f453dd2bb5 84
koki_konishi 0:a1f453dd2bb5 85 //QEI初期化
koki_konishi 0:a1f453dd2bb5 86 IncEncoder beltR_RE(PC_2,PC_3,IncEncoder::x2_Resolution);
koki_konishi 0:a1f453dd2bb5 87 IncEncoder angR_RE(PF_9,PC_0,IncEncoder::x2_Resolution);
koki_konishi 0:a1f453dd2bb5 88
koki_konishi 0:a1f453dd2bb5 89 //プロトタイプ関数宣言
koki_konishi 0:a1f453dd2bb5 90 void ins_recieve();
koki_konishi 0:a1f453dd2bb5 91 void BELTR_PID();
koki_konishi 0:a1f453dd2bb5 92 void ANGR_PID();
koki_konishi 0:a1f453dd2bb5 93 void belt_reset();
koki_konishi 0:a1f453dd2bb5 94 void beltR_reset();
koki_konishi 0:a1f453dd2bb5 95 void md_setoutput(unsigned char address, int rotate, int duty);
koki_konishi 0:a1f453dd2bb5 96
koki_konishi 0:a1f453dd2bb5 97 volatile char led_command = 0xFF;
koki_konishi 0:a1f453dd2bb5 98
koki_konishi 0:a1f453dd2bb5 99 volatile char mecha_prep = 0;
koki_konishi 0:a1f453dd2bb5 100 volatile int triggerR_flag = 0, triggerL_flag = 0;
koki_konishi 0:a1f453dd2bb5 101
koki_konishi 0:a1f453dd2bb5 102 volatile int lift_pid_flag_R = 1, lift_pid_flag_L = 1;
koki_konishi 0:a1f453dd2bb5 103
koki_konishi 0:a1f453dd2bb5 104 volatile int kick_R = 0, kick_L = 0;
koki_konishi 0:a1f453dd2bb5 105 volatile int pid_off_R = 0, pid_off_L = 0;
koki_konishi 0:a1f453dd2bb5 106
koki_konishi 0:a1f453dd2bb5 107 volatile int beltR_delaycnt = 0, beltL_delaycnt = 0;
koki_konishi 0:a1f453dd2bb5 108 volatile int beltR_delayflag = 1, beltL_delayflag = 1;
koki_konishi 0:a1f453dd2bb5 109
koki_konishi 0:a1f453dd2bb5 110 volatile int delayR_cnt = 0, delayL_cnt = 0;
koki_konishi 0:a1f453dd2bb5 111 volatile int delayR_flag = 1, delayL_flag = 1;
koki_konishi 0:a1f453dd2bb5 112
koki_konishi 0:a1f453dd2bb5 113 volatile int liftR_reset = 0, liftL_reset = 0;
koki_konishi 0:a1f453dd2bb5 114 volatile int stateR_num = 0, stateL_num = 0;
koki_konishi 0:a1f453dd2bb5 115 volatile int cntR = 0, cntL = 0;
koki_konishi 0:a1f453dd2bb5 116
koki_konishi 0:a1f453dd2bb5 117 volatile int BELTR_flag = 0, BELTL_flag = 0;
koki_konishi 0:a1f453dd2bb5 118
koki_konishi 0:a1f453dd2bb5 119 volatile int beltR_POWER = 0;
koki_konishi 0:a1f453dd2bb5 120 volatile int beltL_POWER = 0;
koki_konishi 0:a1f453dd2bb5 121 volatile int beltR_TRIGGER = 0;
koki_konishi 0:a1f453dd2bb5 122 volatile int beltL_TRIGGER = 0;
koki_konishi 0:a1f453dd2bb5 123
koki_konishi 0:a1f453dd2bb5 124 volatile int beltR_UMAX = 100;
koki_konishi 0:a1f453dd2bb5 125 volatile int beltL_UMAX = 100;
koki_konishi 0:a1f453dd2bb5 126
koki_konishi 0:a1f453dd2bb5 127 volatile int beltR_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 128 volatile int beltL_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 129 volatile double beltR_DES = 20;
koki_konishi 0:a1f453dd2bb5 130 volatile double beltL_DES = 20;
koki_konishi 0:a1f453dd2bb5 131 volatile double beltR_STATE = 0;
koki_konishi 0:a1f453dd2bb5 132 volatile double beltL_STATE = 0;
koki_konishi 0:a1f453dd2bb5 133 volatile double beltR_U;
koki_konishi 0:a1f453dd2bb5 134 volatile double beltL_U;
koki_konishi 0:a1f453dd2bb5 135 volatile double beltR_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 136 volatile double beltL_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 137 volatile double beltR_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 138 volatile double beltL_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 139
koki_konishi 0:a1f453dd2bb5 140 volatile int angR_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 141 volatile int angL_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 142 volatile double ANGR_DES = 0;
koki_konishi 0:a1f453dd2bb5 143 volatile double ANGL_DES = 0;
koki_konishi 0:a1f453dd2bb5 144 volatile double ANGR_STATE = 0;
koki_konishi 0:a1f453dd2bb5 145 volatile double ANGL_STATE = 0;
koki_konishi 0:a1f453dd2bb5 146 volatile double ANGR_U;
koki_konishi 0:a1f453dd2bb5 147 volatile double ANGL_U;
koki_konishi 0:a1f453dd2bb5 148 volatile double ANGR_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 149 volatile double ANGL_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 150 volatile double ANGR_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 151 volatile double ANGL_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 152
koki_konishi 0:a1f453dd2bb5 153 volatile int liftR_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 154 volatile int liftL_MOTOR_Rdir;
koki_konishi 0:a1f453dd2bb5 155 volatile double liftR_DES = 0;
koki_konishi 0:a1f453dd2bb5 156 volatile double liftL_DES = 0;
koki_konishi 0:a1f453dd2bb5 157 volatile double liftR_STATE = 0;
koki_konishi 0:a1f453dd2bb5 158 volatile double liftL_STATE = 0;
koki_konishi 0:a1f453dd2bb5 159 volatile double liftR_U;
koki_konishi 0:a1f453dd2bb5 160 volatile double liftL_U;
koki_konishi 0:a1f453dd2bb5 161 volatile double liftR_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 162 volatile double liftL_ERROR[2] = {};
koki_konishi 0:a1f453dd2bb5 163 volatile double liftR_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 164 volatile double liftL_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 165
koki_konishi 0:a1f453dd2bb5 166 //main
koki_konishi 0:a1f453dd2bb5 167 int main(){
koki_konishi 0:a1f453dd2bb5 168 md_i2c_tx.frequency(400000); //MD用I2C
koki_konishi 0:a1f453dd2bb5 169
koki_konishi 0:a1f453dd2bb5 170 beltR_RE.reset();
koki_konishi 0:a1f453dd2bb5 171 angR_RE.reset();
koki_konishi 0:a1f453dd2bb5 172
koki_konishi 0:a1f453dd2bb5 173 angR_zero.mode(PullUp);
koki_konishi 0:a1f453dd2bb5 174
koki_konishi 0:a1f453dd2bb5 175 beltR_i_.mode(PullDown);
koki_konishi 0:a1f453dd2bb5 176
koki_konishi 0:a1f453dd2bb5 177 beltR_i_.rise(&beltR_reset);
koki_konishi 0:a1f453dd2bb5 178
koki_konishi 0:a1f453dd2bb5 179 wait_ms(300);
koki_konishi 0:a1f453dd2bb5 180
koki_konishi 0:a1f453dd2bb5 181 while(!angR_zero){
koki_konishi 0:a1f453dd2bb5 182 if(!angR_zero){
koki_konishi 0:a1f453dd2bb5 183 md_setoutput(angR_MOTOR,bCCW,50);
koki_konishi 0:a1f453dd2bb5 184 }else{
koki_konishi 0:a1f453dd2bb5 185 md_setoutput(angR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 186 }
koki_konishi 0:a1f453dd2bb5 187 }
koki_konishi 0:a1f453dd2bb5 188
koki_konishi 0:a1f453dd2bb5 189 angR_RE.reset();
koki_konishi 0:a1f453dd2bb5 190
koki_konishi 0:a1f453dd2bb5 191 belt_reset();
koki_konishi 0:a1f453dd2bb5 192
koki_konishi 0:a1f453dd2bb5 193 while(mecha_ins.readable()){
koki_konishi 0:a1f453dd2bb5 194 char dummy = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 195 }
koki_konishi 0:a1f453dd2bb5 196
koki_konishi 0:a1f453dd2bb5 197 mecha_ins.attach(&ins_recieve, Serial::RxIrq);
koki_konishi 0:a1f453dd2bb5 198
koki_konishi 0:a1f453dd2bb5 199 while(1){
koki_konishi 0:a1f453dd2bb5 200 if(1){
koki_konishi 0:a1f453dd2bb5 201 //追従処理呼び出し なぜかTickerが新マザーで正常動作しないため\(^o^)/
koki_konishi 0:a1f453dd2bb5 202 if(!beltR_TRIGGER){
koki_konishi 0:a1f453dd2bb5 203 BELTR_PID();
koki_konishi 0:a1f453dd2bb5 204 }
koki_konishi 0:a1f453dd2bb5 205
koki_konishi 0:a1f453dd2bb5 206 ANGR_PID();
koki_konishi 0:a1f453dd2bb5 207
koki_konishi 0:a1f453dd2bb5 208 //発射処理
koki_konishi 0:a1f453dd2bb5 209 if(beltR_TRIGGER == 1){
koki_konishi 0:a1f453dd2bb5 210
koki_konishi 0:a1f453dd2bb5 211 beltR_TRIGGER++;
koki_konishi 0:a1f453dd2bb5 212 }
koki_konishi 0:a1f453dd2bb5 213
koki_konishi 0:a1f453dd2bb5 214 if(beltR_TRIGGER == 2){
koki_konishi 0:a1f453dd2bb5 215 if(pid_off_R){
koki_konishi 0:a1f453dd2bb5 216 md_setoutput(beltR_MOTOR,CW,beltR_POWER);
koki_konishi 0:a1f453dd2bb5 217 }else{
koki_konishi 0:a1f453dd2bb5 218 md_setoutput(beltR_MOTOR,sCW,beltR_POWER);
koki_konishi 0:a1f453dd2bb5 219 }
koki_konishi 0:a1f453dd2bb5 220
koki_konishi 0:a1f453dd2bb5 221 triggerR_flag = 0;
koki_konishi 0:a1f453dd2bb5 222 }
koki_konishi 0:a1f453dd2bb5 223
koki_konishi 0:a1f453dd2bb5 224 if(kick_R){
koki_konishi 0:a1f453dd2bb5 225 if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 2.0 && beltR_TRIGGER == 2){
koki_konishi 0:a1f453dd2bb5 226 pid_off_R = 0;
koki_konishi 0:a1f453dd2bb5 227 beltR_delaycnt = 50;
koki_konishi 0:a1f453dd2bb5 228 beltR_delayflag = 0;
koki_konishi 0:a1f453dd2bb5 229 beltR_TRIGGER++;
koki_konishi 0:a1f453dd2bb5 230 }
koki_konishi 0:a1f453dd2bb5 231 }else{
koki_konishi 0:a1f453dd2bb5 232 if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 0.5 && beltR_TRIGGER == 2){
koki_konishi 0:a1f453dd2bb5 233 pid_off_R = 0;
koki_konishi 0:a1f453dd2bb5 234 beltR_delaycnt = 50;
koki_konishi 0:a1f453dd2bb5 235 beltR_delayflag = 0;
koki_konishi 0:a1f453dd2bb5 236 beltR_TRIGGER++;
koki_konishi 0:a1f453dd2bb5 237 }
koki_konishi 0:a1f453dd2bb5 238 }
koki_konishi 0:a1f453dd2bb5 239
koki_konishi 0:a1f453dd2bb5 240 if(beltR_TRIGGER == 3){
koki_konishi 0:a1f453dd2bb5 241 kick_R = 0;
koki_konishi 0:a1f453dd2bb5 242 md_setoutput(beltR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 243 }
koki_konishi 0:a1f453dd2bb5 244
koki_konishi 0:a1f453dd2bb5 245 if(beltR_delayflag && beltR_TRIGGER == 3){
koki_konishi 0:a1f453dd2bb5 246 beltR_TRIGGER++;
koki_konishi 0:a1f453dd2bb5 247 }
koki_konishi 0:a1f453dd2bb5 248
koki_konishi 0:a1f453dd2bb5 249 if(beltR_TRIGGER == 4){
koki_konishi 0:a1f453dd2bb5 250 md_setoutput(beltR_MOTOR,bCW,70);
koki_konishi 0:a1f453dd2bb5 251 }
koki_konishi 0:a1f453dd2bb5 252
koki_konishi 0:a1f453dd2bb5 253 if(beltR_RE.GetIncPulses()/(BELT_RPP*2) >= 5 && beltR_TRIGGER == 4){
koki_konishi 0:a1f453dd2bb5 254 BELTR_flag = 1;
koki_konishi 0:a1f453dd2bb5 255 beltR_TRIGGER++;
koki_konishi 0:a1f453dd2bb5 256 }
koki_konishi 0:a1f453dd2bb5 257
koki_konishi 0:a1f453dd2bb5 258 if(beltR_TRIGGER == 5){
koki_konishi 0:a1f453dd2bb5 259 md_setoutput(beltR_MOTOR,bCCW,70);
koki_konishi 0:a1f453dd2bb5 260 }
koki_konishi 0:a1f453dd2bb5 261
koki_konishi 0:a1f453dd2bb5 262 if(!BELTR_flag && beltR_TRIGGER == 5){
koki_konishi 0:a1f453dd2bb5 263 md_setoutput(beltR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 264 beltR_INTEGRAL = 0;
koki_konishi 0:a1f453dd2bb5 265 beltR_TRIGGER = 0;
koki_konishi 0:a1f453dd2bb5 266 }
koki_konishi 0:a1f453dd2bb5 267 }
koki_konishi 0:a1f453dd2bb5 268
koki_konishi 0:a1f453dd2bb5 269 mecha_prep = 0x00;
koki_konishi 0:a1f453dd2bb5 270
koki_konishi 0:a1f453dd2bb5 271 if(triggerR_flag){
koki_konishi 0:a1f453dd2bb5 272 mecha_prep = 0x80;
koki_konishi 0:a1f453dd2bb5 273 }
koki_konishi 0:a1f453dd2bb5 274 if(triggerL_flag){
koki_konishi 0:a1f453dd2bb5 275 mecha_prep = 0x40;
koki_konishi 0:a1f453dd2bb5 276 }
koki_konishi 0:a1f453dd2bb5 277 if(beltR_TRIGGER){
koki_konishi 0:a1f453dd2bb5 278 mecha_prep = 0x20;
koki_konishi 0:a1f453dd2bb5 279 }
koki_konishi 0:a1f453dd2bb5 280 if(beltL_TRIGGER){
koki_konishi 0:a1f453dd2bb5 281 mecha_prep = 0x10;
koki_konishi 0:a1f453dd2bb5 282 }
koki_konishi 0:a1f453dd2bb5 283 if(stateR_num){
koki_konishi 0:a1f453dd2bb5 284 mecha_prep = 0x08;
koki_konishi 0:a1f453dd2bb5 285 }
koki_konishi 0:a1f453dd2bb5 286 if(stateL_num){
koki_konishi 0:a1f453dd2bb5 287 mecha_prep = 0x04;
koki_konishi 0:a1f453dd2bb5 288 }
koki_konishi 0:a1f453dd2bb5 289 if(liftR_reset || liftL_reset){
koki_konishi 0:a1f453dd2bb5 290 mecha_prep = 0x02;
koki_konishi 0:a1f453dd2bb5 291 }
koki_konishi 0:a1f453dd2bb5 292
koki_konishi 0:a1f453dd2bb5 293 mecha_ins.putc(mecha_prep);
koki_konishi 0:a1f453dd2bb5 294
koki_konishi 0:a1f453dd2bb5 295 wait_ms(10);
koki_konishi 0:a1f453dd2bb5 296 }
koki_konishi 0:a1f453dd2bb5 297 }
koki_konishi 0:a1f453dd2bb5 298
koki_konishi 0:a1f453dd2bb5 299 //右発射角追従制御
koki_konishi 0:a1f453dd2bb5 300 void ANGR_PID(){
koki_konishi 0:a1f453dd2bb5 301 ANGR_STATE = 360 * -angR_RE.GetIncPulses()/(ANG_RPP*2);
koki_konishi 0:a1f453dd2bb5 302
koki_konishi 0:a1f453dd2bb5 303 ANGR_ERROR[0] = ANGR_ERROR[1];
koki_konishi 0:a1f453dd2bb5 304 ANGR_ERROR[1] = (double)ANG_Ke * (ANGR_STATE - ANGR_DES);
koki_konishi 0:a1f453dd2bb5 305 ANGR_INTEGRAL += ANGR_ERROR[1]*ANG_dt;
koki_konishi 0:a1f453dd2bb5 306
koki_konishi 0:a1f453dd2bb5 307 ANGR_U = ANG_Kp * ANGR_ERROR[1] + ANG_Ki * ANGR_INTEGRAL + ANG_Kd * (ANGR_ERROR[1] - ANGR_ERROR[0])/ANG_dt;
koki_konishi 0:a1f453dd2bb5 308
koki_konishi 0:a1f453dd2bb5 309 if(ANGR_U >= 0){
koki_konishi 0:a1f453dd2bb5 310 angR_MOTOR_Rdir = bCCW;
koki_konishi 0:a1f453dd2bb5 311 }else{
koki_konishi 0:a1f453dd2bb5 312 angR_MOTOR_Rdir = bCW;
koki_konishi 0:a1f453dd2bb5 313 ANGR_U *= -1;
koki_konishi 0:a1f453dd2bb5 314 }
koki_konishi 0:a1f453dd2bb5 315
koki_konishi 0:a1f453dd2bb5 316 if(ANGR_U >= 100){
koki_konishi 0:a1f453dd2bb5 317 ANGR_U = 100;
koki_konishi 0:a1f453dd2bb5 318 }
koki_konishi 0:a1f453dd2bb5 319
koki_konishi 0:a1f453dd2bb5 320 if(angR_zero && angR_MOTOR_Rdir == bCCW){
koki_konishi 0:a1f453dd2bb5 321 md_setoutput(angR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 322 }else{
koki_konishi 0:a1f453dd2bb5 323 md_setoutput(angR_MOTOR,angR_MOTOR_Rdir,ANGR_U+ANG_UC);
koki_konishi 0:a1f453dd2bb5 324 }
koki_konishi 0:a1f453dd2bb5 325 }
koki_konishi 0:a1f453dd2bb5 326
koki_konishi 0:a1f453dd2bb5 327 //右ベルト位置追従制御
koki_konishi 0:a1f453dd2bb5 328 void BELTR_PID(){
koki_konishi 0:a1f453dd2bb5 329 beltR_STATE = BELT_PER * beltR_RE.GetIncPulses()/(BELT_RPP*2*BELT_Rratio);
koki_konishi 0:a1f453dd2bb5 330
koki_konishi 0:a1f453dd2bb5 331 beltR_ERROR[0] = beltR_ERROR[1];
koki_konishi 0:a1f453dd2bb5 332 beltR_ERROR[1] = (double)BELT_Ke * (beltR_STATE - beltR_DES);
koki_konishi 0:a1f453dd2bb5 333 beltR_INTEGRAL += beltR_ERROR[1]*BELT_dt;
koki_konishi 0:a1f453dd2bb5 334
koki_konishi 0:a1f453dd2bb5 335 beltR_U = BELT_Kp * beltR_ERROR[1] + BELT_Ki * beltR_INTEGRAL + BELT_Kd * (beltR_ERROR[1] - beltR_ERROR[0])/BELT_dt;
koki_konishi 0:a1f453dd2bb5 336
koki_konishi 0:a1f453dd2bb5 337 if(beltR_U >= 0){
koki_konishi 0:a1f453dd2bb5 338 beltR_MOTOR_Rdir = bCCW;
koki_konishi 0:a1f453dd2bb5 339 }else{
koki_konishi 0:a1f453dd2bb5 340 beltR_MOTOR_Rdir = bCW;
koki_konishi 0:a1f453dd2bb5 341 beltR_U *= -1;
koki_konishi 0:a1f453dd2bb5 342 }
koki_konishi 0:a1f453dd2bb5 343
koki_konishi 0:a1f453dd2bb5 344 if(beltR_U >= beltR_UMAX){
koki_konishi 0:a1f453dd2bb5 345 beltR_U = beltR_UMAX;
koki_konishi 0:a1f453dd2bb5 346 }
koki_konishi 0:a1f453dd2bb5 347
koki_konishi 0:a1f453dd2bb5 348 md_setoutput(beltR_MOTOR,beltR_MOTOR_Rdir,beltR_U+BELT_UC);
koki_konishi 0:a1f453dd2bb5 349 }
koki_konishi 0:a1f453dd2bb5 350
koki_konishi 0:a1f453dd2bb5 351 void belt_reset(){
koki_konishi 0:a1f453dd2bb5 352 int beltR_flag = 1;
koki_konishi 0:a1f453dd2bb5 353
koki_konishi 0:a1f453dd2bb5 354 while(beltR_flag){
koki_konishi 0:a1f453dd2bb5 355 if(360 * beltR_RE.GetIncPulses()/(BELT_RPP*2) < 180){
koki_konishi 0:a1f453dd2bb5 356 md_setoutput(beltR_MOTOR,bCW,70);
koki_konishi 0:a1f453dd2bb5 357 }else{
koki_konishi 0:a1f453dd2bb5 358 md_setoutput(beltR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 359 beltR_flag = 0;
koki_konishi 0:a1f453dd2bb5 360 }
koki_konishi 0:a1f453dd2bb5 361 }
koki_konishi 0:a1f453dd2bb5 362
koki_konishi 0:a1f453dd2bb5 363 BELTR_flag = 1;
koki_konishi 0:a1f453dd2bb5 364
koki_konishi 0:a1f453dd2bb5 365 while(BELTR_flag){
koki_konishi 0:a1f453dd2bb5 366 if(BELTR_flag){
koki_konishi 0:a1f453dd2bb5 367 md_setoutput(beltR_MOTOR,bCCW,70);
koki_konishi 0:a1f453dd2bb5 368 }else{
koki_konishi 0:a1f453dd2bb5 369 md_setoutput(beltR_MOTOR,bSTOP,0);
koki_konishi 0:a1f453dd2bb5 370 }
koki_konishi 0:a1f453dd2bb5 371 }
koki_konishi 0:a1f453dd2bb5 372
koki_konishi 0:a1f453dd2bb5 373 wait_ms(10);
koki_konishi 0:a1f453dd2bb5 374 }
koki_konishi 0:a1f453dd2bb5 375
koki_konishi 0:a1f453dd2bb5 376 //命令受信処理
koki_konishi 0:a1f453dd2bb5 377 void ins_recieve(){
koki_konishi 0:a1f453dd2bb5 378 char ins_byte = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 379
koki_konishi 0:a1f453dd2bb5 380 switch(ins_byte){
koki_konishi 0:a1f453dd2bb5 381 case 0x0F :
koki_konishi 0:a1f453dd2bb5 382 cntR = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 383 cntL = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 384 liftR_reset = 1;
koki_konishi 0:a1f453dd2bb5 385 liftL_reset = 1;
koki_konishi 0:a1f453dd2bb5 386 break;
koki_konishi 0:a1f453dd2bb5 387 case 0xA1 :
koki_konishi 0:a1f453dd2bb5 388 beltR_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 389 beltR_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 390 break;
koki_konishi 0:a1f453dd2bb5 391 case 0xA2 :
koki_konishi 0:a1f453dd2bb5 392 cntR = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 393 stateR_num = 1;
koki_konishi 0:a1f453dd2bb5 394 break;
koki_konishi 0:a1f453dd2bb5 395 case 0xA3 :
koki_konishi 0:a1f453dd2bb5 396 ANGR_DES = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 397 break;
koki_konishi 0:a1f453dd2bb5 398 case 0xB1 :
koki_konishi 0:a1f453dd2bb5 399 beltL_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 400 beltL_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 401 break;
koki_konishi 0:a1f453dd2bb5 402 case 0xB2 :
koki_konishi 0:a1f453dd2bb5 403 cntL = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 404 stateL_num = 1;
koki_konishi 0:a1f453dd2bb5 405 break;
koki_konishi 0:a1f453dd2bb5 406 case 0xB3 :
koki_konishi 0:a1f453dd2bb5 407 ANGL_DES = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 408 break;
koki_konishi 0:a1f453dd2bb5 409 case 0xAA :
koki_konishi 0:a1f453dd2bb5 410 beltR_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 411 beltR_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 412 pid_off_R = 1;
koki_konishi 0:a1f453dd2bb5 413 kick_R = 1;
koki_konishi 0:a1f453dd2bb5 414 break;
koki_konishi 0:a1f453dd2bb5 415 case 0xAB :
koki_konishi 0:a1f453dd2bb5 416 beltR_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 417 beltR_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 418 kick_R = 1;
koki_konishi 0:a1f453dd2bb5 419 break;
koki_konishi 0:a1f453dd2bb5 420 case 0xBA :
koki_konishi 0:a1f453dd2bb5 421 beltL_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 422 beltL_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 423 pid_off_L = 1;
koki_konishi 0:a1f453dd2bb5 424 kick_L = 1;
koki_konishi 0:a1f453dd2bb5 425 break;
koki_konishi 0:a1f453dd2bb5 426 case 0xBB :
koki_konishi 0:a1f453dd2bb5 427 beltL_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 428 beltL_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 429 kick_L = 1;
koki_konishi 0:a1f453dd2bb5 430 break;
koki_konishi 0:a1f453dd2bb5 431 case 0xAF :
koki_konishi 0:a1f453dd2bb5 432 beltR_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 433 beltR_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 434 pid_off_R = 1;
koki_konishi 0:a1f453dd2bb5 435 break;
koki_konishi 0:a1f453dd2bb5 436 case 0xBF :
koki_konishi 0:a1f453dd2bb5 437 beltL_POWER = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 438 beltL_TRIGGER = 1;
koki_konishi 0:a1f453dd2bb5 439 pid_off_L = 1;
koki_konishi 0:a1f453dd2bb5 440 break;
koki_konishi 0:a1f453dd2bb5 441 case 0xFF :
koki_konishi 0:a1f453dd2bb5 442 led_command = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 443 break;
koki_konishi 0:a1f453dd2bb5 444 }
koki_konishi 0:a1f453dd2bb5 445
koki_konishi 0:a1f453dd2bb5 446 while(mecha_ins.readable()){
koki_konishi 0:a1f453dd2bb5 447 char dummy = mecha_ins.getc();
koki_konishi 0:a1f453dd2bb5 448 }
koki_konishi 0:a1f453dd2bb5 449 }
koki_konishi 0:a1f453dd2bb5 450
koki_konishi 0:a1f453dd2bb5 451 void beltR_reset(){
koki_konishi 0:a1f453dd2bb5 452 if(BELTR_flag){
koki_konishi 0:a1f453dd2bb5 453 beltR_RE.reset();
koki_konishi 0:a1f453dd2bb5 454 BELTR_flag = 0;
koki_konishi 0:a1f453dd2bb5 455 }
koki_konishi 0:a1f453dd2bb5 456 }
koki_konishi 0:a1f453dd2bb5 457
koki_konishi 0:a1f453dd2bb5 458 //MD用I2C通信処理
koki_konishi 0:a1f453dd2bb5 459 void md_setoutput(unsigned char address, int rotate, int duty){
koki_konishi 0:a1f453dd2bb5 460 char data[2];
koki_konishi 0:a1f453dd2bb5 461
koki_konishi 0:a1f453dd2bb5 462 // モーター駆動制御モード
koki_konishi 0:a1f453dd2bb5 463 switch (rotate) {
koki_konishi 0:a1f453dd2bb5 464 case CW: data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
koki_konishi 0:a1f453dd2bb5 465 case CCW: data[0] = 'R'; break;
koki_konishi 0:a1f453dd2bb5 466 case STOP: data[0] = 'S'; break;
koki_konishi 0:a1f453dd2bb5 467
koki_konishi 0:a1f453dd2bb5 468 case bCW: data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
koki_konishi 0:a1f453dd2bb5 469 case bCCW: data[0] = 'r'; break;
koki_konishi 0:a1f453dd2bb5 470 case bSTOP: data[0] = 's'; break;
koki_konishi 0:a1f453dd2bb5 471
koki_konishi 0:a1f453dd2bb5 472 case sCW: data[0] = 'A'; break; // 速度比例制御モード
koki_konishi 0:a1f453dd2bb5 473 case sCCW: data[0] = 'B'; break;
koki_konishi 0:a1f453dd2bb5 474 case sSTOP: data[0] = 'T'; break;
koki_konishi 0:a1f453dd2bb5 475
koki_konishi 0:a1f453dd2bb5 476 case shCW: data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
koki_konishi 0:a1f453dd2bb5 477 case shCCW: data[0] = 'b'; break;
koki_konishi 0:a1f453dd2bb5 478
koki_konishi 0:a1f453dd2bb5 479 case lCW: data[0] = 'L'; break; // 通常LAP
koki_konishi 0:a1f453dd2bb5 480 case lCCW: data[0] = 'M'; break;
koki_konishi 0:a1f453dd2bb5 481 case lSTOP: data[0] = 'N'; break;
koki_konishi 0:a1f453dd2bb5 482 case lsCW: data[0] = 'l'; break; // 速調LAP
koki_konishi 0:a1f453dd2bb5 483 case lsCCW: data[0] = 'm'; break;
koki_konishi 0:a1f453dd2bb5 484 case lsSTOP: data[0] = 'n'; break;
koki_konishi 0:a1f453dd2bb5 485
koki_konishi 0:a1f453dd2bb5 486 case rON: data[0] = 'P'; break; //リレー駆動モード
koki_konishi 0:a1f453dd2bb5 487 case rOFF: data[0] = 'p'; break;
koki_konishi 0:a1f453dd2bb5 488 }
koki_konishi 0:a1f453dd2bb5 489
koki_konishi 0:a1f453dd2bb5 490 data[1] = duty & 0xFF;
koki_konishi 0:a1f453dd2bb5 491 md_i2c_tx.write(address << 1, data, 2); //duty送信
koki_konishi 0:a1f453dd2bb5 492 }