Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

foot.cpp@0:3dc012104243, 2019-09-24 (annotated)

Committer:
kazuryu
Date:
Tue Sep 24 13:35:31 2019 +0000
Revision:
0:3dc012104243
Child:
4:9ba47e5db1e2
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazuryu 0:3dc012104243 1 #include "mbed.h"
kazuryu 0:3dc012104243 2 #include "foot.h"
kazuryu 0:3dc012104243 3 /*
kazuryu 0:3dc012104243 4 PwmOut RF(D3);
kazuryu 0:3dc012104243 5 PwmOut LF(D5);
kazuryu 0:3dc012104243 6 PwmOut RB(D6);
kazuryu 0:3dc012104243 7 PwmOut LB(D9);
kazuryu 0:3dc012104243 8 PwmOut pwms[4] = {RF,LF,RB,LB};
kazuryu 0:3dc012104243 9 */
kazuryu 0:3dc012104243 10 //
kazuryu 0:3dc012104243 11 DigitalOut dig(PG_9);
kazuryu 0:3dc012104243 12 PwmOut RFA(PB_4);//3/1
kazuryu 0:3dc012104243 13 PwmOut RFB(PB_5);//3/2
kazuryu 0:3dc012104243 14 PwmOut LFA(PC_8);//3/3
kazuryu 0:3dc012104243 15 PwmOut LFB(PC_9);//3/4
kazuryu 0:3dc012104243 16 PwmOut RBA(PD_12);//4/1
kazuryu 0:3dc012104243 17 PwmOut RBB(PD_13);//4/2
kazuryu 0:3dc012104243 18 PwmOut LBA(PD_14);//4/3
kazuryu 0:3dc012104243 19 PwmOut LBB(PD_15);//4/4
kazuryu 0:3dc012104243 20 PwmOut pwms[8] = {RFA,RFB,LFA,LFB,RBA,RBB,LBA,LBB};
kazuryu 0:3dc012104243 21 PwmOut lockR(PB_10);
kazuryu 0:3dc012104243 22 PwmOut lockL(PB_11);
kazuryu 0:3dc012104243 23 PwmOut lockpwms[2] = {lockR,lockL};
kazuryu 0:3dc012104243 24 //new
kazuryu 0:3dc012104243 25 //PB_6 -> PD_12
kazuryu 0:3dc012104243 26 //PC_7 -> PB_5
kazuryu 0:3dc012104243 27
kazuryu 0:3dc012104243 28 /*
kazuryu 0:3dc012104243 29 // PB_4 -> PA_6/////
kazuryu 0:3dc012104243 30 // PB_6 -> PD_12
kazuryu 0:3dc012104243 31 */
kazuryu 0:3dc012104243 32 /*
kazuryu 0:3dc012104243 33 DigitalOut dig(PG_9);
kazuryu 0:3dc012104243 34 PwmOut RFA(PB_4);//3/1
kazuryu 0:3dc012104243 35 PwmOut RFB(PC_7);//3/2
kazuryu 0:3dc012104243 36 PwmOut LFA(PC_8);//3/3
kazuryu 0:3dc012104243 37 PwmOut LFB(PC_9);//3/4
kazuryu 0:3dc012104243 38 PwmOut RBA(PB_6);//4/1
kazuryu 0:3dc012104243 39 PwmOut RBB(PD_13);//4/2
kazuryu 0:3dc012104243 40 PwmOut LBA(PD_14);//4/3
kazuryu 0:3dc012104243 41 PwmOut LBB(PD_15);//4/4
kazuryu 0:3dc012104243 42 */
kazuryu 0:3dc012104243 43 int Write[][4] {
kazuryu 0:3dc012104243 44 {255, 255, 255, 255}, //0前
kazuryu 0:3dc012104243 45 { -255, -255, -255, -255}, //1後ろ
kazuryu 0:3dc012104243 46 { -255, 255, 255, -255}, //2右
kazuryu 0:3dc012104243 47 {255, -255, -255, 255}, //3左
kazuryu 0:3dc012104243 48 {0, 255, 255, 0}, //4右前
kazuryu 0:3dc012104243 49 {255, 0, 0, 255}, //5左前
kazuryu 0:3dc012104243 50 { -255, 0, 0, -255}, //6右後ろ
kazuryu 0:3dc012104243 51 {0, -255, -255, 0}, //7左後ろ
kazuryu 0:3dc012104243 52 { -255, 255, -255, 255}, //8右旋回
kazuryu 0:3dc012104243 53 {255, -255, 255, -255},//9左旋回
kazuryu 0:3dc012104243 54 {500,500,500,500}
kazuryu 0:3dc012104243 55 };
kazuryu 0:3dc012104243 56 float pwm_g[] = {1.1052f,1,1.0421f,1.0526f};
kazuryu 0:3dc012104243 57
kazuryu 0:3dc012104243 58 void Move(int way,float pwm){
kazuryu 0:3dc012104243 59 int num;
kazuryu 0:3dc012104243 60 for(int i =0;i<4;i++){
kazuryu 0:3dc012104243 61 num = Write[way][i];
kazuryu 0:3dc012104243 62 if(num > 0)dig = 0;
kazuryu 0:3dc012104243 63 else if(num < 0)dig = 1;
kazuryu 0:3dc012104243 64 if(num != 0)pwms[i] = pwm;
kazuryu 0:3dc012104243 65 else pwms[i] = 0;
kazuryu 0:3dc012104243 66 }
kazuryu 0:3dc012104243 67 }
kazuryu 0:3dc012104243 68 int count_Move = 0;
kazuryu 0:3dc012104243 69 int count_Move_lock = 0;
kazuryu 0:3dc012104243 70 void Move2(int way,float pwm){
kazuryu 0:3dc012104243 71 int num;
kazuryu 0:3dc012104243 72 if(count_Move == 0){
kazuryu 0:3dc012104243 73 for(int i =0;i<8;i++){
kazuryu 0:3dc012104243 74 pwms[i].period_ms(1);
kazuryu 0:3dc012104243 75 }
kazuryu 0:3dc012104243 76 count_Move = 1;
kazuryu 0:3dc012104243 77 }
kazuryu 0:3dc012104243 78 for(int i =0;i<4;i++){
kazuryu 0:3dc012104243 79 num = Write[way][i];
kazuryu 0:3dc012104243 80 if(num == 255){
kazuryu 0:3dc012104243 81 pwms[i*2] = pwm*pwm_g[i];
kazuryu 0:3dc012104243 82 pwms[i*2+1] = 0;
kazuryu 0:3dc012104243 83 }else if(num == -255){
kazuryu 0:3dc012104243 84 pwms[i*2] = 0;
kazuryu 0:3dc012104243 85 pwms[i*2+1] = pwm*pwm_g[i];
kazuryu 0:3dc012104243 86 }else if(num == 500){
kazuryu 0:3dc012104243 87 pwms[i*2] = 1;
kazuryu 0:3dc012104243 88 pwms[i*2+1] = 1;
kazuryu 0:3dc012104243 89 }
kazuryu 0:3dc012104243 90 }
kazuryu 0:3dc012104243 91 }
kazuryu 0:3dc012104243 92
kazuryu 0:3dc012104243 93 void Move_lock(int way,float pwm){
kazuryu 0:3dc012104243 94 int num;
kazuryu 0:3dc012104243 95 if(count_Move_lock == 0){
kazuryu 0:3dc012104243 96 for(int i =0;i<2;i++){
kazuryu 0:3dc012104243 97 lockpwms[i].period_ms(1);
kazuryu 0:3dc012104243 98 }
kazuryu 0:3dc012104243 99 count_Move_lock = 1;
kazuryu 0:3dc012104243 100 }
kazuryu 0:3dc012104243 101 for(int i =0;i<1;i++){
kazuryu 0:3dc012104243 102 num = Write[way][i];
kazuryu 0:3dc012104243 103 if(num == 255){
kazuryu 0:3dc012104243 104 pwms[i*2] = pwm;
kazuryu 0:3dc012104243 105 pwms[i*2+1] = 0;
kazuryu 0:3dc012104243 106 }else if(num == -255){
kazuryu 0:3dc012104243 107 pwms[i*2] = 0;
kazuryu 0:3dc012104243 108 pwms[i*2+1] = pwm;
kazuryu 0:3dc012104243 109 }
kazuryu 0:3dc012104243 110 }
kazuryu 0:3dc012104243 111 }
kazuryu 0:3dc012104243 112
kazuryu 0:3dc012104243 113 void trans(int a,int dis,float *x,float *y,int *sign){
kazuryu 0:3dc012104243 114 if(a == 0){
kazuryu 0:3dc012104243 115 *y=dis;
kazuryu 0:3dc012104243 116 *x = 0;
kazuryu 0:3dc012104243 117 }else if(a == 1){
kazuryu 0:3dc012104243 118 *y = -dis;
kazuryu 0:3dc012104243 119 *x = 0;
kazuryu 0:3dc012104243 120 }else if(a == 2){
kazuryu 0:3dc012104243 121 *y = 0;
kazuryu 0:3dc012104243 122 *x = dis;
kazuryu 0:3dc012104243 123 }else if(a == 3){
kazuryu 0:3dc012104243 124 *y = 0;
kazuryu 0:3dc012104243 125 *x = -dis;
kazuryu 0:3dc012104243 126 }else if(a == 4){
kazuryu 0:3dc012104243 127 *y = dis/1.414;
kazuryu 0:3dc012104243 128 *x = dis/1.414;
kazuryu 0:3dc012104243 129 }else if(a == 5){
kazuryu 0:3dc012104243 130 *y = dis/1.414;
kazuryu 0:3dc012104243 131 *x = -dis/1.414;
kazuryu 0:3dc012104243 132 }else if(a == 6){
kazuryu 0:3dc012104243 133 *y = -dis/1.414;
kazuryu 0:3dc012104243 134 *x = -dis/1.414;
kazuryu 0:3dc012104243 135 }else if(a == 7){
kazuryu 0:3dc012104243 136 *y = -dis/1.414;
kazuryu 0:3dc012104243 137 *x = dis/1.414;
kazuryu 0:3dc012104243 138 }
kazuryu 0:3dc012104243 139 *sign = 0;
kazuryu 0:3dc012104243 140 if(*x < 0)*sign = 0b10;
kazuryu 0:3dc012104243 141 if(*y < 0)*sign = *sign | 0b01;
kazuryu 0:3dc012104243 142 }