|
|
尊敬的各位大侠、专家、高手:
我打算采用ArduinoMega2560板子采集11路PWM输入,然后用USB口通过串口协议发送给手机解算后,再发回Arduino,由arduino生成4路PWM波,驱动小车的四个轮子旋转。
现在已经搞了一个初步的代码,其中有5路采用查询方式采集,6路采用外部中断采集。然而,问题是当手机从串口发回数据时会影响Arduino的PWM波采集值,大约有10%的概率采集到的PWM波会突然变小一些,导致采集的误差发生,波动较大。
请问各位尊敬的大侠、专家、高手改如何解决?不胜感激。
- int ppm1 = 2;
- int ppm2 = 3;
- int ppm3 = 18;
- int ppm4 = 19;
- int ppm5 = 20;
- int ppm6 = 21;
- int pwmInput1 = 0;
- int pwmInput2 = 0;
- int pwmInput3 = 0;
- int pwmInput4 = 0;
- int pwmInput5 = 0;
- unsigned long rc1_PulseStartTicks,rc2_PulseStartTicks,rc3_PulseStartTicks,rc4_PulseStartTicks,rc5_PulseStartTicks,rc6_PulseStartTicks;
- volatile int rc1_val, rc2_val,rc3_val, rc4_val,rc5_val,rc6_val;
- float startTime = 0;
- void setup() {
- Serial.begin(115200);
-
-
- ////PPM inputs from RC receiver
- pinMode(ppm1, INPUT);
- pinMode(ppm2, INPUT);
- pinMode(ppm3, INPUT);
- pinMode(ppm4, INPUT);
- pinMode(ppm5, INPUT);
-
- // 电平变化即触发中断
- attachInterrupt(0, rc1, CHANGE);
- attachInterrupt(1, rc2, CHANGE);
- attachInterrupt(5, rc3, CHANGE);
- attachInterrupt(4, rc4, CHANGE);
- attachInterrupt(3, rc5, CHANGE);
- attachInterrupt(2, rc6, CHANGE);
- pinMode(31,INPUT);
- pinMode(33,INPUT);
- pinMode(35,INPUT);
- pinMode(37,INPUT);
- pinMode(39,INPUT);
- startTime = millis();
- }
-
- void rc1()
- {
- // did the pin change to high or low?
- if (digitalRead(ppm1 ) == HIGH)
- rc1_PulseStartTicks = micros(); // store the current micros() value
- else
- rc1_val = micros() - rc1_PulseStartTicks;
- map(rc1_val,900,2100,0,255);
- }
-
- void rc2()
- {
- // did the pin change to high or low?
- if (digitalRead( ppm2 ) == HIGH)
- rc2_PulseStartTicks = micros();
- else
- rc2_val = micros() - rc2_PulseStartTicks;
- map(rc2_val,900,2100,0,255);
- }
- void rc3()
- {
- // did the pin change to high or low?
- if (digitalRead( ppm3 ) == HIGH)
- rc3_PulseStartTicks = micros();
- else
- rc3_val = micros() - rc3_PulseStartTicks;
- map(rc3_val,900,2100,0,255);
- }
- void rc4()
- {
- // did the pin change to high or low?
- if (digitalRead( ppm4 ) == HIGH)
- rc4_PulseStartTicks = micros();
- else
- rc4_val = micros() - rc4_PulseStartTicks;
- map(rc4_val,900,2100,0,255);
- }
- void rc5()
- {
- // did the pin change to high or low?
- if (digitalRead( ppm5 ) == HIGH)
- rc5_PulseStartTicks = micros();
- else
- rc5_val = micros() - rc5_PulseStartTicks;
- map(rc5_val,900,2100,0,255);
- }
- void rc6()
- {
- // did the pin change to high or low?
- if (digitalRead( ppm6 ) == HIGH)
- rc6_PulseStartTicks = micros();
- else
- rc6_val = micros() - rc6_PulseStartTicks;
- map(rc6_val,900,2100,0,255);
- }
- unsigned long time = 0;
- int loopCounter = 1;
- void loop() {
- int checkSum = 0;
- delayMicroseconds(17500);
- pwmInput1 = pulseIn(31, HIGH);
- delayMicroseconds(500);
- pwmInput2 = pulseIn(33,HIGH);
- delayMicroseconds(500);
- pwmInput3 = pulseIn(35,HIGH);
- delayMicroseconds(500);
- pwmInput4 = pulseIn(37, HIGH);
- delayMicroseconds(500);
- pwmInput5 = pulseIn(39, HIGH);
- delayMicroseconds(500);
- pwmInput1 = map(pwmInput1,900,2100,0,255);
- pwmInput2 = map(pwmInput2,900,2100,0,255);
- pwmInput3 = map(pwmInput3,900,2100,0,255);
- pwmInput4 = map(pwmInput4,900,2100,0,255);
- pwmInput5 = map(pwmInput5,900,2100,0,255);
- checkSum = pwmInput1+pwmInput2+pwmInput3+pwmInput4+pwmInput5+rc1_val+rc2_val+rc3_val+rc4_val+rc5_val+rc6_val;
- checkSum = checkSum%256;
-
- // //Send to Console
- // Serial.print("No. ");
- // Serial.print(loopCounter);
- // Serial.print('\t');
- // Serial.print("Time ");
- // Serial.print((millis()-startTime)/1000);
- // Serial.print('\t');
- // Serial.print(pwmInput1);
- // Serial.print('\t');
- // Serial.print(pwmInput2);
- // Serial.print('\t');
- // Serial.print(pwmInput3);
- // Serial.print('\t');
- // Serial.print(pwmInput4);
- // Serial.print('\t');
- // Serial.print(pwmInput5);
- // Serial.print('\t');
- // Serial.print(rc1_val);
- // Serial.print('\t');
- // Serial.print(rc2_val);
- // Serial.print('\t');
- // Serial.print(rc3_val);
- // Serial.print('\t');
- // Serial.print(rc4_val);
- // Serial.print('\t');
- // Serial.print(rc5_val);
- // Serial.print('\t');
- // Serial.print(rc6_val);
- // Serial.print('\n');
- // loopCounter ++;
-
- //Send to Android
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write((byte)loopCounter);
- Serial.write((byte)pwmInput1);
- Serial.write((byte)pwmInput2);
- Serial.write((byte)pwmInput3);
- Serial.write((byte)pwmInput4);
- Serial.write((byte)pwmInput5);
- Serial.write((byte)rc1_val);
- Serial.write((byte)rc2_val);
- Serial.write((byte)rc3_val);
- Serial.write((byte)rc4_val);
- Serial.write((byte)rc5_val);
- Serial.write((byte)rc6_val);
- Serial.write((byte)checkSum);
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write((byte)loopCounter);
- Serial.write((byte)pwmInput1);
- Serial.write((byte)pwmInput2);
- Serial.write((byte)pwmInput3);
- Serial.write((byte)pwmInput4);
- Serial.write((byte)pwmInput5);
- Serial.write((byte)rc1_val);
- Serial.write((byte)rc2_val);
- Serial.write((byte)rc3_val);
- Serial.write((byte)rc4_val);
- Serial.write((byte)rc5_val);
- Serial.write((byte)rc6_val);
- Serial.write((byte)checkSum);
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write(0xFE);
- Serial.write((byte)loopCounter);
- Serial.write((byte)pwmInput1);
- Serial.write((byte)pwmInput2);
- Serial.write((byte)pwmInput3);
- Serial.write((byte)pwmInput4);
- Serial.write((byte)pwmInput5);
- Serial.write((byte)rc1_val);
- Serial.write((byte)rc2_val);
- Serial.write((byte)rc3_val);
- Serial.write((byte)rc4_val);
- Serial.write((byte)rc5_val);
- Serial.write((byte)rc6_val);
- Serial.write((byte)checkSum);
- loopCounter++;
- if (loopCounter==255)
- loopCounter = 0;
-
-
- }
复制代码 |
|