lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

motor.h@3:7b195612e26d, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:27:11 2020 +0000
Revision:
3:7b195612e26d
Parent:
0:7cff999a7f5c 
fgdhgfhfghgfh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gohgwaja 0:7cff999a7f5c 1 #ifndef _MOTOR_H_
gohgwaja 0:7cff999a7f5c 2 #define _MOTOR_H_
gohgwaja 0:7cff999a7f5c 3
gohgwaja 0:7cff999a7f5c 4 #include "FastPWM.h"
gohgwaja 0:7cff999a7f5c 5
lsh3146 3:7b195612e26d 6 uint32_t motor_stop_cnt[6] = {0, 0, 0, 0, 0, 0};
gohgwaja 0:7cff999a7f5c 7
gohgwaja 0:7cff999a7f5c 8 FastPWM pwm1(MOTOR_PWM1, -1);
gohgwaja 0:7cff999a7f5c 9 FastPWM pwm2(MOTOR_PWM2, -1);
gohgwaja 0:7cff999a7f5c 10 FastPWM pwm3(MOTOR_PWM3, -1);
gohgwaja 0:7cff999a7f5c 11 FastPWM pwm4(MOTOR_PWM4, -1);
gohgwaja 0:7cff999a7f5c 12 FastPWM pwm5(MOTOR_PWM5, -1);
gohgwaja 0:7cff999a7f5c 13 FastPWM pwm6(MOTOR_PWM6, -1);
gohgwaja 0:7cff999a7f5c 14 FastPWM pwm7(MOTOR_PWM7, -1);
gohgwaja 0:7cff999a7f5c 15
gohgwaja 0:7cff999a7f5c 16 void motor_init()
gohgwaja 0:7cff999a7f5c 17 {
lsh3146 3:7b195612e26d 18 pwm1.period_us(2500.0);
lsh3146 3:7b195612e26d 19 pwm2.period_us(2500.0);
lsh3146 3:7b195612e26d 20 pwm3.period_us(2500.0);
lsh3146 3:7b195612e26d 21 pwm4.period_us(2500.0);
lsh3146 3:7b195612e26d 22 pwm5.period_us(2500.0);
lsh3146 3:7b195612e26d 23 pwm6.period_us(2500.0);
lsh3146 3:7b195612e26d 24 pwm7.period_us(2500.0);
gohgwaja 0:7cff999a7f5c 25
gohgwaja 0:7cff999a7f5c 26 pwm7.write(0.2);
gohgwaja 0:7cff999a7f5c 27 }
gohgwaja 0:7cff999a7f5c 28
gohgwaja 0:7cff999a7f5c 29 int ex_encoder[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 30 int now_encoder[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 31 int stop_cnt[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 32
gohgwaja 0:7cff999a7f5c 33 double now_motor_duty[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 34 double after_motor_duty[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 35 double motor_currnt_cut=1.0;
gohgwaja 0:7cff999a7f5c 36 double sum_duty_123axis=0;
gohgwaja 0:7cff999a7f5c 37 double sum_duty_456axis=0;
gohgwaja 0:7cff999a7f5c 38 double total_duty=0;;
gohgwaja 0:7cff999a7f5c 39 int duty_limit=1000;
gohgwaja 0:7cff999a7f5c 40
gohgwaja 0:7cff999a7f5c 41 int duty_limit_cnt=0;
gohgwaja 0:7cff999a7f5c 42 bool duty_limit_on=false;
gohgwaja 0:7cff999a7f5c 43
gohgwaja 0:7cff999a7f5c 44 int error_check_boost_duty[6]={0,};
gohgwaja 0:7cff999a7f5c 45
lsh3146 3:7b195612e26d 46
lsh3146 3:7b195612e26d 47 double motor_offset[6]={0,0,0,0,0,0};
lsh3146 3:7b195612e26d 48
lsh3146 3:7b195612e26d 49 double last_percent[6]={0,};
lsh3146 3:7b195612e26d 50
lsh3146 3:7b195612e26d 51 bool zero_state[6] = {false, false, false, false, false, false};
lsh3146 3:7b195612e26d 52
gohgwaja 0:7cff999a7f5c 53 void motor_power(int motor_num,double percent)
gohgwaja 0:7cff999a7f5c 54 {
gohgwaja 0:7cff999a7f5c 55
gohgwaja 0:7cff999a7f5c 56 percent=-percent;
gohgwaja 0:7cff999a7f5c 57 double output=offset[motor_num];
lsh3146 3:7b195612e26d 58 if(percent<-500*motor_gain[motor_num])
lsh3146 3:7b195612e26d 59 percent=-500*motor_gain[motor_num];
lsh3146 3:7b195612e26d 60 if(percent>500*motor_gain[motor_num])
lsh3146 3:7b195612e26d 61 percent=500*motor_gain[motor_num];
gohgwaja 0:7cff999a7f5c 62
gohgwaja 0:7cff999a7f5c 63
gohgwaja 0:7cff999a7f5c 64
gohgwaja 0:7cff999a7f5c 65 if(motor_num==0)
gohgwaja 0:7cff999a7f5c 66 {
lsh3146 3:7b195612e26d 67 if(percent != 0)
lsh3146 3:7b195612e26d 68 {
lsh3146 3:7b195612e26d 69 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 70 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 71 pwm1.write(output);
lsh3146 3:7b195612e26d 72 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 73 }
lsh3146 3:7b195612e26d 74 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 75 {
lsh3146 3:7b195612e26d 76 pwm1.pulsewidth_us(1040.0);
lsh3146 3:7b195612e26d 77 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 78 }
lsh3146 3:7b195612e26d 79 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 80 {
lsh3146 3:7b195612e26d 81 pwm1.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 82 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 83 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 84 {
lsh3146 3:7b195612e26d 85 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 86 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 87 }
lsh3146 3:7b195612e26d 88 }
gohgwaja 0:7cff999a7f5c 89 }
gohgwaja 0:7cff999a7f5c 90 else if(motor_num==1)
gohgwaja 0:7cff999a7f5c 91 {
lsh3146 3:7b195612e26d 92 if(percent != 0)
lsh3146 3:7b195612e26d 93 {
lsh3146 3:7b195612e26d 94 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 95 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 96 pwm2.write(output);
lsh3146 3:7b195612e26d 97 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 98 }
lsh3146 3:7b195612e26d 99 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 100 {
lsh3146 3:7b195612e26d 101 pwm2.pulsewidth_us(1030.0);
lsh3146 3:7b195612e26d 102 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 103 }
lsh3146 3:7b195612e26d 104 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 105 {
lsh3146 3:7b195612e26d 106 pwm2.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 107 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 108 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 109 {
lsh3146 3:7b195612e26d 110 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 111 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 112 }
lsh3146 3:7b195612e26d 113 }
gohgwaja 0:7cff999a7f5c 114 }
gohgwaja 0:7cff999a7f5c 115 else if(motor_num==2)
gohgwaja 0:7cff999a7f5c 116 {
lsh3146 3:7b195612e26d 117 if(percent != 0)
lsh3146 3:7b195612e26d 118 {
lsh3146 3:7b195612e26d 119 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 120 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 121 pwm3.write(output);
lsh3146 3:7b195612e26d 122 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 123 }
lsh3146 3:7b195612e26d 124 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 125 {
lsh3146 3:7b195612e26d 126 pwm3.pulsewidth_us(1000.0);
lsh3146 3:7b195612e26d 127 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 128 }
lsh3146 3:7b195612e26d 129 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 130 {
lsh3146 3:7b195612e26d 131 pwm3.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 132 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 133 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 134 {
lsh3146 3:7b195612e26d 135 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 136 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 137 }
lsh3146 3:7b195612e26d 138 }
gohgwaja 0:7cff999a7f5c 139 }
gohgwaja 0:7cff999a7f5c 140 else if(motor_num==3)
gohgwaja 0:7cff999a7f5c 141 {
lsh3146 3:7b195612e26d 142 if(percent != 0)
lsh3146 3:7b195612e26d 143 {
lsh3146 3:7b195612e26d 144 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 145 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 146 pwm4.write(output);
lsh3146 3:7b195612e26d 147 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 148 }
lsh3146 3:7b195612e26d 149 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 150 {
lsh3146 3:7b195612e26d 151 pwm4.pulsewidth_us(1000.0);
lsh3146 3:7b195612e26d 152 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 153 }
lsh3146 3:7b195612e26d 154 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 155 {
lsh3146 3:7b195612e26d 156 pwm4.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 157 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 158 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 159 {
lsh3146 3:7b195612e26d 160 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 161 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 162 }
lsh3146 3:7b195612e26d 163 }
gohgwaja 0:7cff999a7f5c 164 }
gohgwaja 0:7cff999a7f5c 165 else if(motor_num==4)
gohgwaja 0:7cff999a7f5c 166 {
lsh3146 3:7b195612e26d 167 if(percent != 0)
lsh3146 3:7b195612e26d 168 {
lsh3146 3:7b195612e26d 169 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 170 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 171 pwm5.write(output);
lsh3146 3:7b195612e26d 172 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 173 }
lsh3146 3:7b195612e26d 174 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 175 {
lsh3146 3:7b195612e26d 176 pwm5.pulsewidth_us(1000.0);
lsh3146 3:7b195612e26d 177 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 178 }
lsh3146 3:7b195612e26d 179 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 180 {
lsh3146 3:7b195612e26d 181 pwm5.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 182 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 183 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 184 {
lsh3146 3:7b195612e26d 185 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 186 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 187 }
lsh3146 3:7b195612e26d 188 }
gohgwaja 0:7cff999a7f5c 189 }
gohgwaja 0:7cff999a7f5c 190 else if(motor_num==5)
gohgwaja 0:7cff999a7f5c 191 {
lsh3146 3:7b195612e26d 192 if(percent != 0)
lsh3146 3:7b195612e26d 193 {
lsh3146 3:7b195612e26d 194 last_percent[motor_num]=percent;
lsh3146 3:7b195612e26d 195 output = 1-(offset[motor_num] + percent)/2500;
lsh3146 3:7b195612e26d 196 pwm6.write(output);
lsh3146 3:7b195612e26d 197 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 198 }
lsh3146 3:7b195612e26d 199 else if(zero_state[motor_num] == false && percent == 0)
lsh3146 3:7b195612e26d 200 {
lsh3146 3:7b195612e26d 201 pwm6.pulsewidth_us(1000.0);
lsh3146 3:7b195612e26d 202 zero_state[motor_num] = true;
lsh3146 3:7b195612e26d 203 }
lsh3146 3:7b195612e26d 204 else if(zero_state[motor_num] == true)
lsh3146 3:7b195612e26d 205 {
lsh3146 3:7b195612e26d 206 pwm6.pulsewidth_us(2500.0);
lsh3146 3:7b195612e26d 207 motor_stop_cnt[motor_num]++;
lsh3146 3:7b195612e26d 208 if(motor_stop_cnt[motor_num] > 4000)
lsh3146 3:7b195612e26d 209 {
lsh3146 3:7b195612e26d 210 zero_state[motor_num] = false;
lsh3146 3:7b195612e26d 211 motor_stop_cnt[motor_num] = 0;
lsh3146 3:7b195612e26d 212 }
lsh3146 3:7b195612e26d 213 }
gohgwaja 0:7cff999a7f5c 214 }
gohgwaja 0:7cff999a7f5c 215 }
gohgwaja 0:7cff999a7f5c 216
gohgwaja 0:7cff999a7f5c 217
lsh3146 3:7b195612e26d 218 #endif