Monthly Archives: December 2014

Robothund – Siste Oppdatering (04.12.2014)

Hei!

Da er vi ferdig med presentasjonen av vårt prosjekt. Vi hadde enkelte problemer med routeren i starten av fremføringen, men utenom det, føler gruppen at presentasjonen ble vel gjennomført. Video, Power Point og demonstrasjon fungerte tilslutt bra sammen! 🙂

Under har vi vedlagt koden vi bruker til å kontrollere roboten. Litt videre nedover har vi også vedlagt både koden/skriptet vi bruker for å starte Gstreamer pipelinen og mottakeren fra Windows.

KODEN TIL KONTROLLERING:

import curses, time #curses library is used to get realtime keypress and time for sleep function
import RPi.GPIO as GPIO
from time import sleep

GPIO.setmode (GPIO.BOARD)

#Motor 1 Venstre drivhjul
#Motor 2 Høyre drivhjul
#Motor 3 Fremre heve/senke
#Motor 4 Bakre heve/senke

Motor1A = 5
Motor1B = 7

Motor2A = 8
Motor2B = 10

Motor3A = 11
Motor3B = 13

Motor4A = 16
Motor4B = 18

#Definere GPIO pinner som output

GPIO.setup(Motor1A, GPIO.OUT)
GPIO.setup(Motor1B, GPIO.OUT)
GPIO.setup(3, GPIO.OUT)

GPIO.setup(Motor2A, GPIO.OUT)
GPIO.setup(Motor2B, GPIO.OUT)
GPIO.setup(12, GPIO.OUT)

GPIO.setup(Motor3A, GPIO.OUT)
GPIO.setup(Motor3B, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)

GPIO.setup(Motor4A, GPIO.OUT)
GPIO.setup(Motor4B, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)

#Sette PWM på enable pinner og frekvens

Motor1E = GPIO.PWM(3, 100)
Motor2E = GPIO.PWM(12, 100)
Motor3E = GPIO.PWM(15, 50)
Motor4E = GPIO.PWM(22, 50)

#Definere duty cycle

fart1 = 20 #Frem/bak
fart2 = 30 #Svinge
fart3 = 40 #Heve/senke

Motor1E.start(fart1)
Motor2E.start(fart1)
Motor3E.start(fart3)
Motor4E.start(fart3)

stdscr = curses.initscr() #initialize the curses object
curses.cbreak() #to get special key characters
stdscr.keypad(1) #for getting values such as KEY_UP

key = ”
while key != ord(‘q’): #press ‘q’ to quit from program
key = stdscr.getch() #get a character from terminal

stdscr.refresh()

#change the motor speed based on key value

if key == curses.KEY_UP : #Kjoere Frem
Motor1E.ChangeDutyCycle(fart1)
GPIO.output(Motor1A, GPIO.LOW)
GPIO.output(Motor1B, GPIO.HIGH)

Motor2E.ChangeDutyCycle(fart1)
GPIO.output(Motor2A, GPIO.LOW)
GPIO.output(Motor2B, GPIO.HIGH)

elif key == curses.KEY_DOWN : #Kjoere bakover
Motor1E.ChangeDutyCycle(fart1)
GPIO.output(Motor1A, GPIO.HIGH)
GPIO.output(Motor1B, GPIO.LOW)

Motor2E.ChangeDutyCycle(fart1)
GPIO.output(Motor2A, GPIO.HIGH)
GPIO.output(Motor2B, GPIO.LOW)

elif key == curses.KEY_LEFT : #Svinge venstre
Motor1E.ChangeDutyCycle(fart2)
GPIO.output(Motor1A, GPIO.HIGH)
GPIO.output(Motor1B, GPIO.LOW)

Motor2E.ChangeDutyCycle(fart2)
GPIO.output(Motor2A, GPIO.LOW)
GPIO.output(Motor2B, GPIO.HIGH)

elif key == curses.KEY_RIGHT : #Svinge hoeyre
Motor1E.ChangeDutyCycle(fart2)
GPIO.output(Motor1A, GPIO.LOW)
GPIO.output(Motor1B, GPIO.HIGH)

Motor2E.ChangeDutyCycle(fart2)
GPIO.output(Motor2A, GPIO.HIGH)
GPIO.output(Motor2B, GPIO.LOW)

elif key == ord(‘e’): #Forhjul opp
Motor3E.ChangeDutyCycle(fart3)
GPIO.output(Motor3A, GPIO.LOW)
GPIO.output(Motor3B, GPIO.HIGH)

elif key == ord(‘d’): #Forhjul ned
Motor3E.ChangeDutyCycle(fart3)
GPIO.output(Motor3A, GPIO.HIGH)
GPIO.output(Motor3B, GPIO.LOW)

elif key == ord(‘r’): #Bakhjul opp
Motor4E.ChangeDutyCycle(fart3)
GPIO.output(Motor4A, GPIO.HIGH)
GPIO.output(Motor4B, GPIO.LOW)

elif key == ord(‘f’): #Bakhjul ned
Motor4E.ChangeDutyCycle(fart3)
GPIO.output(Motor4A, GPIO.LOW)
GPIO.output(Motor4B, GPIO.HIGH)

sleep(0.040) #pause for 40 ms

GPIO.output(Motor1A, GPIO.LOW)
GPIO.output(Motor1B, GPIO.LOW)

GPIO.output(Motor2A, GPIO.LOW)
GPIO.output(Motor2B, GPIO.LOW)

GPIO.output(Motor3A, GPIO.LOW)
GPIO.output(Motor3B, GPIO.LOW)

GPIO.output(Motor4A, GPIO.LOW)
GPIO.output(Motor4B, GPIO.LOW)

 

Koden til Gstreamer skriptet på Raspberry Pi’en (Pipeline sender):

#!/bin/bash
clear
raspivid -n -t 0 -w 500 -h 500 -fps 30 -b 600000 -o - | gst-launch-1.0 -e -vvvv fdsrc ! h264parse ! rtph264pay pt=96 config-interval=5 ! udpsink host=192.168.1.2 port=5000

Koden til Gstreamer skripet på Windows (Pipeline mottaker):

@echo off
cd C:\gstreamer\1.0\x86_64\bin
gst-launch-1.0 -e -v udpsrc port=5000 ! application/x-rtp, payload=96 ! rtpjitterbuffer ! rtph264depay ! avdec_h264 ! fpsdisplaysink sync=false text-overlay=false

Takk til alle som har fulgt med på bloggen! 🙂
Takk for oss! 😀

Robothund – Tiende Oppdatering (03.12.2014)

Hallo! 

Dette blir da vår siste oppdatering, ettersom at det er fremføring i morgen. i dag har vi fokuser på å få bilen til å se litt penere ut, og å kjøre diverse tester. I tillegg lå mye av fokuset på presentasjonen.

Vi begynte med å stripse og feste enkelte av ledningen som hang fritt. Videre så testet vi at alt fungerte sammen; kamera, kjøring og heising. Alt gikk relativt bra, bortsett fra at hakkinga i tannhjula, ved heising, førte til at bilen slet litt med trappene. Dette kom hovedsakelig av all vekta den har blitt påført for å kunne være trådløs. Ellers så fungerte roboten ganske bra. På bildet under ser du sluttresultatet av Lego roboten, med alt montert på robotkroppen.

Sluttresultat

Da vi følte oss fornøyde med testing, fokuserte vi på presentasjonen, for å gjøre oss klar til i morgen!

Takk til alle som har fulgt med på bloggen! 😀

IMG_0879_2

Everything works!

Over the course of the last few weeks, we have found a solution for focusing the camera and take pictures with the two buttons on the handle, these uses an interrupt function to not disturb the rest of the loop with delays. While working on this we found out we needed two more buttons, but we solved this by using the third button (the one on the joystick), to have 4 functions on 3 buttons. one function if we press joystick + focus button and another function if we press joystick + shutter button.

There were a few problems recently, for example when we put the arduino board in it’s 3d printed case, the programs loop suddenly stopped, this has been fixed by making a new box (better size of the box). another problem we had, were the gyro sending out wrong output, after some troubleshooting we found out it was a wire on the joystick that had broken off.

All of the components has been mounted to the gimbal, and after some tuning, we have a ready product. We shot a video of the gimbal in action. After some video editing and we have a movie ready for the presentation this Thursday. You can see the movie below.

We also made a video of the extra functions we made for the gimbal, it can be seen below.

While working on this project, we found some things that could be improved and some features we would have liked to have.

  • Replace the servos with step motors for smoother and more accurate movements
  • Send our drawings of the 3d printed bearing supports to be manufactured in metal. This is to make the camera shake less, and support higher weights/movements
  • It would also be nice to get the compass function to work properly, instead of our homemade function that works against the gyroscope buildup of yaw values.
  • The two batteries could be changed out with a battery package and a voltage devider, for easy charging.
  • A screen mounted on the gimbal, showing the liveview from the camera.

 

The final Fritzing drawing is shown below.

Fritzing rev5

The final Arduino code is shown below.

  #include "Wire.h"
    #include "I2Cdev.h"
    #include "MPU6050.h"
    #include "Servo.h"
  
  /*This code was made to function with a custom-made Gimball with an attached DSLR-camera. It's made to
     *function with a 9-axis MPU motion-sensor, equipped with a gyroscope, an accelerometer, and a compass.*/
  
  //Set up some pins.
  int joystickVerticalPin = A0;      
  int joystickHorizontalPin = A1;
  int focusButton = 2;
  int shutterButton = 3;
  int focusPin = 4;
  int shutterPin = 5;
  int joyButtonPin = 6;
  
  MPU6050 mpu;                     //The MPU6050-object is a reference to our 9-axis gyroscope.
  
  Servo servoPitch;                //This servo-object refers to the servo responsible for the pitch-angle.
  Servo servoRoll;                 //This servo-object refers to the servo responsible for the roll-angle.
  Servo servoYaw;                  //This servo-object refers to the servo responsible for the yaw-angle.
  
  int pitchPrevious;               //Stores the pitch-angle from the previous loop for reference in the next iteration.
  int rollPrevious;                //Stores the roll-angle from the previous loop for reference in the next iteration.
  int yawPrevious;                 //Stores the yaw-angle from the previous loop for reference in the next iteration.

  const int maxPitch = 140;        //Limits the maximum pitch-angle.  
  const int maxRoll = 140;         //Limits the maximum roll-angle.  
  const int maxYaw = 100;          //Limits the maximum yaw-angle.
  const int minPitch = 58;         //Limits the minimum pitch-angle. 
  const int minRoll = 63;          //Limits the minimum roll-angle. 
  const int minYaw = 10;           //Limits the minimum yaw-angle.
  const int zeroPitch = 105;       //Neutral pitch.
  const int zeroRoll = 102;        //Neutral roll.
  const int zeroYaw = 58;          //Neutral yaw.
  
  boolean yawLocked = false;       //True if the yaw-rotation is deactivated.
  
  const float percentGyro = 0.90;  //The percentage of the gyro-signal that is used in the complimentary filter computations.
  const float percentAcc = 0.10;   //The percentage of the accelerometer-signal that is used in the complimentary filter computations.
    
  float joystickVertical = 0;      //This holds the additional pitch-angle caused by joystick-tilt.
  float joystickHorizontal = 0;    //This holds the additional roll-angle caused by joystick-pan.
  
  unsigned long focusTimer;        //Timer to know when to stop the focus-signal.
  unsigned long shutterTimer;      //Timer to know when to stop the shutter-signal.
  boolean focusing;                //True if the camera is focusing.
  boolean shuttering;              //True if the camera is taking a picture.
  
  const int adjustmentCoefficient = 0.3; //Used for adjusting and tweaking the written servo-angle based on current angle.
  
  unsigned long timer;                   //The timer measures the time between each gyroscope-reading. Used in the Kalman-filter.
  
  
  void setup() 
  {
      Wire.begin();
      Serial.begin(38400);
  
      //Initialize the MPU and attach the servos to their respective pins.
      Serial.println("Initialize MPU");
      mpu.initialize();
      Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
      servoPitch.attach(9);
      servoRoll.attach(10);
      servoYaw.attach(11);
      
      attachInterrupt(0, focus, RISING);
      attachInterrupt(1, shutter, RISING);
      pinMode(focusPin, OUTPUT);
      pinMode(shutterPin, OUTPUT);
      pinMode(focusButton, INPUT);
      pinMode(shutterButton, INPUT);
      pinMode(joyButtonPin, INPUT);
      focusing = false;
      shuttering = false;
      
      //Initialize the coordinates to fit the starting position.
      reset();
  }
  
  
  void loop() 
  {
      //Create an array to store the pitch, roll, and yaw respectively, after they've been read and computed.
      int orientation[3];             
    
      //Get the motion-data, fill the array.
      readCoordinates(orientation);
      
      //Check for user-input via joystick.
      readJoystick();

       //Re-adjust the signal to make up for physical off-set.
      //A positive offset will increase the pitch and vice versa.
      int offsetPitch = orientation[0] - zeroPitch;
      int offsetRoll = orientation[1] - zeroRoll;

      //Calculate the increased value based on current angle.
      orientation[0] = orientation[0] + (offsetPitch * adjustmentCoefficient);
      orientation[1] = orientation[1] + (offsetRoll * adjustmentCoefficient);
      
      //Keep the signals for the servos within the angles we have set.
      //We can use constrain() here instead, most likely.
      orientation[0] = constrain(orientation[0], minPitch, maxPitch);
      orientation[1] = constrain(orientation[1], minRoll, maxRoll);
      orientation[2] = constrain(orientation[2], minYaw, maxYaw);
      
      //Store the current angles for reference in the next loop.
      pitchPrevious = orientation[0];
      rollPrevious = orientation[1];
      
      //Add together the gyroscpoe data and the user-input from the joystick.
      int pitchFinal = orientation[0] + joystickVertical;
      int rollFinal = orientation[1];
      int yawFinal = yawPrevious;
      
      if (yawLocked == false)
      {
        yawPrevious = orientation[2];
      }
      
      yawFinal =+ joystickHorizontal;
      
      //Write the final angles to the servos.
      servoPitch.write(pitchFinal);
      servoRoll.write(rollFinal);
      servoYaw.write(yawFinal);
      
      //Print the values to the monitor (for testing).
      printToMonitor();
      
      //Update the timer.
      timer = micros();
      
      //If the camera is focusing, check its timer.
      //If 0.5 seconds has passed, stop the focusing.
      if (focusing == true && (timer - focusTimer) >= 500000)
      {
         digitalWrite(focusPin, LOW);
         focusing = false;
      }
      
      //If the camera is taking a picture, check the timer.
      //If 0.5 seconds has passed, set the shutter signal to low again.
      if (shuttering == true && (timer - shutterTimer) >= 500000)
      {
         digitalWrite(shutterPin, LOW);
         shuttering = false;
      }
  }
  
  
  
  /*This function reads the values from the 9-axis gyroscope, performs the necessary computations
  to apply the Kamlan-filter to the recieved values, and then maps the final values into the range
  of angles that our servos are compatible for.*/
  void readCoordinates(int orientation[])
  {
      //Some intermediate values to store the raw motion-data.
      int16_t ax, ay, az;
      int16_t gx, gy, gz;
      int16_t cx, cy, cz;
      
      int pitchGyro;                  //Stores the mapped gyro-signals for the pitch-angle.
      int pitchAccel;                 //Stores the mapped accelerator-signals for the pitch-angle.
      int rollGyro;                   //Stores the mapped gyro-signals for the roll-angle.
      int rollAccel;                  //Stores the mapped accelerator-signals for the roll-angle.
      int yawGyro;                    //Stores the mapped gyro-signals for the yaw-angle.         
      int yawAccel;                   //Stores the mapped accelerator-signals for the yaw-angle. -> REMOVE?
      int yawCompass;                 //Stores the mapped compass-signals for the yaw-angle.
    
      //Get gyroscope-, accelerometer-, and compass-data.
      //getMotion9 calls getMotion6, then reads and stores the compass-data.
      mpu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &cx, &cy, &cz);
      pitchAccel = map(ax, -17000, 17000, 0, 179);
      pitchGyro = map(gx, -33000, 33000, 0, 179);
      
      rollAccel = map(ay, -17000, 17000, 179, 0);
      rollGyro = map(gy, -33000, 33000, 179, 0);
      
      //Accellerator does not measure yaw.
      yawGyro = map(gz, -17000, 17000, 0, 179);
      //yawCompass = map(cz, -33000, 33000, 0, 179);
      
      //Apply the complimentary filter.
      //As the accellerometer does not measure the yaw/z-axis movement, we instead utilize the compass data.
      orientation[0] = (percentGyro*(pitchPrevious+(pitchGyro*(double)(micros()-timer)/1000000)))+(percentAcc*pitchAccel);
      orientation[1] = (percentGyro*(rollPrevious+(rollGyro*(double)(micros()-timer)/1000000)))+(percentAcc*rollAccel);
      orientation[2] =  (yawPrevious+(yawGyro*(double)(micros()-timer)/1000000))-1.65;
  }
  
  
  /*Simply reads the position of the joystick and alters the signal to makes sure it corresponds to the physical position.
  Also makes sure the joystick can't steer the servo past its set maximum angle.*/
  void readJoystick()
  {
      //Read the joystick-position and alter it. +/-3 is to make 0 its signal for the resting position.
      // -512 to provide equal +/- numbers. Ignore the signal if the maximum angle has been reached.
      float stickYposition = ((analogRead(joystickVerticalPin) - 512) / -50)*0.080;///5;
      float stickXposition = ((analogRead(joystickHorizontalPin) - 512) / 50)*0.04;///5; 
      
      if ((pitchPrevious + joystickVertical + stickYposition) >= maxPitch)
      {
        stickYposition = 0;
        joystickVertical -= 0.4;
      }
      else if ((pitchPrevious + joystickVertical + stickYposition) <= (minPitch))
      {
        stickYposition = 0;
        joystickVertical += 0.4;
      }
      
      if ((yawPrevious + joystickHorizontal + stickXposition) >= maxYaw)
      {
        stickXposition = 0;
        joystickHorizontal -= 0.4;
      }
      else if ((yawPrevious + joystickHorizontal + stickXposition) <= (minYaw))
      {
        stickXposition = 0;
        joystickHorizontal += 0.4;
      }
      
      //Add the input to the offset caused by the joystick.
      joystickVertical += stickYposition;
      joystickHorizontal += stickXposition;
  }
  
  //Resets the gimbals zero-angle position based on the current one.Might need to use accelerator-data here.
  //Maybe change this and use it for initializing the position on start-up as well.
  void reset()
  {
      joystickVertical = 0;
      joystickHorizontal = 0;
      servoPitch.write(zeroPitch);
      servoRoll.write(zeroRoll);
      servoYaw.write(zeroYaw);
      pitchPrevious = zeroPitch;
      rollPrevious = zeroRoll;
      yawPrevious = zeroYaw;
  }
  
  //Interupt routine for the camera-focus.
  void focus()
  {
    if (digitalRead(joyButtonPin) == HIGH)
    {
      digitalWrite(focusPin, HIGH);
      focusing = true;
      focusTimer = micros();
    }
    else
    {
      reset();
    }
  }
  
  //Interupt routine for the camera-shutter.
  void shutter()
  {
    if (digitalRead(joyButtonPin) == HIGH)
    {
      digitalWrite(shutterPin, HIGH);
      shuttering = true;
      shutterTimer = micros();
    }
    else
    {
      yawLocked = !yawLocked;
    }

  }
  
  //Prints values to the screen. For testing and monitoring.
  void printToMonitor()
  {
      Serial.print("Pitch:");
      Serial.print(pitchPrevious);
      Serial.print("\tRoll:");
      Serial.print(rollPrevious);
      Serial.print("\tYaw:");
      Serial.println(yawPrevious);
  }

Plight (The result)

God kveld bloggen!

Vi begynte ikveld med siste testing før presentasjonen av produktet imorgen. Under kan dere se et par bilder og videoer.  Filmene kan bli litt uklare, da vi har brukt mobil til å filme med.

Her kan dere se noen av resultatene:

test1

 

Blogges!

D-dagen!

Mandag var den store dagen! Presist klokken 13:30, etter gjentatte tester, ble ballongen sluppet opp!
DSC_0023

DSC_0027

Allerede etter fem minutter fikk vi den første SMS-en, så ting så lovende ut allerede på det tidspunktet! Ved hjelp av koordinatene, fulgte vi ballongen litt under én time rundt Kongsberg, før vi kom til Svene, hvor vi snudde og kjørte tilbake til skolen. Ballongen hadde da retning mot og over Jondalen. Omtrent 15:30, et kvarter før vi fikk de siste GPS-variablene, skjønte vi at Vikersund og Modum var vårt neste stopp. Dette stemte godt med den forutsagte ruten, som dere ser til venstre, og den virkelige ruten ballongen hadde, til høyre:

predict-true

Til tross for mørket som kom fort, ble det enighet i gruppen om å ta en tur, slik at vi vet litt mer om terrenget til dagen etter. Etter en god matbit på Subway, satt vi oss inn i golfen til Sondre, som kjørte oss trygt frem til en skog noen få kilometer forbi Vikersund. Vi gikk til den lokasjonen som vi fikk i den siste SMS-en, men på grunn av mørket og skogens tetthet var det umulig å se.

Vi møttes klokken 9 i Hokksund dagen etter. Kledd for skog og fjell, var det klart for Mann mot natur! Kvelden før hadde vi regnet nærmere på hvor ballongen kunne ha landet, og dette viste seg å være avgjørende. For halvannen time senere etter ankomst fant vi ballongen, ca. 500m fra den “opprinnelige” lokasjonen.

Alt av utstyret overlevde, til og med ballongen! Gleden var stor, og det var god stemning i bilen hjemover. Når vi kom hjem, koblet vi opp alt for å se om dataen var lagret på SD-kortet. Alt gikk som det skulle, all dataen var lagret, og alle bildene var på plass!

Alle resultatene finnes også på hjemmesiden vår.

Nå gjenstår det bare å finpusse presentasjonen til torsdag!

For de som er interesserte, ligger Arduino-koden her.