diff --git a/Code/Temperatur_und_Abstandsmesung/Temperatur_und_Abstandsmessung/Temperatur_und_Abstandsmessung.ino b/Code/Temperatur_und_Abstandsmesung/Temperatur_und_Abstandsmessung/Temperatur_und_Abstandsmessung.ino new file mode 100644 index 0000000..60cd80e --- /dev/null +++ b/Code/Temperatur_und_Abstandsmesung/Temperatur_und_Abstandsmessung/Temperatur_und_Abstandsmessung.ino @@ -0,0 +1,115 @@ +#include + +#define trig PD3 +#define echo PD4 + +long distance = 0; +int count = 0; + +// ISR For Timer +ISR(TIMER0_COMPA_vect){ + + //set count depending on tcnt0 and the prescaler + if(count == 96){ + count = 0; + TCNT0 = 0; + + digitalWrite(trig, HIGH); + // ... wait for 10 µs ... + + delayMicroseconds(10); + // ... put the trigger down ... + digitalWrite(trig, LOW); + + PCICR |= 0b00000100; + PCMSK2 |= 0b00010000; + + } else { + count++; + } +} + +//ISR for PCINT20 +ISR(PCINT2_vect) { + distance = pulseIn(echo, HIGH); + + PCICR &= ~0b00000100; + PCMSK2 &= ~0b00010000; + delayMicroseconds(10); +} + +long calculateDistance(){ + return (long) (((float) distance/ 58.0)); +} + +int16_t dallas(int x, byte start){ + + OneWire ds(x); + byte i; + byte data[2]; + int16_t result; + + do{ + ds.reset(); + ds.write(0xCC); //skip Command + ds.write(0xBE); //Read 1st 2 bytes of Scratchpad + result = 0; + for(i = 0; i < 2; i++){ + data[i] = ds.read(); + result += data[i]; + } + + result = result/2; + ds.reset(); + ds.write(0xCC); + ds.write(0x44, 1); //start conversion + if(start){ + delay(1000); + } + } while(start--); + return result; +} + +void initS(){ + + TCNT0 = 0; // reset timer + TCCR0A |= 0b00000010; + TCCR0B |= 0b0000011; //set prescaler to 64 becasue 256 does not work + OCR0A = 255; + TIMSK0 |= 0b00000010; + sei(); +} + +void setup(){ + Serial.begin(9600); + dallas(2, 1); + + // Initializing Trigger Output and Echo Input + pinMode(trig, OUTPUT); + pinMode(echo, INPUT); + + // Reset the trigger pin and wait a half a second + digitalWrite(trig, LOW); + delayMicroseconds(500); + + initS(); +} + +void loop(){ + + long run_time = micros(); + + Serial.print("Temperatur: "); + Serial.println(dallas(2, 0)); + + Serial.print("Distanz: "); + Serial.print(calculateDistance()); + Serial.println("cm"); + + run_time = micros() - run_time; + Serial.print("Zeit für Durchlauf: "); + Serial.println(run_time); + Serial.println(); + delay(1000); + +} diff --git a/Code/miniRobotRC/fahrsteuerung.ino b/Code/miniRobotRC/fahrsteuerung.ino new file mode 100644 index 0000000..915b3ba --- /dev/null +++ b/Code/miniRobotRC/fahrsteuerung.ino @@ -0,0 +1,53 @@ +void manualDigitalDrive() { + bool goOn = false; + // while(!tasten.getButtonCycle(buttonL1)) { + clearCommands(); + if(!tasten.getAnyPressed()) { + lcd.clear(); + lcd.println("Warte..."); + } + if(tasten.checkButton(buttonB) || tasten.checkButton(buttonUp)) { + pwmA = -215; + pwmB = -255; + lcd.clear(); + lcd.println("geradeaus fahren"); + goOn =true; + } + if(tasten.checkButton(buttonC) || tasten.checkButton(buttonDown)) { + pwmA = 100; + pwmB = 255; + lcd.clear(); + lcd.println("rueckwaerts fahren"); + goOn =true; + } + if(tasten.checkButton(buttonRight)) { + pwmA = -100; //rechter Motor + pwmB = 100; + lcd.clear(); + lcd.println("rechts lenken"); + goOn =true; + } + if(tasten.checkButton(buttonLeft)) { + pwmB = -100; + pwmA = 100; + lcd.clear(); + lcd.println("links lenken"); + goOn =true; + } + if(goOn) { + commands[0] = speedA; + commands[1] = highByte(pwmA); + commands[2] = lowByte(pwmA); + commands[3] = speedB; + commands[4] = highByte(pwmB); + commands[5] = lowByte(pwmB); + commands[6] = timeToDrive; + commands[7] = highByte(driveTimeout); + commands[8] = lowByte(driveTimeout); + commands[9] = goDrive; + radio.write(&commands, sizeof(commands)); + goOn = false; + } + // } + // tasten.clearAllButtons(); +} diff --git a/Code/miniRobotRC/interruptRoutinen.ino b/Code/miniRobotRC/interruptRoutinen.ino new file mode 100644 index 0000000..f3a02de --- /dev/null +++ b/Code/miniRobotRC/interruptRoutinen.ino @@ -0,0 +1,5 @@ +//HIER KOMMT ALLES REIN WAS 1 MAL PRO ms AUFGERUFEN WERDEN SOLL!!! +ISR(TIMER2_COMPA_vect) { + tasten.checkButtons(); +} +//ALLE anderen ISR kommen HIER drunter!!! diff --git a/Code/miniRobotRC/joystick.ino b/Code/miniRobotRC/joystick.ino new file mode 100644 index 0000000..84f2a0e --- /dev/null +++ b/Code/miniRobotRC/joystick.ino @@ -0,0 +1,123 @@ +#define xPin A5 +#define yPin A6 +#define leftMax 255//215 +#define leftMin -255//-100 +#define rightMax 255 +#define rightMin -255 + +volatile int16_t xValue = 0; +volatile int16_t yValue = 0; +volatile int16_t leftPWM = 0; +volatile int16_t rightPWM = 0; + +const int16_t deadZone = 10; +/* +void setup() { + Serial.begin(115200); +} + +void loop() { + koordinaten(analogRead(xPin), analogRead(yPin)); + motorPWM(); + Serial.print("X: "); + Serial.println(xValue); + Serial.print("Y: "); + Serial.println(yValue); + Serial.print("links: "); + Serial.println(leftPWM); + Serial.print("rechts: "); + Serial.println(rightPWM); + + delay(200); + +} +*/ +void motorMapping() { + static long temp = millis(); + koordinaten(analogRead(xPin), analogRead(yPin)); + motorPWM(); + + pwmB = map(leftPWM, -255,255,leftMin,leftMax); + pwmA = map(rightPWM, -255,255,rightMin,rightMax); + if((millis() - temp) > 100) { + lcd.clear(); + lcd.println("Links: "); + lcd.println(pwmB); + lcd.gotoXY(0,2); + lcd.println("Rechts: "); + lcd.println(pwmA); + } + + senden(); +} + +void senden() { + commands[0] = speedA; + commands[1] = highByte(pwmA); + commands[2] = lowByte(pwmA); + commands[3] = speedB; + commands[4] = highByte(pwmB); + commands[5] = lowByte(pwmB); + commands[6] = timeToDrive; + commands[7] = highByte(driveTimeout); + commands[8] = lowByte(driveTimeout); + commands[9] = goDrive; + radio.write(&commands, sizeof(commands)); +} + +void koordinaten(uint16_t x, uint16_t y) { + //9-bit reichen, der ADC schafft bestenfalls 8-bit praezision + x = x >> 1; + y = y >> 1; + xValue = map(x, 0, 511, -255, 255); + yValue = map(y, 0, 511, 255, -255); +} + +void motorPWM() { + if((abs(xValue) > deadZone) || (abs(yValue) > deadZone)) { + if(yValue >= 0) { + if(xValue >= 0) { + //+y , +x + leftPWM = yValue; + rightPWM = yValue - xValue; + if(xValue >= yValue) { + leftPWM = 255; + rightPWM = -255; + } + } else { + //+y , -x + leftPWM = yValue; + rightPWM = yValue + xValue; + if(abs(xValue) >= yValue) { + leftPWM = -255; + rightPWM = 255; + } + } + } else { + if(xValue >= 0) { + //-y , +x + leftPWM = yValue; + rightPWM = yValue + xValue; + if(xValue >= abs(yValue)) { + leftPWM = 255; + rightPWM = -255; + } + } else { + //-y , -x + leftPWM = yValue; + rightPWM = yValue - xValue; + if(abs(xValue) >= abs(yValue)) { + leftPWM = -255; + rightPWM = 255; + } + } + } + if(abs(xValue) < deadZone) { + leftPWM = yValue; + rightPWM = yValue; + } + } else { + leftPWM = 0; + rightPWM = 0; + } +}