#include "reg51.h"
#include <intrins.h>
#define uchar unsigned char
#define uint unsigned int
#define PULSES_PER_ROUND 1
#define Kt 0.01f
#define ACS712_SENSITIVITY 0.185f
#define LCD_DB_PORT P0
#define ADC_DATA_PORT P0
sbit mo_flyback = P2^0;
sbit IN1 = P1^0;
sbit IN2 = P1^1;
sbit ENA = P1^2;
sbit IN3 = P1^3;
sbit IN4 = P1^4;
sbit ENB = P1^5;
sbit encoder = P3^2;
sbit ADC_ALE = P3^0;
sbit ADC_START = P3^1;
sbit ADC_EOC = P3^3;
sbit ADC_OE = P3^5;
sbit ADC_CLOCK = P3^4;
sbit LCD_RS = P2^3;
sbit LCD_RW = P2^4;
sbit LCD_E = P2^5;
uchar count = 0;
char speed = 90;
volatile uint encoder_count = 0;
uchar state = 0;
uint cycle_count = 0;
uint second_count = 0;
uint adc_sample_interval = 0;
float current = 0.0f;
float torque = 0.0f;
uchar adc_current_val = 0;
void adc_init(void);
uchar adc_read(void);
void calculate_torque(void);
void delay_ms(uint t) {
uint i, j;
for(i = 0; i < t; i++)
for(j = 0; j < 240; j++);
}
void delay_us(uchar t) {
while(t--);
}
void timer1_isr(void) interrupt 3 {
TH1 = 0xFF;
TL1 = 0xFF;
ADC_CLOCK = !ADC_CLOCK;
}
uchar adc_read(void) {
uchar adc_value;
uint timeout;
ADC_ALE = 1;
delay_us(2);
ADC_ALE = 0;
ADC_START = 1;
delay_us(2);
ADC_START = 0;
timeout=0;
while(ADC_EOC == 0){
timeout++;
if(timeout > 20000)
return 0;
};
ADC_OE = 1;
delay_us(1);
adc_value = ADC_DATA_PORT;
ADC_OE = 0;
return adc_value;
}
void calculate_torque(void) {
float voltage;
voltage = (float)adc_current_val * 5.0f / 255.0f;
current = (voltage - 2.5f) / ACS712_SENSITIVITY;
if(current < 0) current = -current;
torque = Kt * current;
}
void adc_init(void) {
TMOD |= 0x10;
TH1 = 0xFF;
TL1 = 0xFF;
ET1 = 1;
TR1 = 1;
}
void init_system(void) {
ENA = 0; IN1 = 0; IN2 = 0;
ENB = 0; IN3 = 0; IN4 = 0;
TMOD |= 0x01;
TH0 = (65536 - 1000) / 256;
TL0 = (65536 - 1000) % 256;
EA=1;
ET0 = 1;
TR0=1;
IT0 = 1;
EX0 = 1;
adc_init();
delay_ms(1000);
}
void timer0_isr(void) interrupt 1 {
TH0 = (65536 - 1000) / 256;
TL0 = (65536 - 1000) % 256;
count = (count + 1) % 100;
if(state == 1) {
IN1 = (count < speed) ? 1 : 0;
IN3 = (count < speed) ? 1 : 0;
IN2 = 0; IN4 = 0;
} else if(state == 2) {
IN2 = (count < speed) ? 1 : 0;
IN4 = (count < speed) ? 1 : 0;
IN1 = 0; IN3 = 0;
} else {
IN1 = 0; IN2 = 0; IN3 = 0; IN4 = 0;
}
cycle_count++;
if(cycle_count >= 1000) {
cycle_count = 0;
second_count++;
}
adc_sample_interval++;
if(adc_sample_interval >= 20) {
adc_sample_interval = 0;
adc_current_val = adc_read();
calculate_torque();
}
}
void encoder_isr(void) interrupt 0 {
encoder_count++;
}
void main()
{
uint forward_target = 5 * PULSES_PER_ROUND;
uint reverse_target = forward_target + 20 * PULSES_PER_ROUND;
init_system();
ENA=0;
IN1=0;
IN2=0;
ENB=0;
IN3=0;
IN4=0;
while(1) {
if(state == 0 && mo_flyback == 0) {
delay_ms(10);
while(mo_flyback == 0);
state = 1;
ENA = 1; ENB = 1;
encoder_count = 0;
}
if(state == 1 && encoder_count >= forward_target) {
ENA = 0;
IN1 = 0;
IN2 = 0;
ENB = 0;
IN3 = 0;
IN4 = 0;
delay_ms(50);
state = 2;
ENA = 1; ENB = 1;
}
if(state == 2 && encoder_count >= reverse_target) {
ENA = 0;
IN1 = 0;
IN2 = 0;
ENB = 0;
IN3= 0;
IN4= 0;
delay_ms(50);
state = 1;
ENA = 1; ENB = 1;
encoder_count = 0;
}
if(second_count > 3600) {
ENA = 0; ENB = 0;
IN1 = 0; IN2 = 0; IN3 = 0; IN4 = 0;
break;
}
delay_ms(10);
}
}
|
