lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

motor.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
0:7cff999a7f5c 
aaaaaqqqqq

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
lsh3146 4:bf278ddb8504 46
lsh3146 4:bf278ddb8504 47 double motor_offset[6]={0,0,0,0,0,0};
lsh3146 4:bf278ddb8504 48
lsh3146 4:bf278ddb8504 49 double last_percent[6]={0,};
lsh3146 4:bf278ddb8504 50
gohgwaja 0:7cff999a7f5c 51 void motor_power(int motor_num,double percent)
gohgwaja 0:7cff999a7f5c 52 {
gohgwaja 0:7cff999a7f5c 53
gohgwaja 0:7cff999a7f5c 54 percent=-percent;
gohgwaja 0:7cff999a7f5c 55 double output=offset[motor_num];
lsh3146 4:bf278ddb8504 56 if(percent<-500*motor_gain[motor_num])
lsh3146 4:bf278ddb8504 57 percent=-500*motor_gain[motor_num];
lsh3146 4:bf278ddb8504 58 if(percent>500*motor_gain[motor_num])
lsh3146 4:bf278ddb8504 59 percent=500*motor_gain[motor_num];
gohgwaja 0:7cff999a7f5c 60
gohgwaja 0:7cff999a7f5c 61
gohgwaja 0:7cff999a7f5c 62
gohgwaja 0:7cff999a7f5c 63 if(motor_num==0)
lsh3146 4:bf278ddb8504 64 {
lsh3146 4:bf278ddb8504 65 last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 66 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 67 pwm1.write(output);
gohgwaja 0:7cff999a7f5c 68 }
gohgwaja 0:7cff999a7f5c 69 else if(motor_num==1)
gohgwaja 0:7cff999a7f5c 70 {
lsh3146 4:bf278ddb8504 71 last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 72 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 73 pwm2.write(output);
gohgwaja 0:7cff999a7f5c 74 }
gohgwaja 0:7cff999a7f5c 75 else if(motor_num==2)
gohgwaja 0:7cff999a7f5c 76 {
lsh3146 4:bf278ddb8504 77 last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 78 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 79 pwm3.write(output);
gohgwaja 0:7cff999a7f5c 80 }
gohgwaja 0:7cff999a7f5c 81 else if(motor_num==3)
lsh3146 4:bf278ddb8504 82 { last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 83 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 84 pwm4.write(output);
gohgwaja 0:7cff999a7f5c 85 }
gohgwaja 0:7cff999a7f5c 86 else if(motor_num==4)
gohgwaja 0:7cff999a7f5c 87 {
lsh3146 4:bf278ddb8504 88 last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 89 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 90 pwm5.write(output);
gohgwaja 0:7cff999a7f5c 91 }
gohgwaja 0:7cff999a7f5c 92 else if(motor_num==5)
gohgwaja 0:7cff999a7f5c 93 {
lsh3146 4:bf278ddb8504 94 last_percent[motor_num]=percent;
gohgwaja 0:7cff999a7f5c 95 output = 1-(offset[motor_num] + percent)/2500;
gohgwaja 0:7cff999a7f5c 96 pwm6.write(output);
gohgwaja 0:7cff999a7f5c 97 }
gohgwaja 0:7cff999a7f5c 98 }
gohgwaja 0:7cff999a7f5c 99
gohgwaja 0:7cff999a7f5c 100
gohgwaja 0:7cff999a7f5c 101 #endif