lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

motor.h@0:7cff999a7f5c, 2020-05-11 (annotated)

Committer:
gohgwaja
Date:
Mon May 11 08:47:18 2020 +0000
Revision:
0:7cff999a7f5c
Child:
3:7b195612e26d
Child:
4:bf278ddb8504
hahaha

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
gohgwaja 0:7cff999a7f5c 6
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 {
gohgwaja 0:7cff999a7f5c 18 pwm1.period_us(2500);
gohgwaja 0:7cff999a7f5c 19 pwm2.period_us(2500);
gohgwaja 0:7cff999a7f5c 20 pwm3.period_us(2500);
gohgwaja 0:7cff999a7f5c 21 pwm4.period_us(2500);
gohgwaja 0:7cff999a7f5c 22 pwm5.period_us(2500);
gohgwaja 0:7cff999a7f5c 23 pwm6.period_us(2500);
gohgwaja 0:7cff999a7f5c 24 pwm7.period_us(2500);
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
gohgwaja 0:7cff999a7f5c 46 void motor_power(int motor_num,double percent)
gohgwaja 0:7cff999a7f5c 47 {
gohgwaja 0:7cff999a7f5c 48
gohgwaja 0:7cff999a7f5c 49 percent=-percent;
gohgwaja 0:7cff999a7f5c 50 double output=offset[motor_num];
gohgwaja 0:7cff999a7f5c 51 if(percent<-500)
gohgwaja 0:7cff999a7f5c 52 percent=-500;
gohgwaja 0:7cff999a7f5c 53 if(percent>500)
gohgwaja 0:7cff999a7f5c 54 percent=500;
gohgwaja 0:7cff999a7f5c 55
gohgwaja 0:7cff999a7f5c 56
gohgwaja 0:7cff999a7f5c 57 if(percent>150 || percent<-150)
gohgwaja 0:7cff999a7f5c 58 {
gohgwaja 0:7cff999a7f5c 59 now_encoder[motor_num]=encoder_data[motor_num];
gohgwaja 0:7cff999a7f5c 60
gohgwaja 0:7cff999a7f5c 61 if(ex_encoder[motor_num]==now_encoder[motor_num])
gohgwaja 0:7cff999a7f5c 62 stop_cnt[motor_num]++;
gohgwaja 0:7cff999a7f5c 63 else
gohgwaja 0:7cff999a7f5c 64 stop_cnt[motor_num]=0;
gohgwaja 0:7cff999a7f5c 65
gohgwaja 0:7cff999a7f5c 66 ex_encoder[motor_num]=now_encoder[motor_num];
gohgwaja 0:7cff999a7f5c 67
gohgwaja 0:7cff999a7f5c 68 if(stop_cnt[motor_num]>300)
gohgwaja 0:7cff999a7f5c 69 {
gohgwaja 0:7cff999a7f5c 70 if(abs(error_check_boost_duty[motor_num])>500)
gohgwaja 0:7cff999a7f5c 71 motor_onoff[motor_num]=false;
gohgwaja 0:7cff999a7f5c 72 else
gohgwaja 0:7cff999a7f5c 73 {
gohgwaja 0:7cff999a7f5c 74 if(percent<0)
gohgwaja 0:7cff999a7f5c 75 error_check_boost_duty[motor_num]--;
gohgwaja 0:7cff999a7f5c 76 else
gohgwaja 0:7cff999a7f5c 77 error_check_boost_duty[motor_num]++;
gohgwaja 0:7cff999a7f5c 78
gohgwaja 0:7cff999a7f5c 79 percent=percent+error_check_boost_duty[motor_num];
gohgwaja 0:7cff999a7f5c 80 }
gohgwaja 0:7cff999a7f5c 81
gohgwaja 0:7cff999a7f5c 82 }
gohgwaja 0:7cff999a7f5c 83 }else
gohgwaja 0:7cff999a7f5c 84 {
gohgwaja 0:7cff999a7f5c 85 stop_cnt[motor_num]=0;
gohgwaja 0:7cff999a7f5c 86
gohgwaja 0:7cff999a7f5c 87 if(error_check_boost_duty[motor_num]<0)
gohgwaja 0:7cff999a7f5c 88 error_check_boost_duty[motor_num]++;
gohgwaja 0:7cff999a7f5c 89 else if(error_check_boost_duty[motor_num]>0)
gohgwaja 0:7cff999a7f5c 90 error_check_boost_duty[motor_num]--;
gohgwaja 0:7cff999a7f5c 91
gohgwaja 0:7cff999a7f5c 92 }
gohgwaja 0:7cff999a7f5c 93
gohgwaja 0:7cff999a7f5c 94
gohgwaja 0:7cff999a7f5c 95
gohgwaja 0:7cff999a7f5c 96 now_motor_duty[motor_num]=abs(percent);
gohgwaja 0:7cff999a7f5c 97
gohgwaja 0:7cff999a7f5c 98 sum_duty_123axis = now_motor_duty[0]+now_motor_duty[1]+now_motor_duty[2]+now_motor_duty[3];
gohgwaja 0:7cff999a7f5c 99 sum_duty_456axis = now_motor_duty[4]+now_motor_duty[5];
gohgwaja 0:7cff999a7f5c 100
gohgwaja 0:7cff999a7f5c 101 total_duty=sum_duty_123axis;
gohgwaja 0:7cff999a7f5c 102
gohgwaja 0:7cff999a7f5c 103
gohgwaja 0:7cff999a7f5c 104 if(total_duty>duty_limit)
gohgwaja 0:7cff999a7f5c 105 motor_currnt_cut=(duty_limit/total_duty);
gohgwaja 0:7cff999a7f5c 106 else
gohgwaja 0:7cff999a7f5c 107 motor_currnt_cut=1.0;
gohgwaja 0:7cff999a7f5c 108
gohgwaja 0:7cff999a7f5c 109 if(duty_limit_on)
gohgwaja 0:7cff999a7f5c 110 {
gohgwaja 0:7cff999a7f5c 111 if( (motor_num==1) || (motor_num==2) || (motor_num==3) || (motor_num==4) )
gohgwaja 0:7cff999a7f5c 112 percent=percent*motor_currnt_cut;
gohgwaja 0:7cff999a7f5c 113
gohgwaja 0:7cff999a7f5c 114 duty_limit_cnt++;
gohgwaja 0:7cff999a7f5c 115 if(duty_limit_cnt>200)
gohgwaja 0:7cff999a7f5c 116 {
gohgwaja 0:7cff999a7f5c 117 duty_limit_cnt=0;
gohgwaja 0:7cff999a7f5c 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]);
gohgwaja 0:7cff999a7f5c 119 }
gohgwaja 0:7cff999a7f5c 120 }
gohgwaja 0:7cff999a7f5c 121
gohgwaja 0:7cff999a7f5c 122 if(motor_num==0)
gohgwaja 0:7cff999a7f5c 123 {
gohgwaja 0:7cff999a7f5c 124 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 125 percent=1000;
gohgwaja 0:7cff999a7f5c 126
gohgwaja 0:7cff999a7f5c 127 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 128 pwm1.write(output);
gohgwaja 0:7cff999a7f5c 129 }
gohgwaja 0:7cff999a7f5c 130 else if(motor_num==1)
gohgwaja 0:7cff999a7f5c 131 {
gohgwaja 0:7cff999a7f5c 132 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 133 percent=1000;
gohgwaja 0:7cff999a7f5c 134
gohgwaja 0:7cff999a7f5c 135 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 136 pwm2.write(output);
gohgwaja 0:7cff999a7f5c 137 }
gohgwaja 0:7cff999a7f5c 138 else if(motor_num==2)
gohgwaja 0:7cff999a7f5c 139 {
gohgwaja 0:7cff999a7f5c 140 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 141 percent=1000;
gohgwaja 0:7cff999a7f5c 142
gohgwaja 0:7cff999a7f5c 143 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 144 pwm3.write(output);
gohgwaja 0:7cff999a7f5c 145 }
gohgwaja 0:7cff999a7f5c 146 else if(motor_num==3)
gohgwaja 0:7cff999a7f5c 147 {
gohgwaja 0:7cff999a7f5c 148 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 149 percent=1000;
gohgwaja 0:7cff999a7f5c 150
gohgwaja 0:7cff999a7f5c 151 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 152 pwm4.write(output);
gohgwaja 0:7cff999a7f5c 153 }
gohgwaja 0:7cff999a7f5c 154 else if(motor_num==4)
gohgwaja 0:7cff999a7f5c 155 {
gohgwaja 0:7cff999a7f5c 156 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 157 percent=1000;
gohgwaja 0:7cff999a7f5c 158
gohgwaja 0:7cff999a7f5c 159 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 160 pwm5.write(output);
gohgwaja 0:7cff999a7f5c 161 }
gohgwaja 0:7cff999a7f5c 162 else if(motor_num==5)
gohgwaja 0:7cff999a7f5c 163 {
gohgwaja 0:7cff999a7f5c 164 if(motor_onoff[motor_num]==false)
gohgwaja 0:7cff999a7f5c 165 percent=1000;
gohgwaja 0:7cff999a7f5c 166
gohgwaja 0:7cff999a7f5c 167 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 168 pwm6.write(output);
gohgwaja 0:7cff999a7f5c 169 }
gohgwaja 0:7cff999a7f5c 170 }
gohgwaja 0:7cff999a7f5c 171
gohgwaja 0:7cff999a7f5c 172
gohgwaja 0:7cff999a7f5c 173 #endif