- Код: Выделить всё
/*
Программа диагностики мотора
Вход:
1 - чтение потенциометра, прекращается вводом любого символа
2 - определение полярности
3 - расчет коэффициентов двигателя
*/
#define INA 8
#define INB 9
#define _PWM 10
#define AT 0
#define CNT 5
#define DEG 300.0
boolean serialIn;
char buf[8];
int pmin,pmax,pot;
int dir;
byte speed_pwm;
long sum;
float a1,a2,k1,k2;
boolean use_pot;
void setup()
{
pmin=(int)(1024.0*((DEG-180.0)/2)/DEG);
pmax=(int)(1024.0*((DEG-180.0)/2+180.0)/DEG);
serialIn=false;
dir=0;
speed_pwm=0;
use_pot=false;
Serial.begin(9600);
}
void loop()
{
int i,k;
if(serialIn){
serialIn=false;
use_pot=false;
switch(buf[0]){
case '1':
use_pot=true;
break;
case '2':
pot=analogRead(AT);
Go(1,255);
delay(132);
Go(0,0);
delay(100);
i=analogRead(AT);
if((i-pot)>1)
Serial.println("OK");
else if((i-pot)<-1)
Serial.println("Change pot polarity");
else
Serial.println("Error");
Serial.print("pot=");Serial.println(i);
break;
case '3':
pot=analogRead(AT);
if(pot>512)
k=-1;
else
k=1;
sum=0;
for(i=0;i<10;i++){
pot=analogRead(AT);
Go(k,255);
delay(33*CNT);
sum+=k*(analogRead(AT)-pot);
Go(0,0);
delay(1000);
k=-k;
}
a1=2.0*(float)(map(sum/10,0,pmax-pmin,0,255))/(float)(CNT*CNT);
pot=analogRead(AT);
// Serial.println(sum/10);
//Serial.println(map(sum/10,0,pmax-pmin,0,255));
if(pot>512)
k=-1;
else
k=1;
sum=0;
for(i=0;i<10;i++){
pot=analogRead(AT);
Go(k,127);
delay(33*CNT);
sum+=k*(analogRead(AT)-pot);
Go(0,0);
delay(1000);
k=-k;
}
a2=2.0*(float)(map(sum/10,0,pmax-pmin,0,255))/(float)(CNT*CNT);
k1=(255.0-127.0)/(a1-a2);
k2=255.0-k1*a1;
Serial.print("K1=");Serial.println(k1);
Serial.print("K2=");Serial.println(k2);
break;
default:
Serial.println("Command error");
}
}
if(use_pot){
Serial.println(analogRead(AT));
delay(100);
}
}
void serialEvent() {
int i=0;
while (Serial.available()) {
// get the new byte:
buf[i++] = (char)Serial.read();
delay(2); //для 9600
}
serialIn=true;
}
void Go(int d,byte s)
{
if(d!=dir){
analogWrite(_PWM,0);
speed_pwm=0;
digitalWrite(INA, LOW);
digitalWrite(INB, LOW);
if(d==1)
digitalWrite(INA, HIGH);
else if(d==-1)
digitalWrite(INB, HIGH);
dir=d;
}
if(s!=speed_pwm){
analogWrite(_PWM,s);
speed_pwm=s;
}
}
Инструкция для использования:
Программа позволяет провести 3 теста:
1. Проверка потенциометра. Побегут циферки значения потенциометра. Удобно, например, выставить пот на середину. Ввести со встроенной консоли (кнопка "Монитор порта" на окне Ардуино) символ '1'. Останавливает этот режим ввод любого символа.
2. Проверка полярности. Потенциометр должен быть уже связан с двигателем. Немного проворачивает двигатель вверх (в сторону подъема платформы). Если двигатель провернулся в обратном направлении - сменить полярность двигателя и провести тест еще раз. Если консоль отвечает "ОК", значит ОК. Если просит сменить полярность потенциометра, значит сменить. Тест проводится вводом символа '2'.
3. Определение коэффициентов двигателя. После ввода '3' двигатель дергается 20 раз и выдает К1 и К2. Холодный двигатель выдает неверные коэффициенты, поэтому тест надо провести раз 5, пока значения не начнут повторяться.
записанный как то, не в обиду, через попу. 