kannst du mir sagen was an dem Folgenden Sketch falsch ist ? Eine fehlermeldung bekomme ich nicht aber laufen tut er auch nicht!!!!!
Code: Alles auswählen
// Arduino steuert Fischertechnik
// Versuchs Sketch 01.02.2016
// Bernd Hendrich
#include <Wire.h> // zum ansprechen der I2C:
#include <Adafruit_MotorShield.h> // zum ansprechen des Motor Shields:
#include "utility/Adafruit_PWMServoDriver.h" // zum ansprechen des Motor Shields:
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); // Erzeugt das MotorShield Objekt:
Adafruit_DCMotor *myMotor0 = AFMS.getMotor(1); // Gleichstrom Motor an M1:
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(2); // Gleichstrom Motor an M2:
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(3); // Gleichstrom Motor an M3:
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(4); // Gleichstrom Motor an M4:
int bufferCount; // Anzahl der eingelesenen Zeichen
char buffer[80]; // Serial Input-Buffer
long debounceDelay = 50; // the debounce time; increase if the output flickers
////////////////////////////////////////////////////////////////////////////////
// The default maximum values for the Encoders must be determined for each Robot
// Please be careful and have the finger on the reset button when testing
const int defaultMax[] = { 2286, 3838, 2375, 587};
//
////////////////////////////////////////////////////////////////////////////////
const int latchPin[] = { 30, 28, 26, 24};
const int encdrPin[] = { 21, 20, 19, 18};
int latchState[4];
int lastlState[] = { HIGH, HIGH, HIGH, HIGH};
long lastDTime[] = { 0, 0, 0, 0};
volatile int targetPr[] = { 0, 0, 0, 0};
volatile int maxEncdr[] = { 0, 0, 0, 0};
volatile int encdrPos[] = { 0, 0, 0, 0};
volatile boolean encdrSet[] = { 0, 0, 0, 0};
volatile boolean blocked = false;
volatile boolean calibrate = false;
volatile boolean targetmove = false;
volatile int increment[] = { 1, 1, 1, 1};
boolean printed[] = { false, false, false, false};
void setup() {
Serial.begin(115200); // set up Serial library at 115200 bps
for (int i=0;i<4;i++){
pinMode(encdrPin[i], INPUT);
pinMode(latchPin[i], INPUT);
digitalWrite(encdrPin[i], HIGH);
digitalWrite(latchPin[i], HIGH);
maxEncdr[i] = defaultMax[i];
}
attachInterrupt(2, doE0, FALLING); // interrupt 2 is on pin 21
attachInterrupt(3, doE1, FALLING); // interrupt 3 is on pin 20
attachInterrupt(4, doE2, CHANGE); // interrupt 4 is on pin 19
attachInterrupt(5, doE3, CHANGE); // interrupt 5 is on pin 18
myMotor0->run(RELEASE);
myMotor1->run(RELEASE);
myMotor2->run(RELEASE);
myMotor3->run(RELEASE);
}
void loop() {
char dstr[180];
boolean movefwd[] = { false, false, false, false};
boolean worthit[] = { false, false, false, false};
int moveMin[] = { 10, 10, 10, 5};
int loopcnt = 0;
int calspeed[] = { 65, 135, 125, 85};
int targetPos[] = { 0, 0, 0, 0};
int caldelay = 100;
int setdelay = 10;
double value;
boolean stoploop = true;
boolean done[] = { false, false, false, false};
if (targetmove) {
stoploop = false;
for (int i=0;i<4;i++) {
value = float(targetPr[i])/1000.0;
value = float(maxEncdr[i])*value;
targetPos[i] = int(value);
movefwd[i] = (targetPos[i]>encdrPos[i]);
worthit[i] = (abs(targetPos[i]-encdrPos[i])>moveMin[i]);
done[i] = !worthit[i];
lastlState[i] = HIGH;
}
// due to the inaccurate motor control of the grip, moving the motor 4 is disabled
done[3] = true;
worthit[3] = false;
calspeed[3] = 0;
if (!stoploop){
myMotor0->setSpeed(calspeed[0]);
delay(setdelay);
myMotor1->setSpeed(calspeed[1]);
delay(setdelay);
myMotor2->setSpeed(calspeed[2]);
delay(setdelay);
myMotor3->setSpeed(calspeed[3]);
delay(setdelay);
if (worthit[0]){
if (movefwd[0]) {
myMotor0->run(FORWARD);
increment[0] = 1;
} else {
myMotor0->run(BACKWARD);
increment[0] = -1;
}
delay(setdelay);
}
if (worthit[1]){
Serial.print("Moving motor 2 ");
if (movefwd[1]) {
myMotor1->run(FORWARD);
increment[1] = 1;
Serial.println("forward");
} else {
myMotor1->run(BACKWARD);
increment[1] = -1;
Serial.println("backward");
}
delay(setdelay);
}
if (worthit[2]){
if (movefwd[2]) {
myMotor2->run(FORWARD);
increment[2] = 1;
} else {
myMotor2->run(BACKWARD);
increment[2] = -1;
}
delay(setdelay);
}
if (worthit[3]){
if (movefwd[3]) {
myMotor3->run(FORWARD);
increment[3] = 1;
} else {
myMotor3->run(BACKWARD);
increment[3] = -1;
}
}
delay(caldelay);
}
delay(caldelay);
while (!stoploop){
if (!done[0]){
if (( movefwd[0]) && (encdrPos[0]>=targetPos[0])) {done[0]=true; }
if ((!movefwd[0]) && (encdrPos[0]<=targetPos[0])) {done[0]=true; }
}
if (!done[1]){
if (( movefwd[1]) && (encdrPos[1]>=targetPos[1])) {done[1]=true; }
if ((!movefwd[1]) && (encdrPos[1]<=targetPos[1])) {done[1]=true; }
}
if (!done[2]){
if (( movefwd[2]) && (encdrPos[2]>=targetPos[2])) {done[2]=true; }
if ((!movefwd[2]) && (encdrPos[2]<=targetPos[2])) {done[2]=true; }
}
if (!done[3]){
if (( movefwd[3]) && (encdrPos[3]>=targetPos[3])) {done[3]=true; }
if ((!movefwd[3]) && (encdrPos[3]<=targetPos[3])) {done[3]=true; }
}
if (loopcnt>0) {
checkLatch();
if (!latchState[0] || done[0]) {
myMotor0->setSpeed(0);
myMotor0->run(RELEASE);
if (!latchState[0]) {encdrPos[0]=0;}
done[0] = true;
}
if (!latchState[1] || done[1]) {
myMotor1->setSpeed(0);
myMotor1->run(RELEASE);
if (!latchState[1]) {encdrPos[1]=0;}
done[1] = true;
}
if (!latchState[2] || done[2]) {
myMotor2->setSpeed(0);
myMotor2->run(RELEASE);
if (!latchState[2]) {encdrPos[2]=0;}
done[2] = true;
}
if (!latchState[3] || done[3]) {
myMotor3->setSpeed(0);
myMotor3->run(RELEASE);
if (!latchState[3]) {encdrPos[3]=0;}
done[3] = true;
}
}
loopcnt++;
stoploop = done[0] && done[1] && done[2] && done[3];
}
for (int i=0;i<4;i++) {
sprintf(dstr,"SAFMm%d%6dE",i+1,encdrPos[i]);
Serial.println(dstr);
delay(setdelay);
}
targetmove = false;
delay(setdelay);
myMotor0->setSpeed(0);
delay(setdelay);
myMotor0->run(RELEASE);
delay(setdelay);
myMotor1->setSpeed(0);
delay(setdelay);
myMotor1->run(RELEASE);
delay(setdelay);
myMotor2->setSpeed(0);
delay(setdelay);
myMotor2->run(RELEASE);
delay(setdelay);
myMotor3->setSpeed(0);
delay(setdelay);
myMotor3->run(RELEASE);
delay(setdelay);
}
if (calibrate) {
stoploop = false;
for (int i=0;i<4;i++) {
encdrPos[i] = 0;
increment[i] = 1;
}
myMotor0->setSpeed(calspeed[0]);
delay(caldelay);
myMotor1->setSpeed(calspeed[1]);
delay(caldelay);
myMotor2->setSpeed(calspeed[2]);
delay(caldelay);
myMotor3->setSpeed(calspeed[3]);
delay(caldelay);
myMotor0->run(BACKWARD);
delay(caldelay);
myMotor1->run(BACKWARD);
delay(caldelay);
myMotor2->run(BACKWARD);
delay(caldelay);
myMotor3->run(BACKWARD);
while (!stoploop){
checkLatch();
if (!latchState[0]) {
myMotor0->setSpeed(0);
myMotor0->run(RELEASE);
done[0] = true;
}
if (!latchState[1]) {
myMotor1->setSpeed(0);
myMotor1->run(RELEASE);
done[1] = true;
}
if (!latchState[2]) {
myMotor2->setSpeed(0);
myMotor2->run(RELEASE);
done[2] = true;
}
if (!latchState[3]) {
myMotor3->setSpeed(0);
myMotor3->run(RELEASE);
done[3] = true;
}
stoploop = done[0] && done[1] && done[2] && done[3];
}
/**/
calibrate = false;
delay(caldelay);
myMotor0->setSpeed(0);
delay(caldelay);
myMotor0->run(RELEASE);
delay(caldelay);
myMotor1->setSpeed(0);
delay(caldelay);
myMotor1->run(RELEASE);
delay(caldelay);
myMotor2->setSpeed(0);
delay(caldelay);
myMotor2->run(RELEASE);
delay(caldelay);
myMotor3->setSpeed(0);
delay(caldelay);
myMotor3->run(RELEASE);
delay(caldelay);
sprintf(dstr,"SAFMCal.%6d.%6d.%6d.%6d.E",encdrPos[0],encdrPos[1],encdrPos[2],encdrPos[3]);
Serial.println(dstr);
for (int i=0;i<4;i++) {
maxEncdr[i] = encdrPos[i];
encdrPos[i] = 0;
// motorState[i] = RELEASE;
// motorSpeed[i] = 0;
// lastSpeedM[i] = 0;
// lastStateM[i] = RELEASE;
}
delay(1000);
}
checkLatch();
if (!latchState[0]) {
if (!printed[0]){
myMotor0->setSpeed(0);
myMotor0->run(RELEASE);
sprintf(dstr,"SAFMm1%6dE",encdrPos[0]);
Serial.println(dstr);
encdrPos[0] = 0;
printed[0] = true;
}
} else {
printed[0]=false;
}
if (!latchState[1]) {
if (!printed[1]){
myMotor1->setSpeed(0);
myMotor1->run(RELEASE);
sprintf(dstr,"SAFMm2%6dE",encdrPos[1]);
Serial.println(dstr);
encdrPos[1] = 0;
printed[1] = true;
}
} else {
printed[1]=false;
}
if (!latchState[2]) {
if (!printed[2]){
myMotor2->setSpeed(0);
myMotor2->run(RELEASE);
sprintf(dstr,"SAFMm3%6dE",encdrPos[2]);
Serial.println(dstr);
encdrPos[2] = 0;
printed[2] = true;
}
} else {
printed[2]=false;
}
if (!latchState[3]) {
if (!printed[3]){
myMotor3->setSpeed(0);
myMotor3->run(RELEASE);
sprintf(dstr,"SAFMm4%6dE",encdrPos[3]);
Serial.println(dstr);
encdrPos[3] = 0;
printed[3] = true;
}
} else {
printed[3]=false;
}
}
void checkLatch(){
int reading;
for (int i=0;i<4;i++) {
reading = digitalRead(latchPin[i]);
if (reading != lastlState[i]) {
lastDTime[i] = millis();
}
if ((millis() - lastDTime[i]) > debounceDelay) {
latchState[i] = reading;
}
lastlState[i] = reading;
}
}
void doE0(){
encdrPos[0] += increment[0];
}
void doE1(){
encdrPos[1] += increment[1];
}
void doE2(){
encdrPos[2] += increment[2];
}
void doE3(){
encdrPos[3] += increment[3];
}
void serialEvent(){
char ch = Serial.read();
buffer[bufferCount] = ch;
bufferCount++;
if(ch == 13){
evalSerialData();
}
}
void evalSerialData()
{
int ptr;
int val;
blocked = true;
if ((buffer[0]=='S')&&(buffer[bufferCount-2]=='E')) {
if ((buffer[1]=='A')&&(buffer[2]=='F')&&(buffer[3]=='M')){
if (buffer[4]=='C'){
// calibration run
calibrate = true;
}
// Serial.println(buffer);
if (buffer[4]=='T'){
// SAFMT.vvvv.vvvv.vvvv.vvvvE
// 012345678901234567890123456789
// 1 2
for (int i=0;i<4;i++){
targetPr[i] = (buffer[6+i*5]-48)*1000 + (buffer[7+i*5]-48)*100 + (buffer[8+i*5]-48)*10 + (buffer[9+i*5]-48);
}
targetmove = true;
}
if (buffer[4]=='M'){
// SAFMMiFWDE
// SAFMMiBWDE
// SAFMMiRELE
// SAFMMiSvvvvE
// 01234567890
ptr = (buffer[5]-48)-1;
switch (buffer[6]){
case 'F':
// motorState[ptr] = FORWARD;
increment[ptr] = 1;
switch (ptr){
case 0:
myMotor0->run(FORWARD);
break;
case 1:
myMotor1->run(FORWARD);
break;
case 2:
myMotor2->run(FORWARD);
break;
case 3:
myMotor3->run(FORWARD);
break;
}
break;
case 'B':
// motorState[ptr] = BACKWARD;
increment[ptr] = -1;
switch (ptr){
case 0:
myMotor0->run(BACKWARD);
break;
case 1:
myMotor1->run(BACKWARD);
break;
case 2:
myMotor2->run(BACKWARD);
break;
case 3:
myMotor3->run(BACKWARD);
break;
}
break;
case 'R':
// motorState[ptr] = RELEASE;
switch (ptr){
case 0:
myMotor0->run(RELEASE);
myMotor0->run(RELEASE);
break;
case 1:
myMotor1->run(RELEASE);
myMotor1->run(RELEASE);
break;
case 2:
myMotor2->run(RELEASE);
myMotor2->run(RELEASE);
break;
case 3:
myMotor3->run(RELEASE);
myMotor3->run(RELEASE);
break;
}
break;
case 'S':
val = (buffer[7]-48)*1000 + (buffer[8]-48)*100 + (buffer[9]-48)*10 + (buffer[10]-48);
// motorSpeed[ptr] = val;
switch (ptr){
case 0:
myMotor0->setSpeed(val);
break;
case 1:
myMotor1->setSpeed(val);
break;
case 2:
myMotor2->setSpeed(val);
break;
case 3:
myMotor3->setSpeed(val);
break;
}
break;
}
}
}
}
bufferCount=0;
blocked = false;
//Serial.println("--Arduino--");
}