Robotic Team 1
LAB5
Dependencies: mbed ros_lib_indigo
Diff: RemoteCar.cpp
- Revision:
- 0:2d8724cb4e57
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RemoteCar.cpp Thu Apr 19 15:02:33 2018 +0000
@@ -0,0 +1,355 @@
+/* LAB DCMotor */
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Twist.h>
+
+
+//****************************************************************************** Define
+//The number will be compiled as type "double" in default
+//Add a "f" after the number can make it compiled as type "float"
+#define Ts 0.01f //period of timer1 (s)
+#define PI 3.1415926
+//****************************************************************************** End of Define
+
+//****************************************************************************** I/O
+//PWM
+//Dc motor
+PwmOut pwm1(D7);
+PwmOut pwm1n(D11);
+PwmOut pwm2(D8);
+PwmOut pwm2n(A3);
+
+//Motor1 sensor
+InterruptIn HallA_1(A1);
+InterruptIn HallB_1(A2);
+//Motor2 sensor
+InterruptIn HallA_2(D13);
+InterruptIn HallB_2(D12);
+
+//LED
+DigitalOut led1(A4);
+DigitalOut led2(A5);
+
+//Timer Setting
+Ticker timer;
+//****************************************************************************** End of I/O
+
+//****************************************************************************** Functions
+void init_timer(void);
+void init_CN(void);
+void init_PWM(void);
+void timer_interrupt(void);
+void CN_interrupt(void);
+//****************************************************************************** End of Functions
+
+//****************************************************************************** Variables
+// Servo
+float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
+
+// motor 1
+int8_t HallA_state_1 = 0;
+int8_t HallB_state_1 = 0;
+int8_t motor_state_1 = 0;
+int8_t motor_state_old_1 = 0;
+int count_1 = 0;
+float speed_1 = 0.0f;
+float v_ref_1 = 80.0f;
+float v_err_1 = 0.0f; // v_err_old_1 = v_err_1 ; v_err_1 = v_ref_1 - speed_1 ;
+float v_ierr_1 = 0.0f; //integral error : v_ierr_1 = v_err_old_1 + v_err_1;
+float ctrl_output_1 = 0.0f;
+float pwm1_duty = 0.0f;
+
+float Kp_1 = 10; //need to be tested
+float Ki_1 = 0.05; //need to be tested
+//float ctrl_output_old_1 = 0.0f;
+
+
+//motor 2
+int8_t HallA_state_2 = 0;
+int8_t HallB_state_2 = 0;
+int8_t motor_state_2 = 0;
+int8_t motor_state_old_2 = 0;
+int count_2 = 0;
+float speed_2 = 0.0f;
+float v_ref_2 = 150.0f;
+float v_err_2 = 0.0f;
+float v_ierr_2 = 0.0f;
+float ctrl_output_2 = 0.0f;
+float pwm2_duty = 0.0f;
+
+
+float Kp_2 = 10;
+float Ki_2 = 0.05;
+//float ctrl_output_old_2 = 0.0f;
+
+
+//****************************************************************************** End of Variables
+ ros::NodeHandle nh;
+
+ geometry_msgs::Twist vel_msg;
+ ros::Publisher p("feedback_wheel_angularVel", &vel_msg);
+
+
+ void messageCb(const geometry_msgs::Vector3& msg)
+ {
+ //receive velocity cmd from PC to motor
+
+
+ }
+
+ ros:: Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel", messageCb);
+//****************************************************************************** Main
+int main()
+{
+ init_PWM();
+ init_timer();
+ init_CN();
+ nh.initNode();
+ nh.subscribe(s);
+ nh.advertise(p);
+
+
+ while(1)
+ {
+ vel_msg.linear.x = v_ref_1;
+ vel_msg.linear.y = speed_1;
+ vel_msg.linear.z = 0.0f;
+
+ vel_msg.angular.x = v_ref_2;
+ vel_msg.angular.y = speed_2;
+ vel_msg.angular.z = 0.0f;
+
+ p.publish(&vel_msg);
+ nh.spinOnce();
+ wait_ms(1000);
+
+ }
+}
+//****************************************************************************** End of Main
+
+//****************************************************************************** timer_interrupt
+void timer_interrupt()
+{
+ // Motor1
+ speed_1 = (float)count_1 * 100.0f / 12.0f * 2.0f * PI/ 29.0f; //(rad/s) of output wheel (period=0.01 sec, each period has 12 segments, reduction ratio 29)
+ count_1 = 0;
+ // Code for PI controller //
+
+
+ v_err_1 = v_ref_1 - speed_1 ;
+
+ ctrl_output_1 = (Kp_1)*(v_err_1) + (Ki_1)*(v_ierr_1);
+
+ v_ierr_1 = (v_ierr_1) + (v_err_1)*Ts ;
+
+
+
+ ///////////////////////////
+
+ if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
+ else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
+ pwm1_duty = ctrl_output_1 + 0.5f;
+ pwm1.write(pwm1_duty);
+ TIM1->CCER |= 0x4;
+
+
+
+ // Motor2
+ speed_2 = (float)count_2 * 100.0f / 12.0f * 2.0f * PI/ 29.0f; //(rad/s)
+ count_2 = 0;
+ // Code for PI controller //
+
+ v_err_2 = v_ref_2 - speed_2 ;
+
+ ctrl_output_2 = (Kp_2)*(v_err_2) + (Ki_2)*(v_ierr_2);
+
+ v_ierr_2 = (v_ierr_2) + (v_err_2)*Ts ;
+
+
+ ///////////////////////////
+
+ if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
+ else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
+ pwm2_duty = ctrl_output_2 + 0.5f;
+ pwm2.write(pwm2_duty);
+ TIM1->CCER |= 0x40;
+
+}
+//****************************************************************************** End of timer_interrupt
+
+//****************************************************************************** CN_interrupt
+void CN_interrupt()
+{
+ // Motor1
+ // Read the current status of hall sensor
+ HallA_state_1 = HallA_1.read();
+ HallB_state_1 = HallB_1.read();
+
+ ///code for state determination///
+ if(HallA_state_1==0)
+ {
+ if(HallB_state_1==0)
+ {
+ motor_state_1 =1;
+ }
+ else
+ {
+ motor_state_1 =2;
+ }
+ }
+ else
+ {
+ if(HallB_state_1==0)
+ {
+ motor_state_1 = 3;
+ }
+ else
+ {
+ motor_state_1 = 4;
+ }
+ }
+
+
+ //////////////////////////////////
+ switch(motor_state_1)
+ {
+ case 1:
+ if(motor_state_old_1 == 4)
+ count_1--;
+ else if(motor_state_old_1 == 2)
+ count_1++;
+ break;
+ case 2:
+ if(motor_state_old_1 == 1)
+ count_1--;
+ else if(motor_state_old_1 == 3)
+ count_1++;
+ break;
+ case 3:
+ if(motor_state_old_1== 2)
+ count_1--;
+ else if(motor_state_old_1 == 4)
+ count_1++;
+ break;
+ case 4:
+ if(motor_state_old_1 == 3)
+ count_1--;
+ else if(motor_state_old_1 == 1)
+ count_1++;
+ break;
+ }
+ motor_state_old_1 = motor_state_1;
+ //Forward
+ //v1Count +1
+ //Inverse
+ //v1Count -1
+
+ // Motor2
+ // Read the current status of hall sensor
+ HallA_state_2 = HallA_2.read();
+ HallB_state_2 = HallB_2.read();
+
+ ///code for state determination///
+ if(HallA_state_2==0)
+ {
+ if(HallB_state_2==0)
+ {
+ motor_state_2 =1;
+ }
+ else
+ {
+ motor_state_2 =2;
+ }
+ }
+ else
+ {
+ if(HallB_state_2==0)
+ {
+ motor_state_2 = 3;
+ }
+ else
+ {
+ motor_state_2 = 4;
+ }
+ }
+
+ //////////////////////////////////
+ switch(motor_state_2)
+ {
+ case 1:
+ if(motor_state_old_2 == 4)
+ count_2--;
+ else if(motor_state_old_2 == 2)
+ count_2++;
+ break;
+ case 2:
+ if(motor_state_old_2 == 1)
+ count_1--;
+ else if(motor_state_old_2 == 3)
+ count_2++;
+ break;
+ case 3:
+ if(motor_state_old_2== 2)
+ count_2--;
+ else if(motor_state_old_2 == 4)
+ count_2++;
+ break;
+ case 4:
+ if(motor_state_old_2 == 3)
+ count_2--;
+ else if(motor_state_old_2 == 1)
+ count_2++;
+ break;
+ }
+ motor_state_old_2 = motor_state_2;
+
+ //Forward
+ //v2Count +1
+ //Inverse
+ //v2Count -1
+}
+//****************************************************************************** End of CN_interrupt
+
+//****************************************************************************** init_timer
+void init_timer()
+{
+ timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
+}
+//****************************************************************************** End of init_timer
+
+//****************************************************************************** init_PWM
+void init_PWM()
+{
+ pwm1.period_us(50);
+ pwm1.write(0.5);
+ TIM1->CCER |= 0x4;
+
+ pwm2.period_us(50);
+ pwm2.write(0.5);
+ TIM1->CCER |= 0x40;
+}
+//****************************************************************************** End of init_PWM
+
+//****************************************************************************** init_CN
+void init_CN()
+{
+ // Motor1
+ HallA_1.rise(&CN_interrupt);
+ HallA_1.fall(&CN_interrupt);
+ HallB_1.rise(&CN_interrupt);
+ HallB_1.fall(&CN_interrupt);
+
+ HallA_state_1 = HallA_1.read();
+ HallB_state_1 = HallB_1.read();
+
+ // Motor2
+ HallA_2.rise(&CN_interrupt);
+ HallA_2.fall(&CN_interrupt);
+ HallB_2.rise(&CN_interrupt);
+ HallB_2.fall(&CN_interrupt);
+
+ HallA_state_2 = HallA_2.read();
+ HallB_state_2 = HallB_2.read();
+}
+//****************************************************************************** End of init_CN
\ No newline at end of file