Diff: main.cpp

Revision:

2:72952132894a

Parent:

1:77096ac47e7a

--- a/main.cpp	Thu Jan 28 15:51:12 2021 +0000
+++ b/main.cpp	Sat Feb 20 09:04:41 2021 +0000
@@ -1,83 +1,90 @@
 #include "mbed.h"
 #include "stdio.h"
 
-DigitalOut gpo(D10);
-DigitalOut led(LED1);
+//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(D2); // relais fin de course
-
-PwmOut l1_dc_motor_en(D0);
-DigitalOut l1_dc_motor_cw(D1);
-DigitalOut l1_dc_motor_ccw(D2);
+DigitalIn my_button(USER_BUTTON); // Use the user button to switch from control mode and auto mode
+DigitalIn end_stop(A5); // relais fin de course
 
-PwmOut l1_servo_en(D5);
-DigitalOut l1_servo_cw(D3);
-DigitalOut l1_servo_ccw(D4);
-
-
-PwmOut l2_dc_motor_en(D6);
-DigitalOut l2_dc_motor_cw(D7);
-DigitalOut l2_dc_motor_ccw(D8);
-
-PwmOut l2_servo_en(D11);
-DigitalOut l2_servo_cw(D9);
-DigitalOut l2_servo_ccw(D10);
-
+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);
+int l1_dc_speed=2500;
 
 void run_l1_dc_motor_cw(void){
-    l1_dc_motor_en.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
-    l1_dc_motor_en.pulsewidth_us(3000);
-    l1_dc_motor_cw=0;
-    l1_dc_motor_ccw=1;
+    l1_dc_motor_cw.period_ms(20);
+    l1_dc_motor_cw.pulsewidth_us(l1_dc_speed);//1800
 }
 void run_l1_dc_motor_ccw(void){
-    l1_dc_motor_en.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
-    l1_dc_motor_en.pulsewidth_us(3000);
-    l1_dc_motor_cw=1;
-    l1_dc_motor_ccw=0;
+    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(l1_dc_speed);
 }
 void run_l2_dc_motor_cw(void){
-    l2_dc_motor_en.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
-    l2_dc_motor_en.pulsewidth_us(3000);
-    l2_dc_motor_cw=0;
-    l2_dc_motor_ccw=1;
+    l2_dc_motor_cw.period_ms(20);
+    l2_dc_motor_cw.pulsewidth_us(5000);
 }
 void run_l2_dc_motor_ccw(void){
-    l2_dc_motor_en.period_ms(20);    // shoul be 20 ms (490 Hz) and 10 ms for the servo (980 Hz)
-    l2_dc_motor_en.pulsewidth_us(3000);
-    l2_dc_motor_cw=1;
-    l2_dc_motor_ccw=0;
+    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=0;
-    l1_dc_motor_ccw=0; 
-    l2_dc_motor_cw=0;
-    l2_dc_motor_ccw=0;
-
+    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;
-    l1_servo_en.period_us(870);
-    l1_servo_en.pulsewidth_ms(870);  
 }
 void run_l1_servo_ccw(void){
     l1_servo_cw=0;
     l1_servo_ccw=1;
-    l1_servo_en.period_us(870);
-    l1_servo_en.pulsewidth_ms(870);  
+}
+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){
@@ -85,24 +92,61 @@
 
         if(end_stop.read()==1){
             stop_dc_motor();
-            led=0;
+//            led=0;
             printf("stop_dc_motor\n");
         }
         else{
-            run_l1_dc_motor_ccw();
+            run_l2_dc_motor_ccw();
             printf("motor runs\n");
-            led=1;
+//            led=1;
         }
     }
     
 }
-
+void auto_run(void){
+    int l1_wait = 1300;
+    int l2_wait = 1250;
+    int l3_wait = 2500;
 
-int main()
-{
+    printf("Hello World !\n");
+    run_l1_dc_motor_cw();
+    wait_ms(l1_wait);
+    stop_dc_motor();
+    run_l1_dc_motor_ccw();  
+    wait_ms(l1_wait);
+    stop_dc_motor();   
+    wait_ms(1000);  
+    run_l1_servo_cw();
+    wait_ms(1000);
+    stop_servo();
+    run_l2_dc_motor_ccw();
+    wait_ms(l2_wait);
+    stop_dc_motor();
+    wait_ms(1000);
+    run_l2_servo_ccw();
+    wait_ms(1000);
+    stop_servo();
+    run_l2_dc_motor_cw();
+    wait_ms(l2_wait);
+    stop_dc_motor();
+    run_l1_dc_motor_cw();
+    wait_ms(l1_wait);
+    stop_dc_motor();
+    run_l1_dc_motor_ccw();  
+    wait_ms(l1_wait);
+    stop_dc_motor();  
+    wait_ms(1000);   
+    run_l1_servo_ccw();
+    wait_ms(1000);
+    stop_servo();
+    run_l3_dc_motor_ccw();
+    wait_ms(l3_wait);
+    stop_dc_motor();
+}
+
+void control_mode(void){
     unsigned char cmd=0;
-           
-    while (true) {
+    while(1){
         scanf("%c", &cmd);
         if(cmd == '1')
         {
@@ -150,14 +194,83 @@
             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 == 'e')
+        {
+            cmd = 0;
+            printf("exit\r\n");
+            break;
+        }
 
         if(cmd == 'h')
         {
             cmd=0;
-            printf("1=stop_dc_motor; 2=run_l1_dc_motor_cw; 3=run_l1_dc_motor_ccw; 4=run_l1_servo_cw; 5=run_l1_servo_ccw, 6=run_l1_dc_motor_cw; 7=run_l1_dc_motor_ccw\n");
+            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");
         }
+        
+    }   
+
+}
+
+int main(){
+    while(1){
+        for (int i=0; i<3; i++) {
+            auto_run();
+        }
+        control_mode();        
     }
-   
 }