NTHU_PME_Robotics_2018 / Mbed 2 deprecated Robotics_Lab_DCMotor

Robotics LAB DCMotor

Dependencies:   mbed

Fork of Robotics_Lab_DCMotor by Robotics Term Project

main.cpp@3:0856a9fa97ec, 2018-03-19 (annotated)

Committer:
cpul5338
Date:
Mon Mar 19 08:20:03 2018 +0000
Revision:
3:0856a9fa97ec
Parent:
2:9caf46a5fe5a 
Robotics LAB DCMotor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cpul5338 3:0856a9fa97ec 1 /* LAB DCMotor */
YCTung 0:0971f0666990 2 #include "mbed.h"
YCTung 0:0971f0666990 3
cpul5338 3:0856a9fa97ec 4 //****************************************************************************** Define
YCTung 0:0971f0666990 5 //The number will be compiled as type "double" in default
YCTung 0:0971f0666990 6 //Add a "f" after the number can make it compiled as type "float"
YCTung 0:0971f0666990 7 #define Ts 0.01f //period of timer1 (s)
cpul5338 3:0856a9fa97ec 8
cpul5338 3:0856a9fa97ec 9 //****************************************************************************** End of Define
YCTung 0:0971f0666990 10
cpul5338 3:0856a9fa97ec 11 //****************************************************************************** I/O
cpul5338 3:0856a9fa97ec 12 //PWM
cpul5338 3:0856a9fa97ec 13 //Dc motor
YCTung 0:0971f0666990 14 PwmOut pwm1(D7);
YCTung 0:0971f0666990 15 PwmOut pwm1n(D11);
YCTung 0:0971f0666990 16 PwmOut pwm2(D8);
YCTung 0:0971f0666990 17 PwmOut pwm2n(A3);
YCTung 0:0971f0666990 18
YCTung 0:0971f0666990 19 //Motor1 sensor
YCTung 0:0971f0666990 20 InterruptIn HallA_1(A1);
YCTung 0:0971f0666990 21 InterruptIn HallB_1(A2);
YCTung 0:0971f0666990 22 //Motor2 sensor
YCTung 0:0971f0666990 23 InterruptIn HallA_2(D13);
YCTung 0:0971f0666990 24 InterruptIn HallB_2(D12);
YCTung 0:0971f0666990 25
cpul5338 3:0856a9fa97ec 26 //LED
cpul5338 3:0856a9fa97ec 27 DigitalOut led1(A4);
cpul5338 3:0856a9fa97ec 28 DigitalOut led2(A5);
YCTung 0:0971f0666990 29
cpul5338 3:0856a9fa97ec 30 //Timer Setting
cpul5338 3:0856a9fa97ec 31 Ticker timer;
cpul5338 3:0856a9fa97ec 32 //****************************************************************************** End of I/O
cpul5338 3:0856a9fa97ec 33
cpul5338 3:0856a9fa97ec 34 //****************************************************************************** Functions
cpul5338 3:0856a9fa97ec 35 void init_timer(void);
cpul5338 3:0856a9fa97ec 36 void init_CN(void);
YCTung 0:0971f0666990 37 void init_PWM(void);
cpul5338 3:0856a9fa97ec 38 void timer_interrupt(void);
cpul5338 3:0856a9fa97ec 39 void CN_interrupt(void);
cpul5338 3:0856a9fa97ec 40 //****************************************************************************** End of Functions
YCTung 0:0971f0666990 41
cpul5338 3:0856a9fa97ec 42 //****************************************************************************** Variables
cpul5338 3:0856a9fa97ec 43 // Servo
cpul5338 3:0856a9fa97ec 44 float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
cpul5338 3:0856a9fa97ec 45 // motor 1
cpul5338 3:0856a9fa97ec 46 int8_t HallA_state_1 = 0;
cpul5338 3:0856a9fa97ec 47 int8_t HallB_state_1 = 0;
cpul5338 3:0856a9fa97ec 48 int8_t motor_state_1 = 0;
cpul5338 3:0856a9fa97ec 49 int8_t motor_state_old_1 = 0;
cpul5338 3:0856a9fa97ec 50 int count_1 = 0;
cpul5338 3:0856a9fa97ec 51 float speed_1 = 0.0f;
cpul5338 3:0856a9fa97ec 52 float v_ref_1 = 80.0f;
cpul5338 3:0856a9fa97ec 53 float v_err_1 = 0.0f;
cpul5338 3:0856a9fa97ec 54 float v_ierr_1 = 0.0f;
cpul5338 3:0856a9fa97ec 55 float ctrl_output_1 = 0.0f;
cpul5338 3:0856a9fa97ec 56 float pwm1_duty = 0.0f;
cpul5338 3:0856a9fa97ec 57 //motor 2
cpul5338 3:0856a9fa97ec 58 int8_t HallA_state_2 = 0;
cpul5338 3:0856a9fa97ec 59 int8_t HallB_state_2 = 0;
cpul5338 3:0856a9fa97ec 60 int8_t motor_state_2 = 0;
cpul5338 3:0856a9fa97ec 61 int8_t motor_state_old_2 = 0;
cpul5338 3:0856a9fa97ec 62 int count_2 = 0;
cpul5338 3:0856a9fa97ec 63 float speed_2 = 0.0f;
cpul5338 3:0856a9fa97ec 64 float v_ref_2 = 150.0f;
cpul5338 3:0856a9fa97ec 65 float v_err_2 = 0.0f;
cpul5338 3:0856a9fa97ec 66 float v_ierr_2 = 0.0f;
cpul5338 3:0856a9fa97ec 67 float ctrl_output_2 = 0.0f;
cpul5338 3:0856a9fa97ec 68 float pwm2_duty = 0.0f;
cpul5338 3:0856a9fa97ec 69 //****************************************************************************** End of Variables
YCTung 0:0971f0666990 70
cpul5338 3:0856a9fa97ec 71 //****************************************************************************** Main
cpul5338 3:0856a9fa97ec 72 int main()
cpul5338 3:0856a9fa97ec 73 {
cpul5338 3:0856a9fa97ec 74 init_timer();
YCTung 0:0971f0666990 75 init_PWM();
YCTung 0:0971f0666990 76 init_CN();
cpul5338 3:0856a9fa97ec 77 while(1)
roger5641 2:9caf46a5fe5a 78 {
YCTung 0:0971f0666990 79 }
YCTung 0:0971f0666990 80 }
cpul5338 3:0856a9fa97ec 81 //****************************************************************************** End of Main
YCTung 0:0971f0666990 82
cpul5338 3:0856a9fa97ec 83 //****************************************************************************** timer_interrupt
cpul5338 3:0856a9fa97ec 84 void timer_interrupt()
cpul5338 3:0856a9fa97ec 85 {
cpul5338 3:0856a9fa97ec 86 // Motor1
cpul5338 3:0856a9fa97ec 87 speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
cpul5338 3:0856a9fa97ec 88 count_1 = 0;
cpul5338 3:0856a9fa97ec 89 // Code for PI controller //
YCTung 0:0971f0666990 90
cpul5338 3:0856a9fa97ec 91 ///////////////////////////
YCTung 0:0971f0666990 92
cpul5338 3:0856a9fa97ec 93 if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
cpul5338 3:0856a9fa97ec 94 else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
cpul5338 3:0856a9fa97ec 95 pwm1_duty = ctrl_output_1 + 0.5f;
cpul5338 3:0856a9fa97ec 96 pwm1.write(pwm1_duty);
YCTung 0:0971f0666990 97 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 98
cpul5338 3:0856a9fa97ec 99 // Motor2
cpul5338 3:0856a9fa97ec 100 speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
cpul5338 3:0856a9fa97ec 101 count_2 = 0;
cpul5338 3:0856a9fa97ec 102 // Code for PI controller //
YCTung 0:0971f0666990 103
cpul5338 3:0856a9fa97ec 104 ///////////////////////////
roger5641 2:9caf46a5fe5a 105
cpul5338 3:0856a9fa97ec 106 if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
cpul5338 3:0856a9fa97ec 107 else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
cpul5338 3:0856a9fa97ec 108 pwm2_duty = ctrl_output_2 + 0.5f;
cpul5338 3:0856a9fa97ec 109 pwm2.write(pwm2_duty);
cpul5338 3:0856a9fa97ec 110 TIM1->CCER |= 0x40;
YCTung 0:0971f0666990 111 }
cpul5338 3:0856a9fa97ec 112 //****************************************************************************** End of timer_interrupt
YCTung 0:0971f0666990 113
cpul5338 3:0856a9fa97ec 114 //****************************************************************************** CN_interrupt
cpul5338 3:0856a9fa97ec 115 void CN_interrupt()
YCTung 0:0971f0666990 116 {
cpul5338 3:0856a9fa97ec 117 // Motor1
cpul5338 3:0856a9fa97ec 118 // Read the current status of hall sensor
cpul5338 3:0856a9fa97ec 119 HallA_state_1 = HallA_1.read();
cpul5338 3:0856a9fa97ec 120 HallB_state_1 = HallB_1.read();
cpul5338 3:0856a9fa97ec 121
cpul5338 3:0856a9fa97ec 122 ///code for state determination///
YCTung 0:0971f0666990 123
YCTung 0:0971f0666990 124
YCTung 0:0971f0666990 125 //////////////////////////////////
YCTung 0:0971f0666990 126
YCTung 0:0971f0666990 127 //Forward
YCTung 0:0971f0666990 128 //v1Count +1
YCTung 0:0971f0666990 129 //Inverse
YCTung 0:0971f0666990 130 //v1Count -1
cpul5338 3:0856a9fa97ec 131
cpul5338 3:0856a9fa97ec 132 // Motor2
cpul5338 3:0856a9fa97ec 133 // Read the current status of hall sensor
cpul5338 3:0856a9fa97ec 134 HallA_state_2 = HallA_2.read();
cpul5338 3:0856a9fa97ec 135 HallB_state_2 = HallB_2.read();
cpul5338 3:0856a9fa97ec 136
YCTung 0:0971f0666990 137 ///code for state determination///
YCTung 0:0971f0666990 138
YCTung 0:0971f0666990 139
YCTung 0:0971f0666990 140 //////////////////////////////////
YCTung 0:0971f0666990 141
YCTung 0:0971f0666990 142 //Forward
YCTung 0:0971f0666990 143 //v2Count +1
YCTung 0:0971f0666990 144 //Inverse
YCTung 0:0971f0666990 145 //v2Count -1
YCTung 0:0971f0666990 146 }
cpul5338 3:0856a9fa97ec 147 //****************************************************************************** End of CN_interrupt
YCTung 0:0971f0666990 148
cpul5338 3:0856a9fa97ec 149 //****************************************************************************** init_timer
cpul5338 3:0856a9fa97ec 150 void init_timer()
YCTung 0:0971f0666990 151 {
cpul5338 3:0856a9fa97ec 152 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
cpul5338 3:0856a9fa97ec 153 }
cpul5338 3:0856a9fa97ec 154 //****************************************************************************** End of init_timer
cpul5338 3:0856a9fa97ec 155
cpul5338 3:0856a9fa97ec 156 //****************************************************************************** init_PWM
cpul5338 3:0856a9fa97ec 157 void init_PWM()
YCTung 0:0971f0666990 158 {
YCTung 0:0971f0666990 159 pwm1.period_us(50);
YCTung 0:0971f0666990 160 pwm1.write(0.5);
YCTung 0:0971f0666990 161 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 162
YCTung 0:0971f0666990 163 pwm2.period_us(50);
YCTung 0:0971f0666990 164 pwm2.write(0.5);
YCTung 0:0971f0666990 165 TIM1->CCER |= 0x40;
YCTung 0:0971f0666990 166 }
cpul5338 3:0856a9fa97ec 167 //****************************************************************************** End of init_PWM
YCTung 0:0971f0666990 168
cpul5338 3:0856a9fa97ec 169 //****************************************************************************** init_CN
cpul5338 3:0856a9fa97ec 170 void init_CN()
YCTung 0:0971f0666990 171 {
cpul5338 3:0856a9fa97ec 172 // Motor1
YCTung 0:0971f0666990 173 HallA_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 174 HallA_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 175 HallB_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 176 HallB_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 177
cpul5338 3:0856a9fa97ec 178 HallA_state_1 = HallA_1.read();
cpul5338 3:0856a9fa97ec 179 HallB_state_1 = HallB_1.read();
cpul5338 3:0856a9fa97ec 180
cpul5338 3:0856a9fa97ec 181 // Motor2
YCTung 0:0971f0666990 182 HallA_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 183 HallA_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 184 HallB_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 185 HallB_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 186
cpul5338 3:0856a9fa97ec 187 HallA_state_2 = HallA_2.read();
cpul5338 3:0856a9fa97ec 188 HallB_state_2 = HallB_2.read();
roger5641 2:9caf46a5fe5a 189 }
cpul5338 3:0856a9fa97ec 190 //****************************************************************************** End of init_CN