Robotics ^___^
/
Robotics_LAB_UART
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
Robotics_LAB_UART
by 
NTHUPME_Robotics_2017
Revision 1:6228de50cbf4, committed 2017-04-06
- Comitter:
- Andy_Lee
- Date:
- Thu Apr 06 11:07:28 2017 +0000
- Parent:
- 0:d41917b28387
- Commit message:
- done
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d41917b28387 -r 6228de50cbf4 main.cpp
--- a/main.cpp Sat Mar 25 18:06:10 2017 +0000
+++ b/main.cpp Thu Apr 06 11:07:28 2017 +0000
@@ -17,10 +17,79 @@
void init_UART();
void RX_ITR();
+/////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////
+// servo motor
+PwmOut servo_cmd(A0);
+// DC motor
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+// Motor1 sensor
+InterruptIn HallA(A1);
+InterruptIn HallB(A2);
+// Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+
+void init_CN();
+void CN_ITR();
+void init_PWM();
+
+// servo motor
+float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
+// 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
+int angle = 0;
+
+// Hall sensor
+int HallA_1_state = 0;
+int HallB_1_state = 0;
+int state_1 = 0;
+int state_1_old = 0;
+int HallA_2_state = 0;
+int HallB_2_state = 0;
+int state_2 = 0;
+int state_2_old = 0;
+int i=0;
+
+// DC motor rotation speed control
+int speed_count_1 = 0;
+float rotation_speed_1 = 0.0;
+float rotation_speed_ref_1 = 0;
+float pwm1_duty = 0.5;
+float PI_out_1 = 0.0;
+float err_1 = 0.0;
+float ierr_1 = 0.0;
+int speed_count_2 = 0;
+float rotation_speed_2 = 0.0;
+float rotation_speed_ref_2 = 0;
+float pwm2_duty = 0.5;
+float PI_out_2 = 0.0;
+float err_2 = 0.0;
+float ierr_2 = 0.0;
+float p_gain=0.015;
+float i_gain=0.003;
+float duty;
+
+float err_1_old=0;
+float err_2_old=0;
+bool servo=1;
+
+
+Serial pc(USBTX,USBRX);
+/////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////
int main()
{
+ pc.baud(9600);
init_TIMER();
init_UART();
+ init_PWM();
+ init_CN();
while(1) {
}
}
@@ -48,6 +117,71 @@
data_received_old[1] = data_received[1];
data_received_old[2] = data_received[2];
}
+
+ /*
+ servo=1;
+ while(servo==1){
+ if(i==0 || i==100 || i==200 || i==300 || i==400 || i==500 || i==600){
+
+ duty=0.113f-0.0000733f*(float)i;
+
+ }
+ //pc.printf("duty= %f ,\n",duty);
+ //servo_cmd.period_ms(20);
+ servo_cmd.write( data_received_old[2]);
+ servo=0;
+ i=i+1;
+ }
+ if(i==601){
+ i=0;
+ }
+
+
+ */
+ /////////////////////////
+
+ if(servo_duty >= 0.113f)servo_duty = 0.113;
+ else if(servo_duty <= 0.025f)servo_duty = 0.025;
+ servo_cmd.write(servo_duty);
+
+ // motor1
+ rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
+ speed_count_1 = 0;
+
+ ///PI controller for motor1///
+
+ err_1=(data_received_old[0]-rotation_speed_1)*p_gain;
+ ierr_1=(err_1_old+err_1)*i_gain;
+ PI_out_1=err_1+ierr_1;
+ err_1_old=err_1;
+ //////////////////////////////
+
+ if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
+ else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
+ pwm1_duty = PI_out_1 + 0.5f;
+ pwm1.write(pwm1_duty);
+ TIM1->CCER |= 0x4;
+
+ //motor2
+ rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
+ speed_count_2 = 0;
+
+ ///PI controller for motor2///
+
+ err_2=(data_received_old[1]-rotation_speed_2)*p_gain;
+ ierr_2=(err_2_old+err_2)*i_gain;
+ PI_out_2=err_2+ierr_2;
+ err_2_old=err_2;
+ //////////////////////////////
+
+ if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
+ else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
+ pwm2_duty = PI_out_2 + 0.5f;
+ pwm2.write(pwm2_duty);
+ TIM1->CCER |= 0x40;
+
+ //pc.printf("SPEED= %f,/n",data_received_old[1]);
+
}
void RX_ITR()
@@ -64,17 +198,141 @@
if(RX_flag2 == 1) {
getData[readcount] = uart_read;
readcount++;
- if(readcount >= 3) {
+ if(readcount >= 5) {
readcount = 0;
RX_flag2 = 0;
///code for decoding///
- data_received[0] = (getData[2] << 8) | getData[1];
-
-
+ data_received[0] = (getData[2] << 8) | getData[1];
+ data_received[1] = (getData[4] << 8) | getData[3];
+ data_received[2] = getData[5];
///////////////////////
}
- } else if(uart_read == 254 && RX_flag1 == 0) {
+ }
+ else if(uart_read == 254 && RX_flag1 == 0) {
RX_flag1 = 1;
}
}
-}
\ No newline at end of file
+}
+
+///////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////
+void init_PWM()
+{
+ servo_cmd.period_ms(20);
+ servo_cmd.write(servo_duty);
+
+ pwm1.period_us(50);
+ pwm1.write(0.5);
+ TIM1->CCER |= 0x4;
+
+ pwm2.period_us(50);
+ pwm2.write(0.5);
+ TIM1->CCER |= 0x40;
+}
+void init_CN()
+{
+ HallA.rise(&CN_ITR);
+ HallA.fall(&CN_ITR);
+ HallB.rise(&CN_ITR);
+ HallB.fall(&CN_ITR);
+
+ HallA_2.rise(&CN_ITR);
+ HallA_2.fall(&CN_ITR);
+ HallB_2.rise(&CN_ITR);
+ HallB_2.fall(&CN_ITR);
+}
+
+void CN_ITR()
+{
+ // motor1
+ HallA_1_state = HallA.read();
+ HallB_1_state = HallB.read();
+
+ ///code for state determination///
+ if(state_1==1 && state_1_old==1){
+ if(HallA_1_state==1 && HallB_1_state==0){
+ speed_count_1=speed_count_1 + 1;
+ }
+ else if(HallA_1_state==0 && HallB_1_state==1){
+ speed_count_1=speed_count_1 - 1;
+ }
+ }
+ if(state_1==1 && state_1_old==0){
+ if(HallA_1_state==0 && HallB_1_state==0){
+ speed_count_1=speed_count_1 + 1;
+ }
+ else if(HallA_1_state==1 && HallB_1_state==1){
+ speed_count_1=speed_count_1 - 1;
+ }
+ }
+ if(state_1==0 && state_1_old==0){
+ if(HallA_1_state==0 && HallB_1_state==1){
+ speed_count_1=speed_count_1 + 1;
+ }
+ else if(HallA_1_state==1 && HallB_1_state==0){
+ speed_count_1=speed_count_1 - 1;
+ }
+ }
+ if(state_1==0 && state_1_old==1){
+ if(HallA_1_state==1 && HallB_1_state==1){
+ speed_count_1=speed_count_1 + 1;
+ }
+ else if(HallA_1_state==0 && HallB_1_state==0){
+ speed_count_1=speed_count_1 - 1;
+ }
+ }
+ state_1=HallA_1_state;
+ state_1_old=HallB_1_state;
+ //////////////////////////////////
+
+ //forward : speed_count_1 + 1
+ //backward : speed_count_1 - 1
+
+
+ // motor2
+ HallA_2_state = HallA_2.read();
+ HallB_2_state = HallB_2.read();
+
+ ///code for state determination///
+ if(state_2==1 && state_2_old==1){
+ if(HallA_2_state==1 && HallB_2_state==0){
+ speed_count_2=speed_count_2 + 1;
+ }
+ else if(HallA_2_state==0 && HallB_2_state==1){
+ speed_count_2=speed_count_2 - 1;
+ }
+ }
+ if(state_2==1 && state_2_old==0){
+ if(HallA_2_state==0 && HallB_2_state==0){
+ speed_count_2=speed_count_2 + 1;
+ }
+ else if(HallA_2_state==1 && HallB_2_state==1){
+ speed_count_2=speed_count_2 - 1;
+ }
+ }
+ if(state_2==0 && state_2_old==0){
+ if(HallA_2_state==0 && HallB_2_state==1){
+ speed_count_2=speed_count_2 + 1;
+ }
+ else if(HallA_2_state==1 && HallB_2_state==0){
+ speed_count_2=speed_count_2 - 1;
+ }
+ }
+ if(state_2==0 && state_2_old==1){
+ if(HallA_2_state==1 && HallB_2_state==1){
+ speed_count_2=speed_count_2 + 1;
+ }
+ else if(HallA_2_state==0 && HallB_2_state==0){
+ speed_count_2=speed_count_2 - 1;
+ }
+ }
+ state_2=HallA_2_state;
+ state_2_old=HallB_2_state;
+
+
+ //////////////////////////////////
+
+ //forward : speed_count_2 + 1
+ //backward : speed_count_2 - 1
+}