![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Open Drive Trapezoid
main.cpp@0:3b5242119d17, 2020-08-27 (annotated)
- Committer:
- oguro
- Date:
- Thu Aug 27 03:03:43 2020 +0000
- Revision:
- 0:3b5242119d17
OPEN Drive Trapezoid
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
oguro | 0:3b5242119d17 | 1 | #include "mbed.h" |
oguro | 0:3b5242119d17 | 2 | int i=0,t=0,tt=0,q=0,START=4; |
oguro | 0:3b5242119d17 | 3 | unsigned int adc; |
oguro | 0:3b5242119d17 | 4 | PwmOut mypwmA(PA_8); //PWM_OUT 8 |
oguro | 0:3b5242119d17 | 5 | PwmOut mypwmB(PA_9); //9 |
oguro | 0:3b5242119d17 | 6 | PwmOut mypwmC(PA_10);//10 |
oguro | 0:3b5242119d17 | 7 | |
oguro | 0:3b5242119d17 | 8 | PwmOut Current_Ref(PB_4); |
oguro | 0:3b5242119d17 | 9 | |
oguro | 0:3b5242119d17 | 10 | DigitalOut EN1(PC_10); |
oguro | 0:3b5242119d17 | 11 | DigitalOut EN2(PC_11); |
oguro | 0:3b5242119d17 | 12 | DigitalOut EN3(PC_12); |
oguro | 0:3b5242119d17 | 13 | |
oguro | 0:3b5242119d17 | 14 | |
oguro | 0:3b5242119d17 | 15 | AnalogIn V_adc(PC_2); |
oguro | 0:3b5242119d17 | 16 | AnalogOut CP_re(PA_4); |
oguro | 0:3b5242119d17 | 17 | |
oguro | 0:3b5242119d17 | 18 | Serial pc(USBTX,USBRX); |
oguro | 0:3b5242119d17 | 19 | |
oguro | 0:3b5242119d17 | 20 | |
oguro | 0:3b5242119d17 | 21 | float Vr_adc=0.0f; |
oguro | 0:3b5242119d17 | 22 | float Speed=0; |
oguro | 0:3b5242119d17 | 23 | |
oguro | 0:3b5242119d17 | 24 | int main() { |
oguro | 0:3b5242119d17 | 25 | pc.baud(128000); |
oguro | 0:3b5242119d17 | 26 | EN1=0; |
oguro | 0:3b5242119d17 | 27 | EN2=0; |
oguro | 0:3b5242119d17 | 28 | EN3=0; |
oguro | 0:3b5242119d17 | 29 | |
oguro | 0:3b5242119d17 | 30 | mypwmA.period_us(20); |
oguro | 0:3b5242119d17 | 31 | |
oguro | 0:3b5242119d17 | 32 | mypwmB.period_us(20); |
oguro | 0:3b5242119d17 | 33 | |
oguro | 0:3b5242119d17 | 34 | mypwmC.period_us(20); |
oguro | 0:3b5242119d17 | 35 | |
oguro | 0:3b5242119d17 | 36 | |
oguro | 0:3b5242119d17 | 37 | while(1) { |
oguro | 0:3b5242119d17 | 38 | |
oguro | 0:3b5242119d17 | 39 | Vr_adc=V_adc.read(); |
oguro | 0:3b5242119d17 | 40 | adc= V_adc.read_u16()/35 ; //>>5 |
oguro | 0:3b5242119d17 | 41 | |
oguro | 0:3b5242119d17 | 42 | |
oguro | 0:3b5242119d17 | 43 | while((Vr_adc>0.15f)&&(q<50)){ |
oguro | 0:3b5242119d17 | 44 | |
oguro | 0:3b5242119d17 | 45 | EN1=1; |
oguro | 0:3b5242119d17 | 46 | EN2=1; |
oguro | 0:3b5242119d17 | 47 | EN3=0; |
oguro | 0:3b5242119d17 | 48 | mypwmA.write(0.7f); |
oguro | 0:3b5242119d17 | 49 | mypwmB=0; |
oguro | 0:3b5242119d17 | 50 | mypwmC=0; |
oguro | 0:3b5242119d17 | 51 | wait_ms(START); |
oguro | 0:3b5242119d17 | 52 | EN1=1; |
oguro | 0:3b5242119d17 | 53 | EN2=0; |
oguro | 0:3b5242119d17 | 54 | EN3=1; |
oguro | 0:3b5242119d17 | 55 | wait_ms(START); |
oguro | 0:3b5242119d17 | 56 | EN1=0; |
oguro | 0:3b5242119d17 | 57 | EN2=1; |
oguro | 0:3b5242119d17 | 58 | EN3=1; |
oguro | 0:3b5242119d17 | 59 | mypwmA=0; |
oguro | 0:3b5242119d17 | 60 | mypwmB.write(0.7f); |
oguro | 0:3b5242119d17 | 61 | mypwmC=0; |
oguro | 0:3b5242119d17 | 62 | wait_ms(START); |
oguro | 0:3b5242119d17 | 63 | EN1=1; |
oguro | 0:3b5242119d17 | 64 | EN2=1; |
oguro | 0:3b5242119d17 | 65 | EN3=0; |
oguro | 0:3b5242119d17 | 66 | wait_ms(START); |
oguro | 0:3b5242119d17 | 67 | EN1=1; |
oguro | 0:3b5242119d17 | 68 | EN2=0; |
oguro | 0:3b5242119d17 | 69 | EN3=1; |
oguro | 0:3b5242119d17 | 70 | mypwmA=0; |
oguro | 0:3b5242119d17 | 71 | mypwmB=0; |
oguro | 0:3b5242119d17 | 72 | mypwmC.write(0.7f); |
oguro | 0:3b5242119d17 | 73 | wait_ms(START); |
oguro | 0:3b5242119d17 | 74 | EN1=0; |
oguro | 0:3b5242119d17 | 75 | EN2=1; |
oguro | 0:3b5242119d17 | 76 | EN3=1; |
oguro | 0:3b5242119d17 | 77 | q++; |
oguro | 0:3b5242119d17 | 78 | wait_ms(START); |
oguro | 0:3b5242119d17 | 79 | } |
oguro | 0:3b5242119d17 | 80 | |
oguro | 0:3b5242119d17 | 81 | if(Vr_adc < 0.005f){ |
oguro | 0:3b5242119d17 | 82 | q=0; |
oguro | 0:3b5242119d17 | 83 | } |
oguro | 0:3b5242119d17 | 84 | |
oguro | 0:3b5242119d17 | 85 | t=3000-(adc);//3000 |
oguro | 0:3b5242119d17 | 86 | if(t<=2){ |
oguro | 0:3b5242119d17 | 87 | t=2; |
oguro | 0:3b5242119d17 | 88 | } |
oguro | 0:3b5242119d17 | 89 | tt=t; |
oguro | 0:3b5242119d17 | 90 | |
oguro | 0:3b5242119d17 | 91 | |
oguro | 0:3b5242119d17 | 92 | if(Vr_adc >0.05f){ |
oguro | 0:3b5242119d17 | 93 | |
oguro | 0:3b5242119d17 | 94 | |
oguro | 0:3b5242119d17 | 95 | EN1=1; //1 |
oguro | 0:3b5242119d17 | 96 | EN2=1; //1 |
oguro | 0:3b5242119d17 | 97 | EN3=0; //0 |
oguro | 0:3b5242119d17 | 98 | |
oguro | 0:3b5242119d17 | 99 | mypwmA.write(Vr_adc); |
oguro | 0:3b5242119d17 | 100 | mypwmB.write(0.0f); |
oguro | 0:3b5242119d17 | 101 | mypwmC.write(0.0f); |
oguro | 0:3b5242119d17 | 102 | |
oguro | 0:3b5242119d17 | 103 | wait_us(t); |
oguro | 0:3b5242119d17 | 104 | |
oguro | 0:3b5242119d17 | 105 | EN1=1; //1 |
oguro | 0:3b5242119d17 | 106 | EN2=0; //0 |
oguro | 0:3b5242119d17 | 107 | EN3=1; //1 |
oguro | 0:3b5242119d17 | 108 | wait_us(tt); |
oguro | 0:3b5242119d17 | 109 | |
oguro | 0:3b5242119d17 | 110 | EN1=0; //0 |
oguro | 0:3b5242119d17 | 111 | EN2=1; //1 |
oguro | 0:3b5242119d17 | 112 | EN3=1; //1 |
oguro | 0:3b5242119d17 | 113 | mypwmA.write(0.0f); |
oguro | 0:3b5242119d17 | 114 | mypwmB.write(Vr_adc); |
oguro | 0:3b5242119d17 | 115 | mypwmC.write(0.0f); |
oguro | 0:3b5242119d17 | 116 | |
oguro | 0:3b5242119d17 | 117 | wait_us(t); |
oguro | 0:3b5242119d17 | 118 | EN1=1;//1 |
oguro | 0:3b5242119d17 | 119 | EN2=1;//1 |
oguro | 0:3b5242119d17 | 120 | EN3=0;//0 |
oguro | 0:3b5242119d17 | 121 | wait_us(tt); |
oguro | 0:3b5242119d17 | 122 | |
oguro | 0:3b5242119d17 | 123 | EN1=1; //1 |
oguro | 0:3b5242119d17 | 124 | EN2=0; //0 |
oguro | 0:3b5242119d17 | 125 | EN3=1; //1 |
oguro | 0:3b5242119d17 | 126 | mypwmA.write(0.0f); |
oguro | 0:3b5242119d17 | 127 | mypwmB.write(0.0f); |
oguro | 0:3b5242119d17 | 128 | mypwmC.write(Vr_adc); |
oguro | 0:3b5242119d17 | 129 | wait_us(t); |
oguro | 0:3b5242119d17 | 130 | EN1=0; //0 |
oguro | 0:3b5242119d17 | 131 | EN2=1; //1 |
oguro | 0:3b5242119d17 | 132 | EN3=1; //1 |
oguro | 0:3b5242119d17 | 133 | |
oguro | 0:3b5242119d17 | 134 | wait_us(tt); |
oguro | 0:3b5242119d17 | 135 | } |
oguro | 0:3b5242119d17 | 136 | else{ |
oguro | 0:3b5242119d17 | 137 | mypwmA.write(0.0f); |
oguro | 0:3b5242119d17 | 138 | mypwmB.write(0.0f); |
oguro | 0:3b5242119d17 | 139 | mypwmC.write(0.0f); |
oguro | 0:3b5242119d17 | 140 | } |
oguro | 0:3b5242119d17 | 141 | Speed=60*(1/(7.0*6*float(t)*1E-6))-476; |
oguro | 0:3b5242119d17 | 142 | pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); |
oguro | 0:3b5242119d17 | 143 | |
oguro | 0:3b5242119d17 | 144 | } |
oguro | 0:3b5242119d17 | 145 | } |