Ryu Kaz / Mbed OS 767zi

t6est

Dependencies:   Pulse

foot.cpp@4:9ba47e5db1e2, 2019-10-05 (annotated)

Committer:
kazuryu
Date:
Sat Oct 05 01:41:37 2019 +0000
Revision:
4:9ba47e5db1e2
Parent:
0:3dc012104243 
B

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 4:9ba47e5db1e2 23
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 4:9ba47e5db1e2 56 //1.1052f,1,1.0421f,1.0726f
kazuryu 4:9ba47e5db1e2 57 float pwm_g[] = {1,0.904f,0.982f,0.970f};
kazuryu 4:9ba47e5db1e2 58 //1,0.904,0.942,0.970
kazuryu 0:3dc012104243 59 void Move(int way,float pwm){
kazuryu 0:3dc012104243 60 int num;
kazuryu 0:3dc012104243 61 for(int i =0;i<4;i++){
kazuryu 0:3dc012104243 62 num = Write[way][i];
kazuryu 0:3dc012104243 63 if(num > 0)dig = 0;
kazuryu 0:3dc012104243 64 else if(num < 0)dig = 1;
kazuryu 0:3dc012104243 65 if(num != 0)pwms[i] = pwm;
kazuryu 0:3dc012104243 66 else pwms[i] = 0;
kazuryu 0:3dc012104243 67 }
kazuryu 0:3dc012104243 68 }
kazuryu 0:3dc012104243 69 int count_Move = 0;
kazuryu 0:3dc012104243 70 int count_Move_lock = 0;
kazuryu 0:3dc012104243 71 void Move2(int way,float pwm){
kazuryu 0:3dc012104243 72 int num;
kazuryu 0:3dc012104243 73 if(count_Move == 0){
kazuryu 0:3dc012104243 74 for(int i =0;i<8;i++){
kazuryu 0:3dc012104243 75 pwms[i].period_ms(1);
kazuryu 0:3dc012104243 76 }
kazuryu 0:3dc012104243 77 count_Move = 1;
kazuryu 0:3dc012104243 78 }
kazuryu 0:3dc012104243 79 for(int i =0;i<4;i++){
kazuryu 0:3dc012104243 80 num = Write[way][i];
kazuryu 0:3dc012104243 81 if(num == 255){
kazuryu 0:3dc012104243 82 pwms[i*2] = pwm*pwm_g[i];
kazuryu 0:3dc012104243 83 pwms[i*2+1] = 0;
kazuryu 0:3dc012104243 84 }else if(num == -255){
kazuryu 0:3dc012104243 85 pwms[i*2] = 0;
kazuryu 0:3dc012104243 86 pwms[i*2+1] = pwm*pwm_g[i];
kazuryu 0:3dc012104243 87 }else if(num == 500){
kazuryu 4:9ba47e5db1e2 88 pwms[i*2] = 0;
kazuryu 4:9ba47e5db1e2 89 pwms[i*2+1] = 0;
kazuryu 0:3dc012104243 90 }
kazuryu 0:3dc012104243 91 }
kazuryu 0:3dc012104243 92 }
kazuryu 4:9ba47e5db1e2 93 /*
kazuryu 0:3dc012104243 94 void Move_lock(int way,float pwm){
kazuryu 0:3dc012104243 95 int num;
kazuryu 0:3dc012104243 96 if(count_Move_lock == 0){
kazuryu 0:3dc012104243 97 for(int i =0;i<2;i++){
kazuryu 0:3dc012104243 98 lockpwms[i].period_ms(1);
kazuryu 0:3dc012104243 99 }
kazuryu 0:3dc012104243 100 count_Move_lock = 1;
kazuryu 0:3dc012104243 101 }
kazuryu 0:3dc012104243 102 for(int i =0;i<1;i++){
kazuryu 0:3dc012104243 103 num = Write[way][i];
kazuryu 0:3dc012104243 104 if(num == 255){
kazuryu 0:3dc012104243 105 pwms[i*2] = pwm;
kazuryu 0:3dc012104243 106 pwms[i*2+1] = 0;
kazuryu 0:3dc012104243 107 }else if(num == -255){
kazuryu 0:3dc012104243 108 pwms[i*2] = 0;
kazuryu 0:3dc012104243 109 pwms[i*2+1] = pwm;
kazuryu 0:3dc012104243 110 }
kazuryu 0:3dc012104243 111 }
kazuryu 0:3dc012104243 112 }
kazuryu 4:9ba47e5db1e2 113 */
kazuryu 0:3dc012104243 114 void trans(int a,int dis,float *x,float *y,int *sign){
kazuryu 0:3dc012104243 115 if(a == 0){
kazuryu 0:3dc012104243 116 *y=dis;
kazuryu 0:3dc012104243 117 *x = 0;
kazuryu 0:3dc012104243 118 }else if(a == 1){
kazuryu 0:3dc012104243 119 *y = -dis;
kazuryu 0:3dc012104243 120 *x = 0;
kazuryu 0:3dc012104243 121 }else if(a == 2){
kazuryu 0:3dc012104243 122 *y = 0;
kazuryu 0:3dc012104243 123 *x = dis;
kazuryu 0:3dc012104243 124 }else if(a == 3){
kazuryu 0:3dc012104243 125 *y = 0;
kazuryu 0:3dc012104243 126 *x = -dis;
kazuryu 0:3dc012104243 127 }else if(a == 4){
kazuryu 0:3dc012104243 128 *y = dis/1.414;
kazuryu 0:3dc012104243 129 *x = dis/1.414;
kazuryu 0:3dc012104243 130 }else if(a == 5){
kazuryu 0:3dc012104243 131 *y = dis/1.414;
kazuryu 0:3dc012104243 132 *x = -dis/1.414;
kazuryu 0:3dc012104243 133 }else if(a == 6){
kazuryu 0:3dc012104243 134 *y = -dis/1.414;
kazuryu 0:3dc012104243 135 *x = -dis/1.414;
kazuryu 0:3dc012104243 136 }else if(a == 7){
kazuryu 0:3dc012104243 137 *y = -dis/1.414;
kazuryu 0:3dc012104243 138 *x = dis/1.414;
kazuryu 0:3dc012104243 139 }
kazuryu 0:3dc012104243 140 *sign = 0;
kazuryu 0:3dc012104243 141 if(*x < 0)*sign = 0b10;
kazuryu 0:3dc012104243 142 if(*y < 0)*sign = *sign | 0b01;
kazuryu 0:3dc012104243 143 }