![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Trapezoid Drive Ver2
main.cpp@5:4efc1a517308, 2020-11-17 (annotated)
- Committer:
- oguro
- Date:
- Tue Nov 17 00:10:21 2020 +0000
- Revision:
- 5:4efc1a517308
- Parent:
- 1:234a28b98ee5
New Trapezoid Drive Hall 2020 11 17
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
oguro | 0:852c73d4e9a3 | 1 | #include "mbed.h" |
oguro | 5:4efc1a517308 | 2 | |
oguro | 0:852c73d4e9a3 | 3 | |
oguro | 0:852c73d4e9a3 | 4 | unsigned int q=0,r=0,s=0,START=8; |
oguro | 0:852c73d4e9a3 | 5 | |
oguro | 0:852c73d4e9a3 | 6 | PwmOut mypwmA(PA_8); //PWM_OUT |
oguro | 0:852c73d4e9a3 | 7 | PwmOut mypwmB(PA_9); //PWM_OUT |
oguro | 0:852c73d4e9a3 | 8 | PwmOut mypwmC(PA_10);//PWM_OUT |
oguro | 0:852c73d4e9a3 | 9 | |
oguro | 0:852c73d4e9a3 | 10 | |
oguro | 0:852c73d4e9a3 | 11 | DigitalOut EN1(PC_10); |
oguro | 0:852c73d4e9a3 | 12 | DigitalOut EN2(PC_11); |
oguro | 0:852c73d4e9a3 | 13 | DigitalOut EN3(PC_12); |
oguro | 0:852c73d4e9a3 | 14 | |
oguro | 0:852c73d4e9a3 | 15 | InterruptIn HA(PA_15); |
oguro | 0:852c73d4e9a3 | 16 | InterruptIn HB(PB_3); |
oguro | 0:852c73d4e9a3 | 17 | InterruptIn HC(PB_10); |
oguro | 0:852c73d4e9a3 | 18 | |
oguro | 0:852c73d4e9a3 | 19 | unsigned int UP,VP,WP; |
oguro | 0:852c73d4e9a3 | 20 | AnalogIn V_adc(PC_2); // gaibu Volume |
oguro | 0:852c73d4e9a3 | 21 | //AnalogIn V_adc(PB_1); //Volume |
oguro | 0:852c73d4e9a3 | 22 | |
oguro | 0:852c73d4e9a3 | 23 | |
oguro | 0:852c73d4e9a3 | 24 | Serial pc(USBTX,USBRX); |
oguro | 0:852c73d4e9a3 | 25 | |
oguro | 0:852c73d4e9a3 | 26 | DigitalOut myled(LED1); |
oguro | 0:852c73d4e9a3 | 27 | |
oguro | 0:852c73d4e9a3 | 28 | float Vr_adc=0.0f; |
oguro | 0:852c73d4e9a3 | 29 | |
oguro | 0:852c73d4e9a3 | 30 | Timer uT; |
oguro | 0:852c73d4e9a3 | 31 | float ut1=0,ut2=0,usi=0; |
oguro | 0:852c73d4e9a3 | 32 | float Speed=0; |
oguro | 0:852c73d4e9a3 | 33 | |
oguro | 0:852c73d4e9a3 | 34 | void HAH(){ |
oguro | 0:852c73d4e9a3 | 35 | |
oguro | 0:852c73d4e9a3 | 36 | s=r%2; |
oguro | 0:852c73d4e9a3 | 37 | if(s==0){ |
oguro | 0:852c73d4e9a3 | 38 | ut1=uT.read_us(); |
oguro | 0:852c73d4e9a3 | 39 | r++; |
oguro | 0:852c73d4e9a3 | 40 | } |
oguro | 0:852c73d4e9a3 | 41 | |
oguro | 0:852c73d4e9a3 | 42 | if(s==1){ |
oguro | 0:852c73d4e9a3 | 43 | ut2=uT.read_us(); |
oguro | 0:852c73d4e9a3 | 44 | r++; |
oguro | 0:852c73d4e9a3 | 45 | uT.reset(); |
oguro | 0:852c73d4e9a3 | 46 | } |
oguro | 0:852c73d4e9a3 | 47 | mypwmA.write(Vr_adc); |
oguro | 0:852c73d4e9a3 | 48 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 49 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 50 | } |
oguro | 0:852c73d4e9a3 | 51 | |
oguro | 0:852c73d4e9a3 | 52 | void HAL(){ |
oguro | 0:852c73d4e9a3 | 53 | |
oguro | 0:852c73d4e9a3 | 54 | mypwmA.write(0); |
oguro | 0:852c73d4e9a3 | 55 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 56 | |
oguro | 0:852c73d4e9a3 | 57 | } |
oguro | 0:852c73d4e9a3 | 58 | void HBH(){ |
oguro | 0:852c73d4e9a3 | 59 | |
oguro | 0:852c73d4e9a3 | 60 | mypwmA.write(0); |
oguro | 0:852c73d4e9a3 | 61 | mypwmB.write(Vr_adc); |
oguro | 0:852c73d4e9a3 | 62 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 63 | } |
oguro | 0:852c73d4e9a3 | 64 | void HBL(){ |
oguro | 0:852c73d4e9a3 | 65 | |
oguro | 0:852c73d4e9a3 | 66 | mypwmA.write(0); |
oguro | 0:852c73d4e9a3 | 67 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 68 | } |
oguro | 0:852c73d4e9a3 | 69 | void HCH(){ |
oguro | 0:852c73d4e9a3 | 70 | |
oguro | 0:852c73d4e9a3 | 71 | mypwmA.write(0); |
oguro | 0:852c73d4e9a3 | 72 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 73 | mypwmC.write(Vr_adc); |
oguro | 0:852c73d4e9a3 | 74 | } |
oguro | 0:852c73d4e9a3 | 75 | |
oguro | 0:852c73d4e9a3 | 76 | void HCL(){ |
oguro | 0:852c73d4e9a3 | 77 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 78 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 79 | } |
oguro | 0:852c73d4e9a3 | 80 | |
oguro | 0:852c73d4e9a3 | 81 | int main() { |
oguro | 0:852c73d4e9a3 | 82 | pc.baud(128000); |
oguro | 0:852c73d4e9a3 | 83 | |
oguro | 0:852c73d4e9a3 | 84 | EN1=1; |
oguro | 0:852c73d4e9a3 | 85 | EN2=1; |
oguro | 0:852c73d4e9a3 | 86 | EN3=1; |
oguro | 0:852c73d4e9a3 | 87 | |
oguro | 0:852c73d4e9a3 | 88 | mypwmA.period_us(20); // PWM 50KHz |
oguro | 0:852c73d4e9a3 | 89 | |
oguro | 0:852c73d4e9a3 | 90 | mypwmB.period_us(20); |
oguro | 0:852c73d4e9a3 | 91 | |
oguro | 0:852c73d4e9a3 | 92 | mypwmC.period_us(20); |
oguro | 0:852c73d4e9a3 | 93 | |
oguro | 0:852c73d4e9a3 | 94 | |
oguro | 0:852c73d4e9a3 | 95 | while(1) { |
oguro | 0:852c73d4e9a3 | 96 | |
oguro | 0:852c73d4e9a3 | 97 | Vr_adc=V_adc.read(); |
oguro | 0:852c73d4e9a3 | 98 | uT.start(); |
oguro | 0:852c73d4e9a3 | 99 | |
oguro | 0:852c73d4e9a3 | 100 | if((Vr_adc>0.15f)&&(q==0)){ |
oguro | 0:852c73d4e9a3 | 101 | while(q<50){ |
oguro | 0:852c73d4e9a3 | 102 | |
oguro | 1:234a28b98ee5 | 103 | mypwmA.write(0); |
oguro | 1:234a28b98ee5 | 104 | mypwmB.write(0.5f); |
oguro | 0:852c73d4e9a3 | 105 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 106 | wait_ms(START); |
oguro | 0:852c73d4e9a3 | 107 | |
oguro | 1:234a28b98ee5 | 108 | mypwmA.write(0.5f); |
oguro | 1:234a28b98ee5 | 109 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 110 | mypwmC.write(0); |
oguro | 0:852c73d4e9a3 | 111 | wait_ms(START); |
oguro | 0:852c73d4e9a3 | 112 | |
oguro | 0:852c73d4e9a3 | 113 | mypwmA.write(0); |
oguro | 0:852c73d4e9a3 | 114 | mypwmB.write(0); |
oguro | 0:852c73d4e9a3 | 115 | mypwmC.write(0.5f); |
oguro | 0:852c73d4e9a3 | 116 | wait_ms(START); |
oguro | 0:852c73d4e9a3 | 117 | q++; |
oguro | 0:852c73d4e9a3 | 118 | } |
oguro | 0:852c73d4e9a3 | 119 | } |
oguro | 0:852c73d4e9a3 | 120 | |
oguro | 1:234a28b98ee5 | 121 | HA.rise(&HCH); //HAH |
oguro | 1:234a28b98ee5 | 122 | HC.fall(&HBL); //HCL |
oguro | 1:234a28b98ee5 | 123 | HB.rise(&HAH); //HBH |
oguro | 1:234a28b98ee5 | 124 | HA.fall(&HCL); //HAL |
oguro | 1:234a28b98ee5 | 125 | HC.rise(&HBH); //HCH |
oguro | 1:234a28b98ee5 | 126 | HB.fall(&HAL); //HBL |
oguro | 0:852c73d4e9a3 | 127 | // s=0; |
oguro | 0:852c73d4e9a3 | 128 | if(Vr_adc < 0.1f){ |
oguro | 0:852c73d4e9a3 | 129 | |
oguro | 0:852c73d4e9a3 | 130 | q=0; |
oguro | 0:852c73d4e9a3 | 131 | |
oguro | 0:852c73d4e9a3 | 132 | } |
oguro | 0:852c73d4e9a3 | 133 | |
oguro | 0:852c73d4e9a3 | 134 | usi=abs(ut2-ut1); |
oguro | 0:852c73d4e9a3 | 135 | Speed=60*(1/(7.0*usi*1E-6)); |
oguro | 0:852c73d4e9a3 | 136 | pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); |
oguro | 0:852c73d4e9a3 | 137 | //UP=HA; VP=HB; WP=HC; |
oguro | 0:852c73d4e9a3 | 138 | // pc.printf("%d ,%d ,%d\r" ,UP,VP,WP); |
oguro | 0:852c73d4e9a3 | 139 | myled = !myled; |
oguro | 0:852c73d4e9a3 | 140 | |
oguro | 0:852c73d4e9a3 | 141 | } |
oguro | 0:852c73d4e9a3 | 142 | } |
oguro | 0:852c73d4e9a3 | 143 |