Revision 2:44e6463e7660, committed 2018-04-02

Comitter:

ss890527

Date:

Mon Apr 02 13:17:44 2018 +0000

Parent:

1:985487f2cb89

Commit message:

cccf

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotMoveControl.lib	Mon Apr 02 13:17:44 2018 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/2017RobotCarNumberOne/code/Robotics_LAB6_Bluetooth_and_UART/#985487f2cb89

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/define.h	Mon Apr 02 13:17:44 2018 +0000
@@ -0,0 +1,30 @@
+void init_IO();void init_TIMER_M();void timerM1_ITR();void init_CN();void CN_ITR();void init_PWM();void M1_RPM_get();void M2_RPM_get();void conquer();
+void init_TIMER();void timer1_ITR();void init_UART();void RX_ITR();void variation();
+//SERVO///////////////////////////////////////////////////
+PwmOut servo_cmd(A0);
+float servo_duty = 0.075;  int servo_angle = 0;//
+//bool servo_clockwise = 1; double servo_speed = 15.0;//deg/s
+
+float SpeedPgain_1 = 0.0065;  
+float SpeedDgain_1 = 0.00000;
+float SpeedIgain_1 = 0.06;
+
+//RX
+int readcount = 0;int RX_flag1 = 0;int RX_flag2 = 0;char getData[6] = {0,0,0,0,0,0};short data_received[3] = {0,0,0};short data_received_old[3] = {0,0,0};
+//DC motor 1//////////////////////////////////////////////
+PwmOut pwm1(D7);PwmOut pwm1n(D11);
+InterruptIn HallA(A1);InterruptIn HallB(A2);
+int HallA_1_state = 0;int HallB_1_state = 0;int state_1 = 0;int state_1_old = 0;
+int speed_count_1 = 0;float rotation_speed_1 = 0.0;
+float output_1 = 0.0;float PI_out_1 = 0.0;float pwm1_duty = 0.5;
+float err_1 = 0.0;float err_old_1 = 0.0;float Ierr_1 = 0.0;float Derr_1 = 0.0;
+float rotation_speed_ref_1 = 0.0;
+// DC motor 2/////////////////////////////////////////////
+PwmOut pwm2(D8);PwmOut pwm2n(A3);
+InterruptIn HallA_2(D13);InterruptIn HallB_2(D12);
+int HallA_2_state = 0;int HallB_2_state = 0;int state_2 = 0;int state_2_old = 0;
+int speed_count_2 = 0;float rotation_speed_2 = 0.0;
+float output_2 = 0.0;float PI_out_2 = 0.0;float pwm2_duty = 0.5;
+float err_2 = 0.0;float err_old_2 = 0.0;float Ierr_2 = 0.0;float Derr_2 = 0.0;
+float SpeedPgain_2 = SpeedPgain_1;float SpeedDgain_2 = SpeedDgain_1;float SpeedIgain_2 = SpeedIgain_1;
+float rotation_speed_ref_2 = 0.0;
\ No newline at end of file

--- a/main.cpp	Fri Apr 07 06:32:01 2017 +0000
+++ b/main.cpp	Mon Apr 02 13:17:44 2018 +0000
@@ -1,90 +1,38 @@
 #include "mbed.h"
-
+#include "define.h"
 //TIMER///////////////////////////////////////////////////
-#define TIMER_CYCLING_TIME 0.2f //s
+#define TIMER_CYCLING_TIME 0.02084f //s
 Ticker timerM1;
-Serial pc(USBTX, USBRX);
-
-int counttimer1 = 0;
-int countmotor1 = 0;
-int countrxi = 0;
-int fucker = 0;
-/////////////////////////////////////
 Ticker timer1;
 Ticker timer2;
-
+Serial pc(USBTX, USBRX);
 Serial bt(D10, D2);  // TXpin, RXpin
 
-//SERVO///////////////////////////////////////////////////
-PwmOut servo_cmd(A0);
-float servo_duty = 0.075;  int angle = 0;//
-bool servo_clockwise = 1; double servo_speed = 15.0;//deg/s
-
-//DC motor parameter setting///////////////////////////////
-float rotation_speed_ref_1 = 0.0;
-float rotation_speed_ref_2 = 0.0;
-float SpeedPgain_1 = 0.002;  
-float SpeedDgain_1 = 0.0000;
-float SpeedIgain_1 = 0.005;
-
-//DC motor 1//////////////////////////////////////////////
-PwmOut pwm1(D7);PwmOut pwm1n(D11);
-InterruptIn HallA(A1);InterruptIn HallB(A2);
-int HallA_1_state = 0;int HallB_1_state = 0;int state_1 = 0;int state_1_old = 0;
-int speed_count_1 = 0;float rotation_speed_1 = 0.0;
-float output_1 = 0.0;float PI_out_1 = 0.0;float pwm1_duty = 0.5;
-float err_1 = 0.0;float err_old_1 = 0.0;float Ierr_1 = 0.0;float Derr_1 = 0.0;
-// DC motor 2/////////////////////////////////////////////
-PwmOut pwm2(D8);PwmOut pwm2n(A3);
-InterruptIn HallA_2(D13);InterruptIn HallB_2(D12);
-int HallA_2_state = 0;int HallB_2_state = 0;int state_2 = 0;int state_2_old = 0;
-int speed_count_2 = 0;float rotation_speed_2 = 0.0;
-float output_2 = 0.0;float PI_out_2 = 0.0;float pwm2_duty = 0.5;
-float err_2 = 0.0;float err_old_2 = 0.0;float Ierr_2 = 0.0;float Derr_2 = 0.0;
-float SpeedPgain_2 = SpeedPgain_1;float SpeedDgain_2 = SpeedDgain_1;float SpeedIgain_2 = SpeedIgain_1;
-// 函式宣告////////////////////////////////////////////////////////////
-void init_IO();void init_TIMER_M();void timerM1_ITR();void init_CN();void CN_ITR();void init_PWM();void M1_RPM_get();void M2_RPM_get();void conquer();
-////////////////////////////////////
-//RX
-int readcount = 0;
-int RX_flag1 = 0;
-int RX_flag2 = 0;
-char getData[6] = {0,0,0,0,0,0};
-short data_received[3] = {0,0,0};
-short data_received_old[3] = {0,0,0};
-
-//函式宣告
-void init_TIMER();
-void timer1_ITR();
-void init_UART();
-void RX_ITR();
-
-void variation();
-
 int main()
 {
     init_TIMER_M();
     init_PWM();
     init_CN();
-    timer2.attach(&variation,0.5);
-    
+    timer2.attach(&variation,0.5);    
     init_TIMER();   // old data vs new data
     init_UART();    // baud and attach
+   
     //conquer2();
     while(1) {
-       
-    }
+      pc.printf("M1:%f  1:%f  P:%f I:%f D:%f\r\n", rotation_speed_1,pwm1_duty,SpeedPgain_1*err_1,SpeedIgain_1*Ierr_1,SpeedDgain_1*Derr_1);      
+      wait(0.5);
+      }  // pc.printf("servo:%d 2:%d\r\n",servo_duty,data_received[2]);  wait(0.5);    
 }
 void variation()
 {
     rotation_speed_ref_1 = data_received[0];
     rotation_speed_ref_2 = data_received[1]*-1;
+    servo_angle = data_received[2];
 }
+
 void RX_ITR()
 {
-    countrxi++;
-    while(bt.readable()) {
-        countrxi++;
+    while(bt.readable()) {        
         static char uart_read;
         uart_read = bt.getc();
         if(uart_read == 127 && RX_flag1 == 1) {
@@ -97,29 +45,29 @@
         if(RX_flag2 == 1) {
             getData[readcount] = uart_read;
             readcount++;
-            if(readcount >= 5) {
+            if(readcount >= 6) {
                 readcount = 0;
-                RX_flag2 = 0;
-                
-                //pc.printf("D0:%d  D1:%d D2:%d D3:%d D4:%d D5:%d D6:%d\r\n",getData[0],getData[1],getData[2],getData[3],getData[4],getData[5]); 
+                RX_flag2 = 0;                
                 ///code for decoding///
                 data_received[0] = (getData[2] << 8) | getData[1];
                 data_received[1] = (getData[4] << 8) | getData[3];
-                data_received[2] = getData[5];
-                //pc.printf("D0:%d  D1:%d D2:%d \r\n",data_received[0],data_received[1],data_received[2]); 
-                ///////////////////////
+                data_received[2] = getData[5];                
             }
-        } else if(uart_read == 254 && RX_flag1 == 0) {
+        } else if(uart_read == 254 && RX_flag1 == 0) 
+        {
             RX_flag1 = 1;
         }
-    }
-    
-}
+    }//while    
+}//RX ITR
 
 void init_TIMER()
 {
     timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
 }
+void init_TIMER_M()//
+{
+    timerM1.attach_us(&timerM1_ITR, TIMER_CYCLING_TIME*1000000);
+}
 
 void init_UART()
 {
@@ -128,10 +76,9 @@
 }
 
 void timer1_ITR()
-{
-    counttimer1++;
+{    
     // 避免收到錯誤資料，若超出設定範圍則用上次的資料
-    if(data_received[0]>300 || data_received[0]<-300 || data_received[1]>300 || data_received[1]<-300 || data_received[2]>90 || data_received[0]<-90) {
+    if(data_received[0]>200 || data_received[0]<-200 || data_received[1]>200 || data_received[1]<-200 || data_received[2]>90 || data_received[0]<-90) {
         data_received[0] = data_received_old[0];
         data_received[1] = data_received_old[1];
         data_received[2] = data_received_old[2];
@@ -147,11 +94,6 @@
 }
 
 
-void init_TIMER_M()//
-{//timerM1.attach_us(&timerM1_ITR, TIMER_CYCLING_TIME*1000000);
-    countmotor1++;
-    timerM1.attach_us(&timerM1_ITR, TIMER_CYCLING_TIME*1000000);
-}
 
 void init_PWM()
 {
@@ -195,23 +137,21 @@
 
 
 void timerM1_ITR()
-{   
-    countmotor1++;
-    //rotation_speed_ref_1 = 0;
-    //rotation_speed_ref_1 = data_received[0];  // left motor
-   // rotation_speed_ref_2 = data_received[1];  // right motor
-
+{       
  //servo PWMoutput//////////////////////////////////////////////////////////////////
-    if(servo_clockwise==1)servo_duty+=0.05*servo_speed*TIMER_CYCLING_TIME/90; else {servo_duty-=0.05*servo_speed*TIMER_CYCLING_TIME/90;}        
-    if(servo_duty >= 0.12f){servo_duty = 0.12;servo_clockwise = 0;}else if(servo_duty <= 0.03f){servo_duty = 0.03;servo_clockwise = 1;}
+    //if(servo_clockwise==1)servo_duty+=0.05*servo_speed*TIMER_CYCLING_TIME/90; else {servo_duty-=0.05*servo_speed*TIMER_CYCLING_TIME/90;}   
+    servo_duty = 0.075+(servo_angle)*0.05/90.0;
+    //pc.printf("servo:%f angle:%d\r\n",servo_duty,servo_angle);
+    if(servo_duty >= 0.12f){servo_duty = 0.12;}else if(servo_duty <= 0.03f){servo_duty = 0.03;}
     servo_cmd.write(servo_duty);
     
     // motor1 PDcontrol/////////////////////////////////////////////////////////////// //unit: rpm        29:reduction ratio
     M1_RPM_get();
     ///PID controller //
-     err_1 = rotation_speed_ref_1 - rotation_speed_1;
+    err_1 = rotation_speed_ref_1 - rotation_speed_1;
     Derr_1 = (err_1 - err_old_1)/TIMER_CYCLING_TIME;
-    if(abs(Derr_1)>0.0){Ierr_1 = Ierr_1 + err_1*TIMER_CYCLING_TIME;}
+    //if(abs(Derr_1)>0.0){Ierr_1 = Ierr_1 + err_1*TIMER_CYCLING_TIME;}
+    Ierr_1 = Ierr_1 + err_1*TIMER_CYCLING_TIME;
     err_old_1 = err_1;
     output_1 = SpeedPgain_1 * err_1 + SpeedDgain_1 * Derr_1 + SpeedIgain_1 * Ierr_1;
     //motor1 PWMoutput//////////////////////////////////////////////////////////////////////
@@ -221,8 +161,9 @@
     M2_RPM_get();
     ///PID controller //
     err_2 = rotation_speed_ref_2 - rotation_speed_2;
-    Derr_2 = (err_2 - err_old_2)/TIMER_CYCLING_TIME;//*INVERSE_TIMER_CYCLING_TIME;
-    if(abs(Derr_2)!=0.0){Ierr_2 = Ierr_2 + err_2*TIMER_CYCLING_TIME;}
+    Derr_2 = (err_2 - err_old_2);//TIMER_CYCLING_TIME;//*INVERSE_TIMER_CYCLING_TIME;
+    Ierr_2 = Ierr_2 + err_2*TIMER_CYCLING_TIME;
+    //if(abs(Derr_2)!=0.0){Ierr_2 = Ierr_2 + err_2*TIMER_CYCLING_TIME;}
     err_old_2 = err_2;
     output_2 = SpeedPgain_2 * err_2 + SpeedDgain_2 * Derr_2 + SpeedIgain_2 * Ierr_2;;
     //motor2 PWMoutput//////////////////////////////////////////////////////////////////////