eLab Team / Mbed OS DcMotorTest

tmp.txt

Committer:
elab
Date:
2021-02-20
Revision:
2:72952132894a

File content as of revision 2:72952132894a:

#include "mbed.h"
#include "stdio.h"

//DigitalOut led(LED1);

Serial pc(USBTX, USBRX);
DigitalIn my_button(USER_BUTTON); // Use the user button to switch from dc motor to servo motor and vice versa
DigitalIn end_stop(A5); // relais fin de course

PwmOut l1_dc_motor_cw(D2);
PwmOut l1_dc_motor_ccw(D3);
DigitalOut l1_servo_cw(D4);
DigitalOut l1_servo_ccw(D5);
PwmOut l2_dc_motor_cw(D6);
PwmOut l2_dc_motor_ccw(D7);
DigitalOut l2_servo_cw(D8);
DigitalOut l2_servo_ccw(D9);
PwmOut l3_dc_motor_cw(D10);
PwmOut l3_dc_motor_ccw(D11);
DigitalOut l3_servo_cw(D12);
DigitalOut l3_servo_ccw(D13);

void run_l1_dc_motor_cw(void){
    l1_dc_motor_cw.period_ms(20);
    l1_dc_motor_cw.pulsewidth_us(2500);
}
void run_l1_dc_motor_ccw(void){
    l1_dc_motor_ccw.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
    l1_dc_motor_ccw.pulsewidth_us(2500);
}
void run_l2_dc_motor_cw(void){
    l2_dc_motor_cw.period_ms(20);
    l2_dc_motor_cw.pulsewidth_us(5000);
}
void run_l2_dc_motor_ccw(void){
    l2_dc_motor_ccw.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
    l2_dc_motor_ccw.pulsewidth_us(5000);
}
void run_l3_dc_motor_cw(void){
//    l3_dc_motor_cw.period_ms(20);
//    l3_dc_motor_cw.pulsewidth_us(10000);
}
void run_l3_dc_motor_ccw(void){
    l3_dc_motor_ccw.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
    l3_dc_motor_ccw.pulsewidth_us(5000);
}
void stop_dc_motor(void){
    l1_dc_motor_cw.pulsewidth_us(0);
    l1_dc_motor_ccw.pulsewidth_us(0);
    l2_dc_motor_cw.pulsewidth_us(0);
    l2_dc_motor_ccw.pulsewidth_us(0);
    l3_dc_motor_cw.pulsewidth_us(0);
    l3_dc_motor_ccw.pulsewidth_us(0);
}
void run_l1_servo_cw(void){
    l1_servo_cw=1;
    l1_servo_ccw=0;
}
void run_l1_servo_ccw(void){
    l1_servo_cw=0;
    l1_servo_ccw=1;
}
void run_l2_servo_cw(void){
//    l2_servo_cw=1;
//  l2_servo_ccw=0;
}
void run_l2_servo_ccw(void){
    l2_servo_cw=0;
    l2_servo_ccw=1;
}
void run_l3_servo_cw(void){
    l3_servo_cw=1;
    l3_servo_ccw=0;
}
void run_l3_servo_ccw(void){
    l3_servo_cw=0;
    l3_servo_ccw=1;
}
void stop_servo(void){
    l1_servo_cw=0;
    l1_servo_ccw=0;
    l2_servo_cw=0;
    l2_servo_ccw=0;
    l3_servo_cw=0;
    l3_servo_ccw=0;
}
void test_limite(void){
    end_stop.mode(PullDown);
    while(1){
        printf("lim=%d\n", end_stop.read());

        if(end_stop.read()==1){
            stop_dc_motor();
//            led=0;
            printf("stop_dc_motor\n");
        }
        else{
            run_l2_dc_motor_ccw();
            printf("motor runs\n");
//            led=1;
        }
    }
    
}
int main()
{
    printf("Hello World !\n");
    unsigned char cmd=0;
    while (true) {
        scanf("%c", &cmd);
        if(cmd == '1')
        {
            cmd = 0;
            stop_dc_motor();
            printf("stop_dc_motor\r\n");
        }
        if(cmd == '2')
        {
            cmd = 0;
            run_l1_dc_motor_cw();    
            printf("run_l1_dc_motor_cw\r\n");
        }
        if(cmd == '3')
        {
            cmd = 0;
            run_l1_dc_motor_ccw();
            printf("run_l1_dc_motor_ccw\r\n");
        }
        if(cmd == '4')
        {
            cmd = 0; 
            run_l1_servo_cw();
            wait(1);
            stop_servo();
            printf("run_l1_servo_cw\r\n");
        }     
        if(cmd == '5')
        {
            cmd = 0; 
            run_l1_servo_ccw();
            wait(1);
            stop_servo();
            printf("run_l1_servo_ccw\r\n");
        }     
        if(cmd == '6')
        {
            cmd = 0;
            run_l2_dc_motor_cw();    
            printf("run_l2_dc_motor_cw\r\n");
        }
        if(cmd == '7')
        {
            cmd = 0;
            run_l2_dc_motor_ccw();
            printf("run_l2_dc_motor_ccw\r\n");
        }
        if(cmd == '8')
        {
            cmd = 0;
            run_l2_servo_cw();
            wait(1);
            stop_servo();
            printf("run_l2_servo_cw\r\n");
        }
        if(cmd == '9')
        {
            cmd = 0;
            run_l2_servo_ccw();
            wait(1);
            stop_servo();
            printf("run_l2_servo_ccw\r\n");
        }


        
        if(cmd == 'a')
        {
            cmd = 0;
            run_l3_dc_motor_cw();    
            printf("run_l3_dc_motor_cw\r\n");
        }
        if(cmd == 'b')
        {
            cmd = 0;
            run_l3_dc_motor_ccw();
            printf("run_l3_dc_motor_ccw\r\n");
        }
        if(cmd == 'c')
        {
            cmd = 0;
            run_l3_servo_cw();
            wait(1);
            stop_servo();
            printf("run_l3_servo_cw\r\n");
        }
        if(cmd == 'd')
        {
            cmd = 0;
            run_l3_servo_ccw();
            wait(1);
            stop_servo();
            printf("run_l3_servo_ccw\r\n");
        }
        if(cmd == 't')
        {
            cmd = 0;
            test_limite();
            printf("test_limite()\r\n");
        }

        if(cmd == 'h')
        {
            cmd=0;
            printf("1=stop_dc_motor\n");
            printf("2=run_l1_dc_motor_cw; 3=run_l1_dc_motor_ccw; 4=run_l1_servo_cw; 5=run_l1_servo_ccw\n");
            printf("6=run_l2_dc_motor_cw; 7=run_l2_dc_motor_ccw; 8=run_l2_servo_cw; 9=run_l2_servo_ccw\n");
            printf("a=run_l3_dc_motor_cw; b=run_l3_dc_motor_ccw; c=run_l3_servo_cw; d=run_l3_servo_ccw.\n");
        }
    }
}