lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

motor.h@0:e12eb40b9fef, 2020-04-23 (annotated)

Committer:
lsh2205
Date:
Thu Apr 23 00:38:16 2020 +0000
Revision:
0:e12eb40b9fef
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:e12eb40b9fef 1 #ifndef _MOTOR_H_
lsh2205 0:e12eb40b9fef 2 #define _MOTOR_H_
lsh2205 0:e12eb40b9fef 3
lsh2205 0:e12eb40b9fef 4 #include "FastPWM.h"
lsh2205 0:e12eb40b9fef 5
lsh2205 0:e12eb40b9fef 6
lsh2205 0:e12eb40b9fef 7
lsh2205 0:e12eb40b9fef 8 FastPWM pwm1(MOTOR_PWM1, -1);
lsh2205 0:e12eb40b9fef 9 FastPWM pwm2(MOTOR_PWM2, -1);
lsh2205 0:e12eb40b9fef 10 FastPWM pwm3(MOTOR_PWM3, -1);
lsh2205 0:e12eb40b9fef 11 FastPWM pwm4(MOTOR_PWM4, -1);
lsh2205 0:e12eb40b9fef 12 FastPWM pwm5(MOTOR_PWM5, -1);
lsh2205 0:e12eb40b9fef 13 FastPWM pwm6(MOTOR_PWM6, -1);
lsh2205 0:e12eb40b9fef 14 FastPWM pwm7(MOTOR_PWM7, -1);
lsh2205 0:e12eb40b9fef 15
lsh2205 0:e12eb40b9fef 16 void motor_init()
lsh2205 0:e12eb40b9fef 17 {
lsh2205 0:e12eb40b9fef 18 pwm1.period_us(2500);
lsh2205 0:e12eb40b9fef 19 pwm2.period_us(2500);
lsh2205 0:e12eb40b9fef 20 pwm3.period_us(2500);
lsh2205 0:e12eb40b9fef 21 pwm4.period_us(2500);
lsh2205 0:e12eb40b9fef 22 pwm5.period_us(2500);
lsh2205 0:e12eb40b9fef 23 pwm6.period_us(2500);
lsh2205 0:e12eb40b9fef 24 pwm7.period_us(2500);
lsh2205 0:e12eb40b9fef 25
lsh2205 0:e12eb40b9fef 26 pwm7.write(0.2);
lsh2205 0:e12eb40b9fef 27 }
lsh2205 0:e12eb40b9fef 28
lsh2205 0:e12eb40b9fef 29 int ex_encoder[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 30 int now_encoder[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 31 int stop_cnt[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 32
lsh2205 0:e12eb40b9fef 33 double now_motor_duty[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 34 double after_motor_duty[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 35 double motor_currnt_cut=1.0;
lsh2205 0:e12eb40b9fef 36 double sum_duty_123axis=0;
lsh2205 0:e12eb40b9fef 37 double sum_duty_456axis=0;
lsh2205 0:e12eb40b9fef 38 double total_duty=0;;
lsh2205 0:e12eb40b9fef 39 int duty_limit=1000;
lsh2205 0:e12eb40b9fef 40
lsh2205 0:e12eb40b9fef 41 int duty_limit_cnt=0;
lsh2205 0:e12eb40b9fef 42 bool duty_limit_on=false;
lsh2205 0:e12eb40b9fef 43
lsh2205 0:e12eb40b9fef 44 int error_check_boost_duty[6]={0,};
lsh2205 0:e12eb40b9fef 45
lsh2205 0:e12eb40b9fef 46 void motor_power(int motor_num,double percent)
lsh2205 0:e12eb40b9fef 47 {
lsh2205 0:e12eb40b9fef 48
lsh2205 0:e12eb40b9fef 49 percent=-percent;
lsh2205 0:e12eb40b9fef 50 double output=offset[motor_num];
lsh2205 0:e12eb40b9fef 51 if(percent<-500)
lsh2205 0:e12eb40b9fef 52 percent=-500;
lsh2205 0:e12eb40b9fef 53 if(percent>500)
lsh2205 0:e12eb40b9fef 54 percent=500;
lsh2205 0:e12eb40b9fef 55
lsh2205 0:e12eb40b9fef 56
lsh2205 0:e12eb40b9fef 57 if(percent>150 || percent<-150)
lsh2205 0:e12eb40b9fef 58 {
lsh2205 0:e12eb40b9fef 59 now_encoder[motor_num]=encoder_data[motor_num];
lsh2205 0:e12eb40b9fef 60
lsh2205 0:e12eb40b9fef 61 if(ex_encoder[motor_num]==now_encoder[motor_num])
lsh2205 0:e12eb40b9fef 62 stop_cnt[motor_num]++;
lsh2205 0:e12eb40b9fef 63 else
lsh2205 0:e12eb40b9fef 64 stop_cnt[motor_num]=0;
lsh2205 0:e12eb40b9fef 65
lsh2205 0:e12eb40b9fef 66 ex_encoder[motor_num]=now_encoder[motor_num];
lsh2205 0:e12eb40b9fef 67
lsh2205 0:e12eb40b9fef 68 if(stop_cnt[motor_num]>300)
lsh2205 0:e12eb40b9fef 69 {
lsh2205 0:e12eb40b9fef 70 if(abs(error_check_boost_duty[motor_num])>500)
lsh2205 0:e12eb40b9fef 71 motor_onoff[motor_num]=false;
lsh2205 0:e12eb40b9fef 72 else
lsh2205 0:e12eb40b9fef 73 {
lsh2205 0:e12eb40b9fef 74 if(percent<0)
lsh2205 0:e12eb40b9fef 75 error_check_boost_duty[motor_num]--;
lsh2205 0:e12eb40b9fef 76 else
lsh2205 0:e12eb40b9fef 77 error_check_boost_duty[motor_num]++;
lsh2205 0:e12eb40b9fef 78
lsh2205 0:e12eb40b9fef 79 percent=percent+error_check_boost_duty[motor_num];
lsh2205 0:e12eb40b9fef 80 }
lsh2205 0:e12eb40b9fef 81
lsh2205 0:e12eb40b9fef 82 }
lsh2205 0:e12eb40b9fef 83 }else
lsh2205 0:e12eb40b9fef 84 {
lsh2205 0:e12eb40b9fef 85 stop_cnt[motor_num]=0;
lsh2205 0:e12eb40b9fef 86
lsh2205 0:e12eb40b9fef 87 if(error_check_boost_duty[motor_num]<0)
lsh2205 0:e12eb40b9fef 88 error_check_boost_duty[motor_num]++;
lsh2205 0:e12eb40b9fef 89 else if(error_check_boost_duty[motor_num]>0)
lsh2205 0:e12eb40b9fef 90 error_check_boost_duty[motor_num]--;
lsh2205 0:e12eb40b9fef 91
lsh2205 0:e12eb40b9fef 92 }
lsh2205 0:e12eb40b9fef 93
lsh2205 0:e12eb40b9fef 94
lsh2205 0:e12eb40b9fef 95
lsh2205 0:e12eb40b9fef 96 now_motor_duty[motor_num]=abs(percent);
lsh2205 0:e12eb40b9fef 97
lsh2205 0:e12eb40b9fef 98 sum_duty_123axis = now_motor_duty[0]+now_motor_duty[1]+now_motor_duty[2]+now_motor_duty[3];
lsh2205 0:e12eb40b9fef 99 sum_duty_456axis = now_motor_duty[4]+now_motor_duty[5];
lsh2205 0:e12eb40b9fef 100
lsh2205 0:e12eb40b9fef 101 total_duty=sum_duty_123axis;
lsh2205 0:e12eb40b9fef 102
lsh2205 0:e12eb40b9fef 103
lsh2205 0:e12eb40b9fef 104 if(total_duty>duty_limit)
lsh2205 0:e12eb40b9fef 105 motor_currnt_cut=(duty_limit/total_duty);
lsh2205 0:e12eb40b9fef 106 else
lsh2205 0:e12eb40b9fef 107 motor_currnt_cut=1.0;
lsh2205 0:e12eb40b9fef 108
lsh2205 0:e12eb40b9fef 109 if(duty_limit_on)
lsh2205 0:e12eb40b9fef 110 {
lsh2205 0:e12eb40b9fef 111 if( (motor_num==1) || (motor_num==2) || (motor_num==3) || (motor_num==4) )
lsh2205 0:e12eb40b9fef 112 percent=percent*motor_currnt_cut;
lsh2205 0:e12eb40b9fef 113
lsh2205 0:e12eb40b9fef 114 duty_limit_cnt++;
lsh2205 0:e12eb40b9fef 115 if(duty_limit_cnt>200)
lsh2205 0:e12eb40b9fef 116 {
lsh2205 0:e12eb40b9fef 117 duty_limit_cnt=0;
lsh2205 0:e12eb40b9fef 118 pc.printf("%0.1f,%4.0f,%4.0f,%4.0f,%4.0f\r\n",motor_currnt_cut,now_motor_duty[0],now_motor_duty[1],now_motor_duty[2],now_motor_duty[3]);
lsh2205 0:e12eb40b9fef 119 }
lsh2205 0:e12eb40b9fef 120 }
lsh2205 0:e12eb40b9fef 121
lsh2205 0:e12eb40b9fef 122 if(motor_num==0)
lsh2205 0:e12eb40b9fef 123 {
lsh2205 0:e12eb40b9fef 124 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 125 percent=1000;
lsh2205 0:e12eb40b9fef 126
lsh2205 0:e12eb40b9fef 127 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 128 pwm1.write(output);
lsh2205 0:e12eb40b9fef 129 }
lsh2205 0:e12eb40b9fef 130 else if(motor_num==1)
lsh2205 0:e12eb40b9fef 131 {
lsh2205 0:e12eb40b9fef 132 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 133 percent=1000;
lsh2205 0:e12eb40b9fef 134
lsh2205 0:e12eb40b9fef 135 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 136 pwm2.write(output);
lsh2205 0:e12eb40b9fef 137 }
lsh2205 0:e12eb40b9fef 138 else if(motor_num==2)
lsh2205 0:e12eb40b9fef 139 {
lsh2205 0:e12eb40b9fef 140 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 141 percent=1000;
lsh2205 0:e12eb40b9fef 142
lsh2205 0:e12eb40b9fef 143 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 144 pwm3.write(output);
lsh2205 0:e12eb40b9fef 145 }
lsh2205 0:e12eb40b9fef 146 else if(motor_num==3)
lsh2205 0:e12eb40b9fef 147 {
lsh2205 0:e12eb40b9fef 148 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 149 percent=1000;
lsh2205 0:e12eb40b9fef 150
lsh2205 0:e12eb40b9fef 151 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 152 pwm4.write(output);
lsh2205 0:e12eb40b9fef 153 }
lsh2205 0:e12eb40b9fef 154 else if(motor_num==4)
lsh2205 0:e12eb40b9fef 155 {
lsh2205 0:e12eb40b9fef 156 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 157 percent=1000;
lsh2205 0:e12eb40b9fef 158
lsh2205 0:e12eb40b9fef 159 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 160 pwm5.write(output);
lsh2205 0:e12eb40b9fef 161 }
lsh2205 0:e12eb40b9fef 162 else if(motor_num==5)
lsh2205 0:e12eb40b9fef 163 {
lsh2205 0:e12eb40b9fef 164 if(motor_onoff[motor_num]==false)
lsh2205 0:e12eb40b9fef 165 percent=1000;
lsh2205 0:e12eb40b9fef 166
lsh2205 0:e12eb40b9fef 167 output = 1-(offset[motor_num] + percent)/2500;
lsh2205 0:e12eb40b9fef 168 pwm6.write(output);
lsh2205 0:e12eb40b9fef 169 }
lsh2205 0:e12eb40b9fef 170 }
lsh2205 0:e12eb40b9fef 171
lsh2205 0:e12eb40b9fef 172
lsh2205 0:e12eb40b9fef 173 #endif