1 files changed, 117 insertions, 0 deletions
diff --git a/Modelica-Arduino-MDD_Arduino_Revised/Resources/src/dcmotor.c b/Modelica-Arduino-MDD_Arduino_Revised/Resources/src/dcmotor.c
new file mode 100755
index 0000000..ac6775c
--- /dev/null
+++ b/Modelica-Arduino-MDD_Arduino_Revised/Resources/src/dcmotor.c
@@ -0,0 +1,117 @@
+#include <errno.h>
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+#include "../Include/serial.h"
+#include "../Include/dcmotor.h"
+
+#define abs(x) (x>=0?x:-x)
+
+void cmd_dcmotor_setup(int h,int driver_type,int motor_no,int pin_no_1,int pin_no_2)
+{
+	printf("init DCmotor\n");
+	char code_sent[10]="C";
+	int wr;
+	char v1[2],v2[2],motor[2];
+    if(driver_type==1)
+    {
+    	sprintf(v1,"%c",pin_no_1+48);
+    	sprintf(v2,"%c",pin_no_2+48);
+    	sprintf(motor,"%c",motor_no+48);
+    	strcat(code_sent,motor);
+    	strcat(code_sent,v1);
+    	strcat(code_sent,v2);
+    	strcat(code_sent,"1");
+    }
+        //code_sent="C"+string(motor_no)+ascii(48+pin_no_1)+ascii(48+pin_no_2)+"1";    //adafruit
+    else if(driver_type==2)
+    {
+    	sprintf(v1,"%c",pin_no_1+48);
+    	sprintf(v2,"%c",pin_no_2+48);
+    	sprintf(motor,"%c",motor_no+48);
+    	strcat(code_sent,motor);
+    	strcat(code_sent,v1);
+    	strcat(code_sent,v2);
+    	strcat(code_sent,"1");
+    }    
+        //code_sent="C"+string(motor_no)+ascii(48+pin_no_1)+ascii(48+pin_no_2)+"1";   //code pour initialiser L298
+    else if(driver_type==3)
+    {
+    	sprintf(v1,"%c",pin_no_1+48);
+    	sprintf(v2,"%c",pin_no_2+48);
+    	sprintf(motor,"%c",motor_no+48);
+    	strcat(code_sent,motor);
+    	strcat(code_sent,v1);
+    	strcat(code_sent,v2);
+    	strcat(code_sent,"0");
+
+    }
+    //    code_sent="C"+string(motor_no)+ascii(48+pin_no_1)+ascii(48+pin_no_2)+"0";   //code pour initialiser L293
+    //printf("%s\n",code_sent);
+    wr=write_serial(h,code_sent,5);
+
+    int stat;
+    int num_bytes[2];
+    //Attente que l'arduino reponde OK
+    stat=status_serial(1,num_bytes);
+    while (num_bytes[0] < 2) 
+        stat=status_serial(1,num_bytes);
+    char values[5];
+    int dcm_rd=read_serial(1,values,2);
+    if (dcm_rd == 0)
+        printf("Init motor successful\n");
+    else
+        printf("Init motor unsuccessful\n");
+}
+
+void cmd_dcmotor_run(int h,int motor_no,int u1)
+{
+	char code_dir[2];
+	char code_sent[10]="M";
+	char motor[2];
+    int val;
+	char v[2];
+    if(u1>=0)
+       	sprintf(code_dir,"%c",49);
+              //code_dir=ascii(49);
+    else
+        sprintf(code_dir,"%c",48);
+              //code_dir=ascii(48);
+    if(abs(u1)>255)
+        val=255;
+    else
+        val=abs(ceil(u1));
+    //printf("%d\n",val);
+    sprintf(motor,"%c",motor_no+48);
+    sprintf(v,"%c",val);
+    strcat(code_sent,motor);
+    strcat(code_sent,code_dir);
+    strcat(code_sent,v);
+    //printf("%s\n",code_sent);      
+          //code_sent="M"+ascii(48+motor_no)+code_dir+ascii(val);
+    int wr=write_serial(h,code_sent,4);
+}
+
+void cmd_dcmotor_release(int h,int motor_no)
+{
+	char code_sent[6]="M";
+	char motor[2];
+	char v[2];
+	sprintf(motor,"%c",motor_no+48);
+	sprintf(v,"%c",0);
+    strcat(code_sent,motor);
+    strcat(code_sent,"1");
+    strcat(code_sent,v);
+	//code_sent="M"+ascii(48+motor_no)+"1"+ascii(0);
+    //printf("%s\n",code_sent);
+    int wr=write_serial(h,code_sent,4);
+    strcpy(code_sent,"M");  
+    strcat(code_sent,motor);
+    strcat(code_sent,"r");
+    //printf("%s\n",code_sent);
+    //code_sent="M"+ascii(48+motor_no)+"r";
+    wr=write_serial(h,code_sent,3);
+}