Open Drive Trapezoid

Dependencies:   mbed

main.cpp

Committer:
oguro
Date:
2020-08-27
Revision:
0:3b5242119d17

File content as of revision 0:3b5242119d17:

#include "mbed.h"
int i=0,t=0,tt=0,q=0,START=4;
unsigned int adc;
PwmOut mypwmA(PA_8); //PWM_OUT 8
PwmOut mypwmB(PA_9); //9
PwmOut mypwmC(PA_10);//10

PwmOut  Current_Ref(PB_4);

DigitalOut EN1(PC_10);
DigitalOut EN2(PC_11);
DigitalOut EN3(PC_12);


AnalogIn V_adc(PC_2);
AnalogOut CP_re(PA_4);

Serial pc(USBTX,USBRX);


float Vr_adc=0.0f;
float Speed=0;   
 
int main() {
   pc.baud(128000);  
   EN1=0;
   EN2=0;
   EN3=0;
   
    mypwmA.period_us(20);
   
    mypwmB.period_us(20);
   
    mypwmC.period_us(20);
     
    
 while(1) {
     
      Vr_adc=V_adc.read();
      adc= V_adc.read_u16()/35 ; //>>5
      
      
   while((Vr_adc>0.15f)&&(q<50)){ 
   
    EN1=1;
    EN2=1;
    EN3=0;
    mypwmA.write(0.7f);
    mypwmB=0;
    mypwmC=0;
    wait_ms(START);
    EN1=1;
    EN2=0;
    EN3=1;
    wait_ms(START);
    EN1=0;
    EN2=1;
    EN3=1;
    mypwmA=0;
    mypwmB.write(0.7f);
    mypwmC=0;
    wait_ms(START);
    EN1=1;
    EN2=1;
    EN3=0;
    wait_ms(START);
    EN1=1;
    EN2=0;
    EN3=1;
    mypwmA=0;
    mypwmB=0;
    mypwmC.write(0.7f);
    wait_ms(START);
    EN1=0;
    EN2=1;
    EN3=1;
    q++;
    wait_ms(START);
    }
    
      if(Vr_adc < 0.005f){
          q=0;         
       }  
       
      t=3000-(adc);//3000
      if(t<=2){
       t=2;
       }
       tt=t;
       
     
if(Vr_adc >0.05f){
      
    
      EN1=1; //1
      EN2=1; //1
      EN3=0; //0
      
      mypwmA.write(Vr_adc);
      mypwmB.write(0.0f);
      mypwmC.write(0.0f);
   
       wait_us(t);
    
       EN1=1; //1
       EN2=0; //0
       EN3=1; //1
      wait_us(tt);
 
      EN1=0; //0
      EN2=1; //1
      EN3=1; //1
       mypwmA.write(0.0f);
       mypwmB.write(Vr_adc);
       mypwmC.write(0.0f);
    
      wait_us(t);
      EN1=1;//1
      EN2=1;//1
      EN3=0;//0
        wait_us(tt);
    
       EN1=1; //1
       EN2=0; //0
       EN3=1; //1
       mypwmA.write(0.0f);
       mypwmB.write(0.0f);
       mypwmC.write(Vr_adc);
        wait_us(t);
       EN1=0; //0
       EN2=1; //1
       EN3=1; //1
    
       wait_us(tt);
      }
     else{ 
       mypwmA.write(0.0f);
       mypwmB.write(0.0f);
       mypwmC.write(0.0f);
       }
       Speed=60*(1/(7.0*6*float(t)*1E-6))-476;
       pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
        
    }
}