diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..dfdf298 --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ +*.elf +*.hex +build/ +.sconsign.dblite +__pycache__/ +node_modules/ +npm-debug.log +*.sublime* +hokuyo/bin/hokuyo +CMakeCache* +CMakeFiles* +cmake_install* +*calculate_xy +*find_port +*get_distance +*get_distance_intensity +*get_multiecho +*get_multiecho_intensity +*sensor_parameter +*sync_time_stamp +*liburg_c.a \ No newline at end of file diff --git a/C2015_Reglement_FR_final.pdf b/C2015_Reglement_FR_final.pdf new file mode 100644 index 0000000..138648b Binary files /dev/null and b/C2015_Reglement_FR_final.pdf differ diff --git a/E2015_Rules_EU_EN_final.pdf b/E2015_Rules_EU_EN_final.pdf new file mode 100644 index 0000000..fecd22f Binary files /dev/null and b/E2015_Rules_EU_EN_final.pdf differ diff --git a/NetworkSharing b/NetworkSharing new file mode 100644 index 0000000..0e638a3 --- /dev/null +++ b/NetworkSharing @@ -0,0 +1,11 @@ +On host (with internet) : +replace by network access interface (ex : wlan0) +replace by network sharing interface (ex : eth0> + +sudo sysctl net.ipv4.ip_forward=1 +sudo iptables -t nat -A POSTROUTING -o -j MASQUERADE +sudo iptables -A FORWARD -m conntrack --ctstate RELATED,ESTABLISHED -j ACCEPT +sudo iptables -A FORWARD -i -o -j ACCEPT + +On target device : +ip route add default via dev diff --git a/README.md b/README.md index 9a9fd3e..85fd53a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,8 @@ -# coupe17 -Attention la lune, on arrive ! #moonvillage +Code source des robots d'UTCoupe 2015 +======= + +Participation à la Coupe de France de Robotique 2015 dont le thème est : Robomovies + +Structure : + +![alt tag](https://raw.githubusercontent.com/utcoupe/coupe15/master/UTCoupe.png) diff --git a/UTCoupe.png b/UTCoupe.png new file mode 100644 index 0000000..180e653 Binary files /dev/null and b/UTCoupe.png differ diff --git a/arduino/GB/AFMotor.cpp b/arduino/GB/AFMotor.cpp new file mode 100644 index 0000000..4686cfa --- /dev/null +++ b/arduino/GB/AFMotor.cpp @@ -0,0 +1,145 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! +// Adapté pour UTCoupe2011 par Arthur, 19/01/2011 +// Moteurs 1 et 2 correspondent à la carte Rugged + + +#include +#include + + +#include "AFMotor.h" + + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; + +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#else + #error "This chip is not supported!" +#endif + pinMode(11, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#else + #error "This chip is not supported!" +#endif +} + +AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + + switch (num) { + case 1: + pinMode(MOTOR1_DIR, OUTPUT); + digitalWrite(MOTOR1_DIR, LOW); + initPWM1(freq); + break; + case 2: + pinMode(MOTOR2_DIR, OUTPUT); + digitalWrite(MOTOR2_DIR, LOW); + initPWM2(freq); + break; + } +} + +void AF_DCMotor::run(uint8_t cmd) { + uint8_t dirPin; + switch (motornum) { + case 1: + dirPin = MOTOR1_DIR; break; + case 2: + dirPin = MOTOR2_DIR; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + digitalWrite(dirPin, LOW); + break; + case BACKWARD: + digitalWrite(dirPin, HIGH); + break; + case RELEASE: + //TODO + break; + } +} + +void AF_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + } +} + + diff --git a/arduino/GB/AFMotor.h b/arduino/GB/AFMotor.h new file mode 100644 index 0000000..b6c34d5 --- /dev/null +++ b/arduino/GB/AFMotor.h @@ -0,0 +1,46 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +#ifndef _AFMotor_h_ +#define _AFMotor_h_ + +#include +#include + + +#define MOTOR12_64KHZ _BV(CS20) // no prescale +#define MOTOR12_8KHZ _BV(CS21) // divide by 8 +#define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 +#define MOTOR12_1KHZ _BV(CS22) // divide by 64 + +#define MOTOR1_DIR 12 +#define MOTOR2_DIR 13 + +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +#define FORWARD 1 +#define BACKWARD 2 +#define BRAKE 3 +#define RELEASE 4 + + +class AF_DCMotor +{ + public: + AF_DCMotor(uint8_t motornum, uint8_t freq = MOTOR12_8KHZ); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +#endif diff --git a/arduino/GB/GB.ino b/arduino/GB/GB.ino new file mode 100644 index 0000000..384ec1e --- /dev/null +++ b/arduino/GB/GB.ino @@ -0,0 +1 @@ +../asserv/asserv.ino \ No newline at end of file diff --git a/arduino/GB/PID.c b/arduino/GB/PID.c new file mode 100644 index 0000000..13c2176 --- /dev/null +++ b/arduino/GB/PID.c @@ -0,0 +1 @@ +../asserv/PID.c \ No newline at end of file diff --git a/arduino/GB/PID.h b/arduino/GB/PID.h new file mode 100644 index 0000000..74e0274 --- /dev/null +++ b/arduino/GB/PID.h @@ -0,0 +1 @@ +../asserv/PID.h \ No newline at end of file diff --git a/arduino/GB/SConstruct b/arduino/GB/SConstruct new file mode 100644 index 0000000..d6c12f1 --- /dev/null +++ b/arduino/GB/SConstruct @@ -0,0 +1 @@ +../SConstruct \ No newline at end of file diff --git a/arduino/GB/block.c b/arduino/GB/block.c new file mode 100644 index 0000000..659ba90 --- /dev/null +++ b/arduino/GB/block.c @@ -0,0 +1 @@ +../asserv/block.c \ No newline at end of file diff --git a/arduino/GB/block.h b/arduino/GB/block.h new file mode 100644 index 0000000..6dc29bc --- /dev/null +++ b/arduino/GB/block.h @@ -0,0 +1 @@ +../asserv/block.h \ No newline at end of file diff --git a/arduino/GB/compat.cpp b/arduino/GB/compat.cpp new file mode 100644 index 0000000..f62664f --- /dev/null +++ b/arduino/GB/compat.cpp @@ -0,0 +1 @@ +../asserv/compat.cpp \ No newline at end of file diff --git a/arduino/GB/compat.h b/arduino/GB/compat.h new file mode 100644 index 0000000..deba0e6 --- /dev/null +++ b/arduino/GB/compat.h @@ -0,0 +1 @@ +../asserv/compat.h \ No newline at end of file diff --git a/arduino/GB/control.c b/arduino/GB/control.c new file mode 100644 index 0000000..baaef8e --- /dev/null +++ b/arduino/GB/control.c @@ -0,0 +1 @@ +../asserv/control.c \ No newline at end of file diff --git a/arduino/GB/control.h b/arduino/GB/control.h new file mode 100644 index 0000000..2f422d0 --- /dev/null +++ b/arduino/GB/control.h @@ -0,0 +1 @@ +../asserv/control.h \ No newline at end of file diff --git a/arduino/GB/emergency.c b/arduino/GB/emergency.c new file mode 100644 index 0000000..e854380 --- /dev/null +++ b/arduino/GB/emergency.c @@ -0,0 +1 @@ +../asserv/emergency.c \ No newline at end of file diff --git a/arduino/GB/emergency.h b/arduino/GB/emergency.h new file mode 100644 index 0000000..3ccf37b --- /dev/null +++ b/arduino/GB/emergency.h @@ -0,0 +1 @@ +../asserv/emergency.h \ No newline at end of file diff --git a/arduino/GB/encoder.c b/arduino/GB/encoder.c new file mode 100644 index 0000000..9efaf98 --- /dev/null +++ b/arduino/GB/encoder.c @@ -0,0 +1 @@ +../asserv/encoder.c \ No newline at end of file diff --git a/arduino/GB/encoder.h b/arduino/GB/encoder.h new file mode 100644 index 0000000..2f64239 --- /dev/null +++ b/arduino/GB/encoder.h @@ -0,0 +1 @@ +../asserv/encoder.h \ No newline at end of file diff --git a/arduino/GB/goals.c b/arduino/GB/goals.c new file mode 100644 index 0000000..c5ae59e --- /dev/null +++ b/arduino/GB/goals.c @@ -0,0 +1 @@ +../asserv/goals.c \ No newline at end of file diff --git a/arduino/GB/goals.h b/arduino/GB/goals.h new file mode 100644 index 0000000..6df1314 --- /dev/null +++ b/arduino/GB/goals.h @@ -0,0 +1 @@ +../asserv/goals.h \ No newline at end of file diff --git a/arduino/GB/local_math.c b/arduino/GB/local_math.c new file mode 100644 index 0000000..c9e2eb8 --- /dev/null +++ b/arduino/GB/local_math.c @@ -0,0 +1 @@ +../asserv/local_math.c \ No newline at end of file diff --git a/arduino/GB/local_math.h b/arduino/GB/local_math.h new file mode 100644 index 0000000..90358c2 --- /dev/null +++ b/arduino/GB/local_math.h @@ -0,0 +1 @@ +../asserv/local_math.h \ No newline at end of file diff --git a/arduino/GB/make b/arduino/GB/make new file mode 100644 index 0000000..af89f98 --- /dev/null +++ b/arduino/GB/make @@ -0,0 +1,11 @@ +#!/bin/bash + +export AVR_HOME=/usr/bin ARDUINO_HOME=../arduino-1.0 +export ARDUINO_BOARD=mega2560 + +if ! ../check_protocol; then + echo "Found duplicate value for orders in protocol.h" + exit 1 +fi + +scons -f ../SConstruct $@ diff --git a/arduino/GB/motor.cpp b/arduino/GB/motor.cpp new file mode 100644 index 0000000..654c1dd --- /dev/null +++ b/arduino/GB/motor.cpp @@ -0,0 +1,61 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#include "parameters.h" +#include "AFMotor.h" +#include + +#ifndef PWM_MIN +#define PWM_MIN 0 +#endif + +//Controleur : +//-255:0 : Marche arrière +//0:255 : Marche avant + +AF_DCMotor motor_left(1, MOTOR12_64KHZ); +AF_DCMotor motor_right(2, MOTOR12_64KHZ); + +extern "C" void set_pwm_right(int pwm); +extern "C" void set_pwm_left(int pwm); + +void set_pwm_left(int pwm){ + pwm = -pwm;//les moteurs sont faces à face, pour avancer il faut qu'il tournent dans un sens différent + if (pwm > 0) + pwm += PWM_MIN; + else if (pwm < 0) + pwm -= PWM_MIN; + + if(pwm > 255) + pwm = 255; + else if(pwm < -255) + pwm = -255; + + if(pwm >= 0) + motor_left.run(FORWARD); + else + motor_left.run(BACKWARD); + + motor_left.setSpeed(abs(pwm)); +} + +void set_pwm_right(int pwm){ + if (pwm > 0) + pwm += PWM_MIN; + else if (pwm < 0) + pwm -= PWM_MIN; + + if(pwm > 255) + pwm = 255; + else if(pwm < -255) + pwm = -255; + + if(pwm >= 0) + motor_right.run(FORWARD); + else + motor_right.run(BACKWARD); + + motor_right.setSpeed(abs(pwm)); +} diff --git a/arduino/GB/motor.h b/arduino/GB/motor.h new file mode 100644 index 0000000..039e74b --- /dev/null +++ b/arduino/GB/motor.h @@ -0,0 +1,17 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 31/03/13 * + ****************************************/ +#ifndef MOTOR_H +#define MOTOR_H + +void set_pwm(int side, int pwm); + +inline void MotorsInit(void) {}; + +void set_pwm_left(int pwm); + +void set_pwm_right(int pwm); + +#endif diff --git a/arduino/GB/parameters.h b/arduino/GB/parameters.h new file mode 100644 index 0000000..5e10049 --- /dev/null +++ b/arduino/GB/parameters.h @@ -0,0 +1,85 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#define BAUDRATE 57600 +#define SERIAL_TYPE SERIAL_8N1 +#define ARDUINO_ID "A" +#define DEBUG_TARGET_SPEED 0 +#define DEBUG_MAINLOOP 0 + +/* Simple ou Double ou Quadruple evaluation ? + * La quadruple evaluation utilise 4 interruption par tick + * (une pour chaque changement de valeur des fils A et B) + * + * La double evaluation utilise 2 interruptions par tick + * (une pour chaque changement de valeur du fil A + * + * La simple evaluation utilise 1 interruption par tick + * (front montant du fil A) + * + * Ces trois méthodes sont equivalentes + * La quadruple evaluation est plus précise mais utilise + * plus de puissance processeur + * Il convient de tester la plus adapté selon le processeur + * et le nombre de ticks du codeur + * + * OPTIONS : '1' - '2 - '4' */ + +#define ENCODER_EVAL 2 + +#define USE_SHARP 1 +#define EMERGENCY_STOP_DISTANCE 0.32 // m +#define EMERGENCY_WAIT_TIME 30 // seconds +#define EMERGENCY_SLOW_GO_RATIO 0.5 // spd = 0.3*max_spd in slow_go mode + +#define HZ 100 +#define DT (1.0/HZ) +#define AUTO_STATUS_HZ 100 // must be a divider a HZ or 0 to disable + +#define SPD_MAX 500 //mm/s +#define ACC_MAX 1000 //mm/s2 +#define RATIO_ROT_SPD_MAX 0.3 +#define K_DISTANCE_REDUCTION 10 // réduction de la vitesse linéaire quand on tourne + +#define ENC_RESOLUTION 1024 //resolution du codeur + +#define ENC_LEFT_RADIUS 20.02 //REGLE PAR TEST - rayon de la roue codeuse +#define ENC_RIGHT_RADIUS 20.02 //REGLE PAR TEST - rayon de la roue codeuse +#define ENTRAXE_ENC 200.0 //REGLE PAR TES - Distance entre chaque roue codeuse en mm + +#define ERROR_ANGLE 0.015 //erreur en angle(radians) maximale pour considérer l'objectif comme atteint +#define ERROR_POS 5 // erreur en position (mm) maximale pour considérer l'objectif comme atteint +#define SPD_TO_STOP 10 + +#define CONE_ALIGNEMENT 100 // NEVER + +#define PID_P 1.5 +#define PID_I 30 +#define PID_D 5 +#define PID_BIAS 0 + +#define LEFT_P (PID_P) +#define LEFT_I (PID_I) +#define LEFT_D (PID_D) +#define LEFT_BIAS (PID_BIAS) + +#define RIGHT_P (PID_P) +#define RIGHT_I (PID_I) +#define RIGHT_D (PID_D) +#define RIGHT_BIAS (PID_BIAS) + +#define PID_I_RATIO (1/1000.0) +#define PID_D_RATIO (1/1000.0) + +#define TIME_BETWEEN_ORDERS 1 // s +#define KEEP_LAST_GOAL 0 + +//DEFINES ARDUINO +#define SERIAL_MAIN Serial + +#endif diff --git a/arduino/GB/pins.h b/arduino/GB/pins.h new file mode 100644 index 0000000..573ad52 --- /dev/null +++ b/arduino/GB/pins.h @@ -0,0 +1 @@ +../asserv/pins.h \ No newline at end of file diff --git a/arduino/GB/protocol.c b/arduino/GB/protocol.c new file mode 100644 index 0000000..400de18 --- /dev/null +++ b/arduino/GB/protocol.c @@ -0,0 +1 @@ +../asserv/protocol.c \ No newline at end of file diff --git a/arduino/GB/protocol.h b/arduino/GB/protocol.h new file mode 100644 index 0000000..1cbc2c4 --- /dev/null +++ b/arduino/GB/protocol.h @@ -0,0 +1 @@ +../asserv/protocol.h \ No newline at end of file diff --git a/arduino/GB/robotstate.c b/arduino/GB/robotstate.c new file mode 100644 index 0000000..75eba38 --- /dev/null +++ b/arduino/GB/robotstate.c @@ -0,0 +1 @@ +../asserv/robotstate.c \ No newline at end of file diff --git a/arduino/GB/robotstate.h b/arduino/GB/robotstate.h new file mode 100644 index 0000000..2ab4d30 --- /dev/null +++ b/arduino/GB/robotstate.h @@ -0,0 +1 @@ +../asserv/robotstate.h \ No newline at end of file diff --git a/arduino/GB/serial_switch.c b/arduino/GB/serial_switch.c new file mode 100644 index 0000000..d875c52 --- /dev/null +++ b/arduino/GB/serial_switch.c @@ -0,0 +1 @@ +../asserv/serial_switch.c \ No newline at end of file diff --git a/arduino/GB/serial_switch.h b/arduino/GB/serial_switch.h new file mode 100644 index 0000000..d36f653 --- /dev/null +++ b/arduino/GB/serial_switch.h @@ -0,0 +1 @@ +../asserv/serial_switch.h \ No newline at end of file diff --git a/arduino/GUI/com.py b/arduino/GUI/com.py new file mode 100644 index 0000000..3b37c40 --- /dev/null +++ b/arduino/GUI/com.py @@ -0,0 +1,121 @@ +import serial +import threading +from variables import Variables +import time + +class Communication: + def __init__(self, path, baudrate): + print("Opening serial port "+path+" ("+str(baudrate)+")") + self.serial = serial.Serial(path, baudrate, timeout=1, writeTimeout=1) + self.robot_ID = self.serial.read(1) + self.vars = Variables("../asserv/protocol.h") + print(self.vars) + self.responses = {} + self.start() + + def sendOrderSafe(self, order, ID=0, args=[]): + try: + self.sendOrder(order, ID, args) + except IOError as e: + print("Failed to send order "+order) + print(e) + + def sendOrder(self, order, ID=0, args=[]): + order = self.vars[order] + order = order.strip("'") + cmd = order+";"+str(int(ID))+";" + for arg in args: + cmd += str(int(arg))+";" + cmd += "\n" + try: + self.serial.write(cmd.encode()) + except: + raise IOError("Write failed") + + begin = time.time() + while (order not in self.responses.keys() or self.responses[order] == ''): + if time.time() - begin > 3: + raise IOError("Arduino denied order (1)") + self.responses[order] = '' + if order not in self.responses.keys() or\ + "FAILED" in self.responses[order]: + raise IOError("Arduino denied order (2)") + else: + return self.responses[order][4:].split(";") + + def goto(self, x, y, a=None): + if a is not None: + sendOrder(self.vars['GOTOA'], args=[x, y, a*self.vars['FLOAT_PRECISION']]) + else: + sendOrder(self.vars['GOTO'], args=[x, y]) + + def getTargetSpd(self): + if '~' in self.responses.keys(): + status = self.responses['~'] + else: + status = '' + if status == '': + return (0, 0) + + status = status.split(';') + if len(status) < 7: + return (0, 0) + + if len(status) % 2 == 1: + spd = status[7:9] + else: + spd = status[6:8] + return spd + + def getSpd(self): + if '~' in self.responses.keys(): + status = self.responses['~'] + else: + status = '' + if status == '': + return (0, 0) + + status = status.split(';') + if len(status) % 2 == 1: + spd = status[5:7] + else: + spd = status[4:6] + return spd + + def getPos(self): + if '~' in self.responses.keys(): + status = self.responses['~'] + else: + status = '' + if status == '': + return (0, 0, 0) + + status = status.split(';') + if len(status) % 2 == 1: + pos = [float(x) for x in status[2:5]] + else: + pos = [float(x) for x in status[1:4]] + return pos + + def stop(self): + self.pause=True + + def start(self): + self.pause=False + self.thread = threading.Thread(target=self.readThread) + self.thread.daemon = True + self.thread.start() + + def readThread(self): + while not self.pause: + l = self.serial.readline() + try: + l = l.decode("ascii")[:-1] + except: + continue + if len(l) > 0: + self.responses[l[0]] = l + if (l[0] == '~'): + print(l) + if (l[0] == '#'): + print(l) diff --git a/arduino/GUI/gui.py b/arduino/GUI/gui.py new file mode 100644 index 0000000..7601345 --- /dev/null +++ b/arduino/GUI/gui.py @@ -0,0 +1,264 @@ +#!/usr/bin/python3 + +import sys +from socket import * +from tkinter import * +import threading +import com +import time + +class GUI: + def __init__(self, path, baud): + #defines + self.widthfen = 800 + self.heightfen = 600 + self.areax = 3000 + self.areay = 2000 + self.robotsize = 50 + self.robot_pos = [0, 0, 0] + + try: + self.com = com.Communication(path, baud) + except: + print("WARNING : Socket non ouvert, mode visualisation") + + #init GUI + self.fen = Tk() + self.fen.title("GUI") + + self.chaine = Label(self.fen) + self.chaine_pos = Label(self.fen) + + self.cadre = Canvas(self.fen, width=self.widthfen, height =self.heightfen, bg="light yellow") + self.cadre.bind("", self.clic_goto) + self.robot_rect = self.cadre.create_oval(0, 0, self.robotsize, self.robotsize, offset='center', fill='red') + self.robot_txt = self.cadre.create_text(0, 0, text='Pos') + self.move_robot(*self.robot_pos) + + #speeds + + #reset + self.reset_pos_button = Button(self.fen, text='Reset position', command=self.reset_pos) + self.reset_goals_button = Button(self.fen, text='Reset objectifs', command=self.reset_goals) + + #fifo + self.fifo_switch = Scale(self.fen, from_=0, to=1, label='Fifo', orient='horizontal') + + #goto manue + self.goto_text = Label(self.fen, text= "Goto") + self.gotox_e = Entry(self.fen) + self.gotox_e.insert(0, '1500') + self.gotoy_e = Entry(self.fen) + self.gotoy_e.insert(0, '0') + self.gotoang = Entry(self.fen) + self.gotoang.insert(0, '0') + self.goto_frame = Frame() + self.send_goto = Button(self.goto_frame, text="Goto", command=self.goto_handler).pack(side='left') + self.send_gotoa = Button(self.goto_frame, text="Gotoa", command=self.gotoa_handler).pack(side='right') + self.send_angle = Button(self.goto_frame, text="Angle", command=self.angle_handler).pack(side='right') + + #pwm menu + self.pwm_text = Label(self.fen, text= "PWM") + self.pwm_g = Entry(self.fen) + self.pwm_d = Entry(self.fen) + self.pwm_duration = Entry(self.fen) + self.pwm_frame = Frame() + self.send_pwm = Button(self.pwm_frame, text="Send pwm", command=self.pwm_handler).pack(side='left') + + #reglages + self.pid_text = Label(self.fen, text="PID") + self.pid_p = Entry(self.fen) + self.pid_p.insert(0, '0.5') + self.pid_i = Entry(self.fen) + self.pid_i.insert(0, '50') + self.pid_d = Entry(self.fen) + self.pid_d.insert(0, '10') + + #self.pidl_text = Label(self.fen, text="PID left") + #self.pidl_p = Entry(self.fen) + #self.pidl_p.insert(0, '0.5') + #self.pidl_i = Entry(self.fen) + #self.pidl_i.insert(0, '0.01') + #self.pidl_d = Entry(self.fen) + #self.pidl_d.insert(0, '0') + + #self.pidr_text = Label(self.fen, text="PID right") + #self.pidr_p = Entry(self.fen) + #self.pidr_p.insert(0, '0.5') + #self.pidr_i = Entry(self.fen) + #self.pidr_i.insert(0, '0.01') + #self.pidr_d = Entry(self.fen) + #self.pidr_d.insert(0, '0') + + self.acc_max_text = Label(self.fen, text="Acc max") + self.acc_max = Entry(self.fen) + self.acc_max.insert(0, '500') + + self.spd_ratio_text = Label(self.fen, text="Spd rotation ratio") + self.spd_ratio = Entry(self.fen) + self.spd_ratio.insert(0, '0.3') + + self.spd_max_text = Label(self.fen, text="Spd max") + self.spd_max = Entry(self.fen) + self.spd_max.insert(0, '750') + + self.send_pause = Button(self.fen, text="Pause", command=self.pause_handler) + self.send_resume = Button(self.fen, text="Resume", command=self.resume_handler) + + self.send_reg = Button(self.fen, text="Send", command=self.val_reg) + + self.chaine.pack(side='bottom') + self.chaine_pos.pack(side='bottom') + self.cadre.pack(side='right', padx=10, pady=10) + self.fifo_switch.pack() + self.reset_goals_button.pack(pady=10) + self.reset_pos_button.pack(pady=10) + + self.goto_text.pack() + self.gotox_e.pack() + self.gotoy_e.pack() + self.gotoang.pack() + self.goto_frame.pack() + + self.pwm_text.pack() + self.pwm_g.pack() + self.pwm_d.pack() + self.pwm_duration.pack() + self.pwm_frame.pack() + + self.send_reg.pack(side='bottom') + self.pid_d.pack(side='bottom') + self.pid_i.pack(side='bottom') + self.pid_p.pack(side='bottom') + self.pid_text.pack(side='bottom') + #self.pidl_d.pack(side='bottom') + #self.pidl_i.pack(side='bottom') + #self.pidl_p.pack(side='bottom') + #self.pidl_text.pack(side='bottom') + #self.pidr_d.pack(side='bottom') + #self.pidr_i.pack(side='bottom') + #self.pidr_p.pack(side='bottom') + #self.pidr_text.pack(side='bottom') + self.spd_ratio.pack(side='bottom') + self.spd_ratio_text.pack(side='bottom') + self.acc_max.pack(side='bottom') + self.acc_max_text.pack(side='bottom') + self.spd_max.pack(side='bottom') + self.spd_max_text.pack(side='bottom') + self.send_pause.pack(side='bottom') + self.send_resume.pack(side='bottom') + + self.thread_pos = threading.Thread(target=self.pos_update); + self.thread_pos.daemon = True + self.thread_pos.start() + + self.fen.after(100, self.pos_loop) + self.fen.mainloop() + + def move_robot(self, x, y, a): + x = int(x) + y = int(y) + a = float(a) + self.cadre.coords(self.robot_rect, ((x / self.areax) * self.widthfen) - self.robotsize / 2, self.heightfen - ((y / self.areay) * self.heightfen) - self.robotsize / 2, ((x / self.areax) * self.widthfen) + self.robotsize / 2, self.heightfen - ((y / self.areay) * self.heightfen) + self.robotsize / 2) + self.cadre.coords(self.robot_txt, ((x / self.areax) * self.widthfen), self.heightfen - ((y / self.areay) * self.heightfen) + self.robotsize / 1.5) + self.cadre.itemconfig(self.robot_txt, text=str(x) + ";" + str(y) + ";" + "{:.2f}".format(a)) + + def pos_update(self): + #while 1: + self.robot_pos = self.com.getPos() + self.chaine_pos.configure(text="Pos : "+str(self.robot_pos)) + self.fen.after(10, self.pos_update) + # time.sleep(0.01) + + def pos_loop(self): + self.move_robot(*self.robot_pos) + self.fen.after(100, self.pos_loop) + + def reset_pos(self): + self.chaine.configure(text = "reset_pos : "+str(0)+" ; "+str(0)+" ; "+str(0)) + arguments = [0, 0, 0] + self.com.sendOrderSafe('SET_POS', args=arguments) + + def reset_goals(self): + self.com.sendOrderSafe('CLEANG') + + def val_reg(self): + arguments = [int(self.acc_max.get())] + self.com.sendOrderSafe('ACCMAX', args=arguments) + arguments = [int(self.spd_max.get()), int(float(self.spd_ratio.get())*1000)] + self.com.sendOrderSafe('SPDMAX', args=arguments) + arguments = [int(1000*float(self.pid_p.get())), + int(1000*float(self.pid_i.get())), + int(1000*float(self.pid_d.get()))] + self.com.sendOrderSafe('PIDALL', args=arguments) + #arguments = [int(1000*float(self.pidl_p.get())), + # int(1000*float(self.pidl_i.get())), + # int(1000*float(self.pidl_d.get()))] + #self.com.sendOrderSafe('PIDLEFT', args=arguments) + #arguments = [int(1000*float(self.pidr_p.get())), + # int(1000*float(self.pidr_i.get())), + # int(1000*float(self.pidr_d.get()))] + #self.com.sendOrderSafe('PIDRIGHT', args=arguments) + + def goto(self, gotox, gotoy): + self.chaine.configure(text = "Goto : "+str(gotox)+" ; "+str(gotoy)) + if self.fifo_switch.get() == 0: + #on clean la file a chaque nouvel ordre + self.com.sendOrderSafe('CLEANG') + + arguments = [int(gotox), int(gotoy)] + self.com.sendOrderSafe('GOTO', args=arguments) + + def gotoa(self, gotox, gotoy, gotoa): + self.chaine.configure(text = "Gotoa : "+str(gotox)+" ; "+str(gotoy)+" ; "+str(gotoa)) + if self.fifo_switch.get() == 0:#on clean la file a chaque nouvel ordre + self.com.sendOrderSafe('CLEANG') + + def angle(self, gotoa): + self.chaine.configure(text = "Angle : "+str(gotoa)) + if self.fifo_switch.get() == 0:#on clean la file a chaque nouvel ordre + self.com.sendOrderSafe('CLEANG') + + arguments = [int(float(gotoa)*1000)] + self.com.sendOrderSafe('ROTNOMODULO', args=arguments) + + def sendPwm(self, g, d, duration): + self.chaine.configure(text = "pwm : "+str(g)+" ; "+str(d)+" ; "+str(duration)) + if self.fifo_switch.get() == 0:#on clean la file a chaque nouvel ordre + self.com.sendOrderSafe('CLEANG') + + arguments = [int(g), int(d), int(duration)] + self.com.sendOrderSafe('PWM', args=arguments) + + def pause_handler(self): + self.com.sendOrderSafe('PAUSE') + + def resume_handler(self): + self.com.sendOrderSafe('RESUME') + + def pwm_handler(self): + self.sendPwm(self.pwm_g.get(), self.pwm_d.get(), self.pwm_duration.get()) + + def goto_handler(self): + self.goto(self.gotox_e.get(), self.gotoy_e.get()) + + def gotoa_handler(self): + self.gotoa(self.gotox_e.get(), self.gotoy_e.get(), self.gotoang.get()) + + def angle_handler(self): + self.angle(self.gotoang.get()) + + def clic_goto(self, event): + gotox = int((event.x/self.widthfen)*self.areax) + gotoy = int(self.areay - (event.y/self.heightfen)*self.areay) + self.goto(gotox, gotoy) + +if __name__ == '__main__': + if len(sys.argv) > 2: + baud = sys.argv[2] + elif len(sys.argv) == 2: + baud = 57600 + else: + print("$0 serial_path [baudrate]") + exit(1) + a = GUI(sys.argv[1], baud) diff --git a/arduino/GUI/plot_spd.py b/arduino/GUI/plot_spd.py new file mode 100644 index 0000000..ea8532d --- /dev/null +++ b/arduino/GUI/plot_spd.py @@ -0,0 +1,54 @@ +#!/usr/bin/python3 + +import sys +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +nr_data = 100 +vmax = 10 + +def init(): + lreal.set_ydata(lr) + ltarget.set_ydata(lt) + rreal.set_ydata(rr) + rtarget.set_ydata(rt) + return lreal, ltarget, rreal, rtarget + +def animate(i): + l = '' + while not '~' in l: + l = sys.stdin.readline() + l = l.split(';') + lr.append(l[5]) + rr.append(l[6]) + lt.append(l[7]) + rt.append(l[8]) + lr.pop(0) + rr.pop(0) + lt.pop(0) + rt.pop(0) + lreal.set_ydata(lr) + ltarget.set_ydata(lt) + rreal.set_ydata(rr) + rtarget.set_ydata(rt) + return lreal, ltarget, rreal, rtarget + + +fig = plt.figure() +ax1 = fig.add_subplot(2, 1, 1) +ax2 = fig.add_subplot(2, 1, 2) + +x = list(range(nr_data)) +lr = lt = rr = rt = [0]*len(x) + +lreal, = ax1.plot(x, lr) +ltarget, = ax1.plot(x, lt) +rreal, = ax2.plot(x, rr) +rtarget, = ax2.plot(x, rt) +ax1.set_ylim([-vmax, vmax]) +ax2.set_ylim([-vmax, vmax]) + +ani = animation.FuncAnimation(fig, animate, init_func=init, + interval=1, blit=True) + +plt.show() diff --git a/arduino/GUI/variables.py b/arduino/GUI/variables.py new file mode 100644 index 0000000..efc9613 --- /dev/null +++ b/arduino/GUI/variables.py @@ -0,0 +1,11 @@ +class Variables(dict): + def __init__(self, path): + f = open(path) + for line in f: + if line[0:7] == "#define": + try: + s = line.split() + self[s[1]] = s[2] + except Exception as e: + pass + diff --git a/arduino/PB/PB.ino b/arduino/PB/PB.ino new file mode 100644 index 0000000..384ec1e --- /dev/null +++ b/arduino/PB/PB.ino @@ -0,0 +1 @@ +../asserv/asserv.ino \ No newline at end of file diff --git a/arduino/PB/PID.c b/arduino/PB/PID.c new file mode 100644 index 0000000..13c2176 --- /dev/null +++ b/arduino/PB/PID.c @@ -0,0 +1 @@ +../asserv/PID.c \ No newline at end of file diff --git a/arduino/PB/PID.h b/arduino/PB/PID.h new file mode 100644 index 0000000..74e0274 --- /dev/null +++ b/arduino/PB/PID.h @@ -0,0 +1 @@ +../asserv/PID.h \ No newline at end of file diff --git a/arduino/PB/block.c b/arduino/PB/block.c new file mode 100644 index 0000000..659ba90 --- /dev/null +++ b/arduino/PB/block.c @@ -0,0 +1 @@ +../asserv/block.c \ No newline at end of file diff --git a/arduino/PB/block.h b/arduino/PB/block.h new file mode 100644 index 0000000..6dc29bc --- /dev/null +++ b/arduino/PB/block.h @@ -0,0 +1 @@ +../asserv/block.h \ No newline at end of file diff --git a/arduino/PB/brushlessMotor.c b/arduino/PB/brushlessMotor.c new file mode 100644 index 0000000..efbf8ec --- /dev/null +++ b/arduino/PB/brushlessMotor.c @@ -0,0 +1,73 @@ +//Par Quentin pour UTCoupe2013 01/04/2013 +//Commande de shield arduino brushless by UTCoupe + +#include +#include "brushlessMotor.h" + +#define NO_PWM 127 + +/****************************************** + MOTORS +******************************************/ +/********************************* +DIG1 and DIG2 defined to 0 : +linear adjustment of the speed + +might be configured to smth else in order use speed control +see datasheet of DEC-MODULE-24/2 +***********************************/ + +void BrushlessMotorsInit() { + pinMode(MOTOR1_SPD, OUTPUT); + pinMode(MOTOR1_EN, OUTPUT); + pinMode(MOTOR1_BRK, OUTPUT); + pinMode(MOTOR2_SPD, OUTPUT); + pinMode(MOTOR2_EN, OUTPUT); + pinMode(MOTOR2_BRK, OUTPUT); + + digitalWrite(MOTOR1_BRK, HIGH); + digitalWrite(MOTOR2_BRK, HIGH); + digitalWrite(MOTOR1_EN, LOW); + digitalWrite(MOTOR2_EN, LOW); + + analogWrite(MOTOR1_SPD, NO_PWM); + analogWrite(MOTOR2_SPD, NO_PWM); +} + +void BrushlessMotorSetPwm(int motor_side, int pwm) { + static int last_pwms[2] = {NO_PWM, NO_PWM}; + int *last_pwm; + if (motor_side == MOTOR_LEFT) { + last_pwm = &last_pwms[0]; + } else { + last_pwm = &last_pwms[1]; + } + if (pwm == *last_pwm) { + return; + } + else { + *last_pwm = pwm; + } + switch (motor_side) { + case MOTOR_LEFT:{ + analogWrite(MOTOR1_SPD, pwm); + if (pwm == NO_PWM) { + digitalWrite(MOTOR1_EN,LOW); + } + else { + digitalWrite(MOTOR1_EN,HIGH); + } + break; + } + case MOTOR_RIGHT:{ + analogWrite(MOTOR2_SPD, pwm); + if (pwm == NO_PWM) { + digitalWrite(MOTOR2_EN,LOW); + } + else { + digitalWrite(MOTOR2_EN,HIGH); + } + break; + } + } +} diff --git a/arduino/PB/brushlessMotor.h b/arduino/PB/brushlessMotor.h new file mode 100644 index 0000000..b26972e --- /dev/null +++ b/arduino/PB/brushlessMotor.h @@ -0,0 +1,24 @@ +//Adapted from Adafruit Motor Shield by Quentin C for UTCoupe +//Defined for brushless controler shield designed by UTCoupe +//08/04/2013 + +#ifndef BRUSHLESSMOTOR_H +#define BRUSHLESSMOTOR_H + +#include "parameters.h" +#include "pins.h" + +#define MOTOR_LEFT 1 +#define MOTOR_RIGHT 2 + +#define LEFT_READY_SHIFT 1 +#define RIGHT_READY_SHIFT 2 + +#define LEFT_READY (1< 255) + pwm = 255; + else if(pwm < 0) + pwm = 0; + BrushlessMotorSetPwm(side, pwm); +} + diff --git a/arduino/PB/motor.h b/arduino/PB/motor.h new file mode 100644 index 0000000..fa1817f --- /dev/null +++ b/arduino/PB/motor.h @@ -0,0 +1,28 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 31/03/13 * + ****************************************/ +#ifndef MOTOR_H +#define MOTOR_H + +#include "brushlessMotor.h" + +#define MOTOR_LEFT 1 +#define MOTOR_RIGHT 2 + +void set_pwm(int side, int pwm); + +inline void MotorsInit(void) { + BrushlessMotorsInit(); +} + +inline void set_pwm_left(int pwm){ + set_pwm(MOTOR_LEFT, pwm); +} + +inline void set_pwm_right(int pwm){ + set_pwm(MOTOR_RIGHT, pwm); +} + +#endif diff --git a/arduino/PB/parameters.h b/arduino/PB/parameters.h new file mode 100644 index 0000000..6876e97 --- /dev/null +++ b/arduino/PB/parameters.h @@ -0,0 +1,88 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef PARAMETERS_H +#define PARAMETERS_H + +#define BAUDRATE 57600 +#define SERIAL_TYPE SERIAL_8N1 +#define ARDUINO_ID "A" +#define DEBUG_TARGET_SPEED 0 +#define DEBUG_MAINLOOP 0 + +/* Simple ou Double ou Quadruple evaluation ? + * La quadruple evaluation utilise 4 interruption par tick + * (une pour chaque changement de valeur des fils A et B) + * + * La double evaluation utilise 2 interruptions par tick + * (une pour chaque changement de valeur du fil A + * + * La simple evaluation utilise 1 interruption par tick + * (front montant du fil A) + * + * Ces trois méthodes sont equivalentes + * La quadruple evaluation est plus précise mais utilise + * plus de puissance processeur + * Il convient de tester la plus adapté selon le processeur + * et le nombre de ticks du codeur + * + * OPTIONS : '1' - '2 - '4' */ + +#define ENCODER_EVAL 2 + +#define USE_SHARP 0 +#define EMERGENCY_STOP_DISTANCE 0.3 // m + +#define HZ 100 +#define DT (1.0/HZ) +#define AUTO_STATUS_HZ 100 // must be a divider a HZ or 0 to disable + +#define SPD_MAX 1000 //mm/s +#define ACC_MAX 1500 //mm/s2 +#define RATIO_ROT_SPD_MAX 0.6 +#define K_DISTANCE_REDUCTION 20 // réduction de la vitesse linéaire quand on tourne +#define EMERGENCY_WAIT_TIME 30 // seconds +#define EMERGENCY_SLOW_GO_RATIO 0.3 // spd = 0.3*max_spd in slow_go mode + +#define BLOCK_TIME 5000 // ms - time between each block check +#define BLOCK_MIN_DIST 5 // mm - distance to move to consider we moved + +#define ENC_RESOLUTION 1024 //resolution du codeur + +#define ENC_LEFT_RADIUS 31.38 //REGLE PAR TEST - rayon de la roue codeuse +#define ENC_RIGHT_RADIUS 31.24 //REGLE PAR TEST - rayon de la roue codeuse +#define ENTRAXE_ENC 202.27 //REGLE PAR TES - Distance entre chaque roue codeuse en mm + +#define ERROR_ANGLE 0.030 //erreur en angle(radians) maximale pour considérer l'objectif comme atteint +#define ERROR_POS 5 // erreur en position (mm) maximale pour considérer l'objectif comme atteint +#define SPD_TO_STOP 10 + +#define CONE_ALIGNEMENT (M_PI/2.0) + +#define PID_P 0.25 +#define PID_I 130 +#define PID_D 13 +#define PID_BIAS 0 + +#define LEFT_P (PID_P) +#define LEFT_I (PID_I) +#define LEFT_D (PID_D) +#define LEFT_BIAS (PID_BIAS) + +#define RIGHT_P (PID_P) +#define RIGHT_I (PID_I) +#define RIGHT_D (PID_D) +#define RIGHT_BIAS (PID_BIAS) + +#define PID_I_RATIO (1/1000.0) +#define PID_D_RATIO (1/1000.0) + +#define TIME_BETWEEN_ORDERS 0 // s +#define KEEP_LAST_GOAL 0 + +//DEFINES ARDUINO +#define SERIAL_MAIN Serial + +#endif diff --git a/arduino/PB/pins.h b/arduino/PB/pins.h new file mode 100644 index 0000000..573ad52 --- /dev/null +++ b/arduino/PB/pins.h @@ -0,0 +1 @@ +../asserv/pins.h \ No newline at end of file diff --git a/arduino/PB/protocol.c b/arduino/PB/protocol.c new file mode 100644 index 0000000..400de18 --- /dev/null +++ b/arduino/PB/protocol.c @@ -0,0 +1 @@ +../asserv/protocol.c \ No newline at end of file diff --git a/arduino/PB/protocol.h b/arduino/PB/protocol.h new file mode 100644 index 0000000..1cbc2c4 --- /dev/null +++ b/arduino/PB/protocol.h @@ -0,0 +1 @@ +../asserv/protocol.h \ No newline at end of file diff --git a/arduino/PB/robotstate.c b/arduino/PB/robotstate.c new file mode 100644 index 0000000..75eba38 --- /dev/null +++ b/arduino/PB/robotstate.c @@ -0,0 +1 @@ +../asserv/robotstate.c \ No newline at end of file diff --git a/arduino/PB/robotstate.h b/arduino/PB/robotstate.h new file mode 100644 index 0000000..2ab4d30 --- /dev/null +++ b/arduino/PB/robotstate.h @@ -0,0 +1 @@ +../asserv/robotstate.h \ No newline at end of file diff --git a/arduino/PB/serial_switch.c b/arduino/PB/serial_switch.c new file mode 100644 index 0000000..d875c52 --- /dev/null +++ b/arduino/PB/serial_switch.c @@ -0,0 +1 @@ +../asserv/serial_switch.c \ No newline at end of file diff --git a/arduino/PB/serial_switch.h b/arduino/PB/serial_switch.h new file mode 100644 index 0000000..d36f653 --- /dev/null +++ b/arduino/PB/serial_switch.h @@ -0,0 +1 @@ +../asserv/serial_switch.h \ No newline at end of file diff --git a/arduino/README b/arduino/README new file mode 100644 index 0000000..c3a65ae --- /dev/null +++ b/arduino/README @@ -0,0 +1,12 @@ +parameters.h : configuration du robot +motor.* : lien entre le code et la lib moteur +goals.* : management des goals +encoder.* : management des coderus, garde le compte des ticks et est appelé par interruptions +robotstate.* : calcule et garde l'etat du robot (position, etc...) +control.* : partie controle, met en relation toutes les parties +PID.* : classe de PID générique +compat.* : "hacks" pour interfacer les interruptuions arduino avec le code +command.* : commandes appelées par serial port +message.* : serial port compatibility + +Il suffit de créer une classe control qui intégrera tout le nécéssaire diff --git a/arduino/SConstruct b/arduino/SConstruct new file mode 100644 index 0000000..a37cd82 --- /dev/null +++ b/arduino/SConstruct @@ -0,0 +1,380 @@ +#!/usr/bin/python2.7 + +# scons script for the Arduino sketch +# http://code.google.com/p/arscons/ +# +# Copyright (C) 2010 by Homin Lee +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. + +# You'll need the serial module: http://pypi.python.org/pypi/pyserial + +# Basic Usage: +# 1. make a folder which have same name of the sketch (ex. Blink/ for Blik.pde) +# 2. put the sketch and SConstruct(this file) under the folder. +# 3. to make the HEX. do following in the folder. +# $ scons +# 4. to upload the binary, do following in the folder. +# $ scons upload + +# Thanks to: +# * Ovidiu Predescu and Lee Pike +# for Mac port and bugfix. +# +# This script tries to determine the port to which you have an Arduino +# attached. If multiple USB serial devices are attached to your +# computer, you'll need to explicitly specify the port to use, like +# this: +# +# $ scons ARDUINO_PORT=/dev/ttyUSB0 +# +# To add your own directory containing user libraries, pass EXTRA_LIB +# to scons, like this: +# +# $ scons EXTRA_LIB= +# +from glob import glob +import serial +import time +import sys +import re +import subprocess +import os +pathJoin = os.path.join + +env = Environment() +platform = env['PLATFORM'] + +def getUsbTty(rx): + usb_ttys = glob(rx) + if len(usb_ttys) == 1: return usb_ttys[0] + else: return None + +AVR_BIN_PREFIX = None +AVRDUDE_CONF = None +ARDUINO_BOARD_DEFAULT = os.environ.get('ARDUINO_BOARD', 'mega2560') + +if platform == 'darwin': + # For MacOS X, pick up the AVR tools from within Arduino.app + ARDUINO_HOME_DEFAULT = '/Applications/Arduino.app/Contents/Resources/Java' + ARDUINO_PORT_DEFAULT = getUsbTty('/dev/tty.usbserial*') + SKETCHBOOK_HOME_DEFAULT = '' +elif platform == 'win32': + # For Windows, use environment variables. + ARDUINO_HOME_DEFAULT = os.environ.get('ARDUINO_HOME') + ARDUINO_PORT_DEFAULT = os.environ.get('ARDUINO_PORT') + SKETCHBOOK_HOME_DEFAULT = '' +else: + # For Ubuntu Linux (9.10 or higher) + ARDUINO_HOME_DEFAULT = os.path.join("..","arduino-1.0/") #'/home/YOU/apps/arduino-00XX/' + if ARDUINO_BOARD_DEFAULT == 'nano328': + ARDUINO_PORT_DEFAULT = getUsbTty('/dev/ttyUSB*') + else: + ARDUINO_PORT_DEFAULT = getUsbTty('/dev/ttyACM*') + AVR_BIN_PREFIX = 'avr-' + SKETCHBOOK_HOME_DEFAULT = os.path.realpath('~/share/arduino/sketchbook/') + +ARDUINO_HOME = ARGUMENTS.get('ARDUINO_HOME', ARDUINO_HOME_DEFAULT) +ARDUINO_PORT = ARGUMENTS.get('ARDUINO_PORT', ARDUINO_PORT_DEFAULT) +ARDUINO_BOARD = ARGUMENTS.get('ARDUINO_BOARD', ARDUINO_BOARD_DEFAULT) +ARDUINO_VER = ARGUMENTS.get('ARDUINO_VER', 0) # Default to 0 if nothing is specified +RST_TRIGGER = ARGUMENTS.get('RST_TRIGGER', None) # use built-in pulseDTR() by default +EXTRA_LIB = ARGUMENTS.get('EXTRA_LIB', None) # handy for adding another arduino-lib dir +SKETCHBOOK_HOME = ARGUMENTS.get('SKETCHBOOK_HOME', SKETCHBOOK_HOME_DEFAULT) # If set will add the libraries dir from the sketchbook + +if not ARDUINO_HOME: + print 'ARDUINO_HOME must be defined.' + raise KeyError('ARDUINO_HOME') + +ARDUINO_CORE = pathJoin(ARDUINO_HOME, 'hardware/arduino/cores/arduino') +ARDUINO_SKEL = pathJoin(ARDUINO_CORE, 'main.cpp') +ARDUINO_CONF = pathJoin(ARDUINO_HOME, 'hardware/arduino/boards.txt') + +arduino_file_path = pathJoin(ARDUINO_CORE, 'Arduino.h') +if ARDUINO_VER == 0: + print "No Arduino version specified. Discovered version", + if os.path.exists(arduino_file_path): + print "100 or above" + ARDUINO_VER = 100 + else: + print "0023 or below" + ARDUINO_VER = 23 +else: + print "Arduino version " + ARDUINO_VER + " specified" + +if ARDUINO_VER < 100: FILE_EXTENSION = ".pde" +if ARDUINO_VER >= 100: FILE_EXTENSION = ".ino" + +# Some OSs need bundle with IDE tool-chain +if platform == 'darwin' or platform == 'win32': + AVR_BIN_PREFIX = pathJoin(ARDUINO_HOME, 'hardware/tools/avr/bin', 'avr-') + AVRDUDE_CONF = pathJoin(ARDUINO_HOME, 'hardware/tools/avr/etc/avrdude.conf') + +ARDUINO_LIBS = [pathJoin(ARDUINO_HOME, 'libraries')] +if EXTRA_LIB: + ARDUINO_LIBS += [EXTRA_LIB] +if SKETCHBOOK_HOME: + ARDUINO_LIBS += [pathJoin(SKETCHBOOK_HOME, 'libraries')] + +# check given board name, ARDUINO_BOARD is valid one +ptnBoard = re.compile(r'^(.*)\.name=(.*)') +boards = {} +for line in open(ARDUINO_CONF): + result = ptnBoard.findall(line) + if result: + boards[result[0][0]] = result[0][1] +if not ARDUINO_BOARD in boards.keys(): + print ("ERROR! the given board name, %s is not in the supported board list:"%ARDUINO_BOARD) + print ("all available board names are:") + for name in boards.keys(): + print ("\t%s for %s"%(name.ljust(14), boards[name])) + print ("however, you may edit %s to add a new board."%ARDUINO_CONF) + sys.exit(-1) + +def getBoardConf(strPtn): + ptn = re.compile(strPtn) + for line in open(ARDUINO_CONF): + result = ptn.findall(line) + if result: + return result[0] + assert(False) + +MCU = getBoardConf(r'^%s\.build\.mcu=(.*)'%ARDUINO_BOARD) +MCU = ARGUMENTS.get('MCU', MCU) +F_CPU = getBoardConf(r'^%s\.build\.f_cpu=(.*)'%ARDUINO_BOARD) +F_CPU = ARGUMENTS.get('F_CPU', F_CPU) + +# There should be a file with the same name as the folder and with the extension .pde +TARGET = os.path.basename(os.path.realpath(os.curdir)) +assert(os.path.exists(TARGET+FILE_EXTENSION)) + +cFlags = ['-Wall', '-ffunction-sections', '-fdata-sections', '-fno-exceptions', + '-funsigned-char', '-funsigned-bitfields', '-fpack-struct', '-fshort-enums', + '-O3', '-mmcu=%s'%MCU] +if ARDUINO_BOARD == "leonardo": + cFlags += ["-DUSB_VID=0x2341"] + cFlags += ["-DUSB_PID=0x8036"] +envArduino = Environment(CC = AVR_BIN_PREFIX+'gcc', CXX = AVR_BIN_PREFIX+'g++', AS=AVR_BIN_PREFIX+'gcc', + CPPPATH = ['build/core'], CPPDEFINES = {'F_CPU':F_CPU, 'ARDUINO':ARDUINO_VER}, + CFLAGS = cFlags+['-Wall'], CCFLAGS = cFlags, ASFLAGS=['-assembler-with-cpp','-mmcu=%s'%MCU], + TOOLS = ['gcc','g++', 'as']) + + +if ARDUINO_VER >= 100: + if ARDUINO_BOARD == 'nano328': envArduino.Append(CPPPATH = pathJoin(ARDUINO_HOME, 'hardware/arduino/variants/eightanaloginputs/')) + elif ARDUINO_BOARD == 'leonardo': envArduino.Append(CPPPATH = pathJoin(ARDUINO_HOME, 'hardware/arduino/variants/leonardo/')) + elif ARDUINO_BOARD == 'mega2560': envArduino.Append(CPPPATH = pathJoin(ARDUINO_HOME, 'hardware/arduino/variants/mega/')) + elif ARDUINO_BOARD == 'micro': envArduino.Append(CPPPATH = pathJoin(ARDUINO_HOME, 'hardware/arduino/variants/micro/')) + else: envArduino.Append(CPPPATH = pathJoin(ARDUINO_HOME, 'hardware/arduino/variants/standard/')) + +def run(cmd): + """Run a command and decipher the return code. Exit by default.""" + import SCons.Script + print cmd + res = os.system(cmd) + # Assumes that if a process doesn't call exit, it was successful + if (os.WIFEXITED(res)): + code = os.WEXITSTATUS(res) + if code != 0: + print "Error: return code: " + str(code) + if SCons.Script.keep_going_on_error == 0: + sys.exit(code) + +def fnCompressCore(target, source, env): + core_files = filter(lambda x: str(x).startswith('build/core_patched/'), source) + for file in core_files: + run(AVR_BIN_PREFIX+'ar rcs %s %s'%(target[0], file)) + + +def fnProcessing(target, source, env): + wp = open ('%s'%target[0], 'wb') + wp.write(open(ARDUINO_SKEL).read()) + + types='''void + int char word long + float double byte long + boolean + uint8_t uint16_t uint32_t uint32_t + int8_t int16_t int32_t int32_t + ''' + types=' | '.join(types.split()) + re_signature=re.compile(r"""^\s* ( + (?: (%s) \s+ )? + \w+ \s* + \( \s* ((%s) \s+ \*? \w+ (?:\s*,\s*)? )* \) + ) \s* {? \s* $""" % (types,types), re.MULTILINE|re.VERBOSE) + + prototypes = {} + + for file in glob(os.path.realpath(os.curdir) + "/*" + FILE_EXTENSION): + for line in open(file): + result = re_signature.findall(line) + if result: + prototypes[result[0][0]] = result[0][1] + + for name in prototypes.keys(): + print ("%s;"%(name)) + wp.write("%s;\n"%name) + + for file in glob(os.path.realpath(os.curdir) + "/*" + FILE_EXTENSION): + print file, TARGET + if not os.path.samefile(file, TARGET+FILE_EXTENSION): + wp.write('#line 1 "%s"\r\n' % file) + wp.write(open(file).read()) + + # Add this preprocessor directive to localize the errors. + sourcePath = str(source[0]).replace('\\', '\\\\'); + wp.write('#line 1 "%s"\r\n' % sourcePath) + wp.write(open('%s'%source[0]).read()) + +def fnPatchCore(target, source, env): + patches = glob(os.path.join(".","*.patch")) + patches = map(lambda x: os.path.basename(x)[:-len('.patch')], patches) + for i,src in enumerate(source): + src = str(src) + trgt = str(target[i]) + f = open(trgt,'w') + f.write(open(src).read()) + f.close() + try: + i_patch = patches.index(os.path.basename(src)) + except ValueError: + continue + patch = patches[i_patch] + p = subprocess.Popen(["patch","-N",trgt,patch+".patch"]) + p.wait() + +envArduino.Append(BUILDERS = {'Processing':Builder(action = fnProcessing, + suffix = '.c', src_suffix = FILE_EXTENSION)}) +envArduino.Append(BUILDERS = {'PatchCore':Builder(action = fnPatchCore) }) +envArduino.Append(BUILDERS = {'CompressCore':Builder(action = fnCompressCore) }) +envArduino.Append(BUILDERS={'Elf':Builder(action=AVR_BIN_PREFIX+'gcc '+ + '-mmcu=%s -Os -Wl,--gc-sections -o $TARGET $SOURCES -lm'%MCU)}) +envArduino.Append(BUILDERS={'Hex':Builder(action=AVR_BIN_PREFIX+'objcopy '+ + '-O ihex -R .eeprom $SOURCES $TARGET')}) + +gatherSources = lambda x: glob(pathJoin(x, '*.c'))+\ + glob(pathJoin(x, '*.cpp'))+\ + glob(pathJoin(x, '*.S')) + +# add arduino core sources +VariantDir('build/core', ARDUINO_CORE) +core_sources = gatherSources(ARDUINO_CORE) +core_sources = filter(lambda x: not (os.path.basename(x) == 'main.cpp'), core_sources) +core_sources = map(lambda x: x.replace(ARDUINO_CORE, 'build/core/'), core_sources) +core_sources_patched = map(lambda x: x.replace('build/core/',"build/core_patched"), core_sources) +envArduino.PatchCore(core_sources_patched,core_sources) + +# add libraries +libCandidates = [] +ptnLib = re.compile(r'^[ ]*#[ ]*include [<"](.*)\.h[>"]') +for line in open (TARGET+FILE_EXTENSION): + result = ptnLib.findall(line) + if result: + # Look for the library directory that contains the header. + filename=result[0]+'.h' + for libdir in ARDUINO_LIBS: + for root, dirs, files in os.walk(libdir, followlinks=True): + for f in files: + if f == filename: + libCandidates += [os.path.basename(root)] + +# Hack. In version 20 of the Arduino IDE, the Ethernet library depends +# implicitly on the SPI library. +if ARDUINO_VER >= 20 and 'Ethernet' in libCandidates: + libCandidates += ['SPI'] + +all_libs_sources = [] +index = 0 +for orig_lib_dir in ARDUINO_LIBS: + lib_sources = [] + lib_dir = 'build/lib_%02d'%index + VariantDir(lib_dir, orig_lib_dir) + for libPath in filter(os.path.isdir, glob(pathJoin(orig_lib_dir, '*'))): + libName = os.path.basename(libPath) + if not libName in libCandidates: + continue + envArduino.Append(CPPPATH = libPath.replace(orig_lib_dir, lib_dir)) + lib_sources = gatherSources(libPath) + utilDir = pathJoin(libPath, 'utility') + if os.path.exists(utilDir) and os.path.isdir(utilDir): + lib_sources += gatherSources(utilDir) + envArduino.Append(CPPPATH = utilDir.replace(orig_lib_dir, lib_dir)) + lib_sources = map(lambda x: x.replace(orig_lib_dir, lib_dir), lib_sources) + all_libs_sources += lib_sources + index += 1 + +# Add raw sources which live in sketch dir. +build_top = os.path.realpath('.') +VariantDir('build/local/', build_top) +local_sources = gatherSources(build_top) +local_sources = map(lambda x: x.replace(build_top, 'build/local/'), local_sources) +if local_sources: + envArduino.Append(CPPPATH = 'build/local') + +# Convert sketch(.pde) to cpp +envArduino.Processing('build/'+TARGET+'.cpp', 'build/'+TARGET+FILE_EXTENSION) +VariantDir('build', '.') + +sources = ['build/'+TARGET+'.cpp'] +#sources += core_sources +sources += local_sources +sources += all_libs_sources + +# Finally Build!! +core_objs = envArduino.Object(core_sources_patched) +objs = envArduino.Object(sources) #, LIBS=libs, LIBPATH='.') +objs = objs + envArduino.CompressCore('build/core.a', core_objs) +envArduino.Elf(TARGET+'.elf', objs) +envArduino.Hex(TARGET+'.hex', TARGET+'.elf') + +# Print Size +# TODO: check binary size +MAX_SIZE = getBoardConf(r'^%s\.upload.maximum_size=(.*)'%ARDUINO_BOARD) +print ("maximum size for hex file: %s bytes"%MAX_SIZE) +envArduino.Command(None, TARGET+'.hex', AVR_BIN_PREFIX+'size --target=ihex $SOURCE') + +# Reset +def pulseDTR(target, source, env): + if ARDUINO_BOARD != 'leonardo': + import serial + import time + print(ARDUINO_PORT) + ser = serial.Serial(ARDUINO_PORT) + ser.setDTR(1) + time.sleep(0.5) + ser.setDTR(0) + ser.close() + +if RST_TRIGGER: + reset_cmd = '%s %s'%(RST_TRIGGER, ARDUINO_PORT) +else: + reset_cmd = pulseDTR + +# Upload +UPLOAD_PROTOCOL = getBoardConf(r'^%s\.upload\.protocol=(.*)'%ARDUINO_BOARD) +UPLOAD_SPEED = getBoardConf(r'^%s\.upload\.speed=(.*)'%ARDUINO_BOARD) + +if UPLOAD_PROTOCOL == 'stk500': + UPLOAD_PROTOCOL = 'stk500v1' + + +avrdudeOpts = ['-V', '-vv', '-D', '-F', '-c %s'%UPLOAD_PROTOCOL, '-b %s'%UPLOAD_SPEED, + '-p %s'%MCU, '-P %s'%ARDUINO_PORT, '-U flash:w:$SOURCES'] +if AVRDUDE_CONF: + avrdudeOpts += ['-C %s'%AVRDUDE_CONF] + +fuse_cmd = '%s %s'%(pathJoin(os.path.dirname(AVR_BIN_PREFIX), 'avrdude'), + ' '.join(avrdudeOpts)) + +upload = envArduino.Alias('upload', TARGET+'.hex', [reset_cmd, fuse_cmd]); + +AlwaysBuild(upload) + +# Clean build directory +envArduino.Clean('all', 'build/') + +# vim: et sw=4 fenc=utf-8: diff --git a/arduino/arduino-1.0 b/arduino/arduino-1.0 new file mode 100644 index 0000000..ade79a4 --- /dev/null +++ b/arduino/arduino-1.0 @@ -0,0 +1 @@ +../libs/arduino-1.0/ \ No newline at end of file diff --git a/arduino/asserv/PID.c b/arduino/asserv/PID.c new file mode 100644 index 0000000..454c273 --- /dev/null +++ b/arduino/asserv/PID.c @@ -0,0 +1,64 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 15/04/15 * + ****************************************/ + +#include "parameters.h" +#include "PID.h" +#include "compat.h" + +PID_t PID_left, PID_right; + +void PIDInit(PID_t *pid) { + pid->P = 0; + pid->I = 0; + pid->D = 0; + pid->bias = 0; + pid->error_sum = 0; + pid->last_error = 0; + pid->init_done = 0; +} + +void PIDReset(PID_t *pid) { + pid->error_sum = 0; + pid->last_error = 0; + pid->init_done = 0; +} + +void PIDSet(PID_t *pid, float P, float I, float D, float bias) { + I *= PID_I_RATIO; + D *= PID_D_RATIO; + I /= HZ; + D *= HZ; + pid->P = P; + pid->I = I; + pid->D = D; + pid->bias = bias; + PIDReset(pid); +} + +float PIDCompute(PID_t *pid, float error) { + float error_D, bias, P_part, I_part, D_part; + + if(!pid->init_done){ + //Lors du premier compute, on ne tient pas compte de D + error_D = 0; + pid->init_done = 1; + } else { + //derivée = deltaErreur/dt - dt est la période de compute + error_D = (error - pid->last_error); + } + + pid->error_sum = (pid->error_sum) + error; + pid->last_error = error; + + //calcul de la sortie avec le PID + bias = pid->bias; + P_part = pid->P * error; + I_part = pid->I * pid->error_sum; + D_part = pid->D * error_D; + pid->output = bias + P_part + I_part + D_part; + + return pid->output; +} diff --git a/arduino/asserv/PID.h b/arduino/asserv/PID.h new file mode 100644 index 0000000..865ff7c --- /dev/null +++ b/arduino/asserv/PID.h @@ -0,0 +1,23 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 15/04/15 * + ****************************************/ +#ifndef PID_H +#define PID_H + +typedef struct PID { + float P, I, D, bias; + float error_sum, last_error; + float output; + int init_done; +} PID_t; + + +extern PID_t PID_left, PID_right; +void PIDInit(PID_t *pid); +void PIDReset(PID_t *pid); +void PIDSet(PID_t *pid, float P, float I, float D, float bias); +float PIDCompute(PID_t *pid, float error); + +#endif diff --git a/arduino/asserv/asserv.ino b/arduino/asserv/asserv.ino new file mode 100644 index 0000000..dd47061 --- /dev/null +++ b/arduino/asserv/asserv.ino @@ -0,0 +1,96 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ + +#include +#include "block.h" +#include "compat.h" +#include "parameters.h" +#include "protocol.h" +#include "control.h" +#include "pins.h" +#include "emergency.h" + +unsigned long nextTime = 0; + +#ifdef PIN_JACK +int JackCheck(void) { + static int last_jack_status = 1; + int i, jack_status, sent_bytes = 0; + jack_status = digitalRead(PIN_JACK); + digitalWrite(LED_JACK, !jack_status); + if (last_jack_status == 0 && jack_status == 1) { + for (i=0; i 10000000) { + digitalWrite(LED_DEBUG, LOW); + } + if (now >= nextTime) { + start_overtime = micros(); + digitalWrite(LED_DEBUG, HIGH); + } +#endif + while (micros() < nextTime); +} diff --git a/arduino/asserv/block.c b/arduino/asserv/block.c new file mode 100644 index 0000000..1a1858d --- /dev/null +++ b/arduino/asserv/block.c @@ -0,0 +1,44 @@ +#include "parameters.h" +#include "block.h" +#include "robotstate.h" +#include "goals.h" +#include "compat.h" + +void ComputeIsBlocked(void) { +#if BLOCK_TIME + static int last_goal_nr = -1; + static long last_time = 0; + static pos_t last_pos = {0, 0, 0, 0}; + long now; + float dist; + goal_t *current_goal; + + now = timeMillis(); + if (now - last_time < BLOCK_TIME) + return; + last_time = now; + + current_goal = FifoCurrentGoal(); + if (current_goal->type == NO_GOAL || + current_goal->type == TYPE_PWM || + current_goal->type == TYPE_SPD) + goto end; + if (fifo.current_goal != last_goal_nr) { + last_goal_nr = fifo.current_goal; + goto end; + } + + // goals type is pos or angle, goal didn't change and + // last calculation was at least BLOCK_TIME ms ago + + dist = sqrt(pow(current_pos.x - last_pos.x, 2) + pow(current_pos.y - last_pos.y, 2)); + dist += abs(current_pos.angle - last_pos.angle)*ENTRAXE_ENC/2.0; + if (dist < BLOCK_MIN_DIST) { + // we did not move enough, we are probably blocked, + // consider the goal reached + current_goal->is_reached = 1; + } +end: + last_pos = current_pos; +#endif +} diff --git a/arduino/asserv/block.h b/arduino/asserv/block.h new file mode 100644 index 0000000..fbd6673 --- /dev/null +++ b/arduino/asserv/block.h @@ -0,0 +1,10 @@ +#ifndef BLOCK_H +#define BLOCK_H + +#ifdef __cplusplus +extern "C" void ComputeIsBlocked(void); +#else +void ComputeIsBlocked(void); +#endif + +#endif diff --git a/arduino/asserv/compat.cpp b/arduino/asserv/compat.cpp new file mode 100644 index 0000000..b4adfb7 --- /dev/null +++ b/arduino/asserv/compat.cpp @@ -0,0 +1,23 @@ +#include "compat.h" +#include "parameters.h" + +extern "C" void serial_print_int(int i) { + SERIAL_MAIN.print(i); +} + +extern "C" void serial_print_float(float f) { + SERIAL_MAIN.print(f); +} + +extern "C" void serial_print(const char *str) { + SERIAL_MAIN.print(str); +} + +extern "C" void serial_send(char data) { //Envoi d'un octet en serial, dépend de la plateforme + SERIAL_MAIN.write(data); +} + +extern "C" char generic_serial_read(){ + return SERIAL_MAIN.read(); +} + diff --git a/arduino/asserv/compat.h b/arduino/asserv/compat.h new file mode 100644 index 0000000..9028925 --- /dev/null +++ b/arduino/asserv/compat.h @@ -0,0 +1,70 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef COMPAARDUINO_H +#define COMPAARDUINO_H + +#include +#include "encoder.h" +#include "parameters.h" +#include "pins.h" + +extern inline unsigned long timeMillis(){ + return millis(); +} +extern inline unsigned long timeMicros(){ + return micros(); +} + +#ifdef __cplusplus +extern "C" void serial_print_int(int i); +extern "C" char generic_serial_read(); +extern "C" void serial_send(char data); +extern "C" void serial_print(const char *str); +extern "C" void serial_print_float(float f); +#else +void serial_print_int(int i); +char generic_serial_read(); +void serial_send(char data); +void serial_print(const char *str); +void serial_print_float(float f); +#endif + +extern inline void initPins(){ + //set mode des pins pour arduino + pinMode(PIN_ENC_LEFT_A,INPUT_PULLUP); + pinMode(PIN_ENC_LEFT_B,INPUT_PULLUP); + pinMode(PIN_ENC_RIGHT_A,INPUT_PULLUP); + pinMode(PIN_ENC_RIGHT_B,INPUT_PULLUP); + + pinMode(LED_DEBUG, OUTPUT); + pinMode(LED_MAINLOOP, OUTPUT); + pinMode(LED_BLOCKED, OUTPUT) ; + + digitalWrite(LED_DEBUG, LOW); //LOW = eteinte + digitalWrite(LED_MAINLOOP, LOW); //LOW = eteinte + digitalWrite(LED_BLOCKED, LOW); //LOW = eteinte + //Definition des interruptions arduino en fonction du type d'évaluation +#ifdef LED_JACK + pinMode(LED_JACK, OUTPUT); +#endif +#if ENCODER_EVAL == 4 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_LEFT_B,leftInterruptB,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_B,rightInterruptB,CHANGE); +#elif ENCODER_EVAL == 2 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,CHANGE); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,CHANGE); +#elif ENCODER_EVAL == 1 + attachInterrupt(INTERRUPT_ENC_LEFT_A,leftInterruptA,RISING); + attachInterrupt(INTERRUPT_ENC_RIGHT_A,rightInterruptA,RISING); +#endif +#ifdef PIN_JACK + pinMode(PIN_JACK,INPUT_PULLUP); +#endif +} + +#endif diff --git a/arduino/asserv/control.c b/arduino/asserv/control.c new file mode 100644 index 0000000..095c473 --- /dev/null +++ b/arduino/asserv/control.c @@ -0,0 +1,336 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ + +#include "encoder.h" +#include "robotstate.h" +#include "goals.h" +#include "control.h" +#include "compat.h" +#include "motor.h" +#include "local_math.h" +#include "emergency.h" +#include + +#define ANG_REACHED (0x1) +#define POS_REACHED (0x2) +#define REACHED (ANG_REACHED | POS_REACHED) + +#define sign(x) ((x)>=0?1:-1) + +control_t control; + +void goalPos(goal_t *goal); +void goalPwm(goal_t *goal); +void goalSpd(goal_t *goal); +void goalAngle(goal_t *goal); +int controlPos(float dd, float da); + +float calcSpeed(float init_spd, float dd, float max_spd, float final_speed); +void applyPID(void); +void applyPwm(void); +void allStop(void); +void stopRobot(void); + +void ControlSetStop(int mask) { + control.status_bits |= mask; +} + +void ControlUnsetStop(int mask) { + control.status_bits &= ~mask; +} + +void ControlInit(void) { + control.reset = 1; + control.status_bits = 0; + control.speeds.angular_speed = 0, + control.speeds.linear_speed = 0; + control.last_finished_id = 0; + + control.max_acc = ACC_MAX; + control.max_spd = SPD_MAX; + control.rot_spd_ratio = RATIO_ROT_SPD_MAX; + + MotorsInit(); + RobotStateInit(); + FifoInit(); + PIDInit(&PID_left); + PIDInit(&PID_right); + PIDSet(&PID_left, LEFT_P, LEFT_I, LEFT_D, LEFT_BIAS); + PIDSet(&PID_right, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_BIAS); +} + +void ControlReset(void) { + control.speeds.linear_speed = 0; + control.last_finished_id = 0; + FifoClearGoals(); + RobotStateReset(); + ControlPrepareNewGoal(); +} + +void ControlPrepareNewGoal(void) { + control.order_started = 0; + PIDReset(&PID_left); + PIDReset(&PID_right); +} + +void ControlCompute(void) { +#if TIME_BETWEEN_ORDERS + static long time_reached = -1; + long now; + now = timeMicros(); +#endif + goal_t* current_goal = FifoCurrentGoal(); + RobotStateUpdate(); + + // clear emergency everytime, it will be reset if necessary + ControlUnsetStop(EMERGENCY_BIT); + ControlUnsetStop(SLOWGO_BIT); + + if (abs(control.speeds.linear_speed) > 1) { + int direction; + if (control.speeds.linear_speed > 0) { + direction = EM_FORWARD; + } else { + direction = EM_BACKWARD; + } + + if (emergency_status[direction].phase == FIRST_STOP) { + ControlSetStop(EMERGENCY_BIT); + } else if (emergency_status[direction].phase == SLOW_GO) { + ControlSetStop(SLOWGO_BIT); + } + } + + + if (control.status_bits & EMERGENCY_BIT || + control.status_bits & PAUSE_BIT || + control.status_bits & TIME_ORDER_BIT) { + stopRobot(); + } else { + switch (current_goal->type) { + case TYPE_ANG: + goalAngle(current_goal); + break; + case TYPE_POS: + goalPos(current_goal); + break; + case TYPE_PWM: + goalPwm(current_goal); + break; + case TYPE_SPD: + goalSpd(current_goal); + break; + default: + stopRobot(); + break; + } + } + + applyPwm(); + + if (current_goal->is_reached) { + control.last_finished_id = current_goal->ID; + FifoNextGoal(); + ControlPrepareNewGoal(); +#if TIME_BETWEEN_ORDERS + time_reached = now; + } + if (time_reached > 0 && (now - time_reached) < TIME_BETWEEN_ORDERS*1000000) { + ControlSetStop(TIME_ORDER_BIT); + } else { + ControlUnsetStop(TIME_ORDER_BIT); + time_reached = -1; +#endif + } +} + +/* INTERNAL FUNCTIONS */ + +void goalPwm(goal_t *goal) { + static long start_time; + long now = timeMicros(); + if (!control.order_started){ + start_time = now; + control.order_started = 1; + } + if ((now - start_time)/1000.0 <= goal->data.pwm_data.time){ + float pwmR, pwmL; + pwmL = goal->data.pwm_data.pwm_l; + pwmR = goal->data.pwm_data.pwm_r; + + control.speeds.pwm_left = pwmL; + control.speeds.pwm_right = pwmR; + } + else { + control.speeds.pwm_left = 0; + control.speeds.pwm_right = 0; + goal->is_reached = 1; + } +} + +void goalSpd(goal_t *goal) { + static long start_time; + long now = timeMicros(); + if (!control.order_started){ + start_time = now; + control.order_started = 1; + } + if ((now - start_time)/1000.0 <= goal->data.spd_data.time){ + float time_left, v_dec; + time_left = (goal->data.spd_data.time - ((now - start_time)/1000.0)) / 1000.0; + v_dec = time_left * control.max_acc; + + control.speeds.linear_speed = min(min( + control.speeds.linear_speed+DT*control.max_acc, + goal->data.spd_data.lin), + v_dec); + control.speeds.angular_speed = min(min( + control.speeds.angular_speed+DT*control.max_acc, + goal->data.spd_data.ang), + v_dec); + } + else { + control.speeds.linear_speed = 0; + control.speeds.angular_speed = 0; + goal->is_reached = 1; + } + applyPID(); +} + +void goalAngle(goal_t *goal) { + float angle, da; + angle = goal->data.ang_data.angle; + da = angle - current_pos.angle; + + if (goal->data.ang_data.modulo) { + da = moduloTwoPI(da); + } + + if (controlPos(0, da) & ANG_REACHED) { + goal->is_reached = 1; + } + applyPID(); +} + +void goalPos(goal_t *goal) { + int x, y; + float dx, dy, da, dd, goal_a; + + x = goal->data.pos_data.x; + y = goal->data.pos_data.y; + dx = x - current_pos.x; + dy = y - current_pos.y; + goal_a = atan2(dy, dx); + da = (goal_a - current_pos.angle); + da = moduloTwoPI(da); + dd = sqrt(pow(dx, 2.0)+pow(dy, 2.0)); + + if (goal->data.pos_data.d == ANY) { + if (abs(da) > CONE_ALIGNEMENT) { + da = moduloPI(da); + dd = - dd; + } + } else if (goal->data.pos_data.d == BACKWARD) { + dd = - dd; + da = moduloTwoPI(da+M_PI); + } + + if (controlPos(dd, da) & POS_REACHED) { + goal->is_reached = 1; + } + applyPID(); +} + +int controlPos(float dd, float da) { + int ret; + float dda, ddd, max_speed; + float ang_spd, lin_spd; + + dda = da * (ENTRAXE_ENC/2); + ddd = dd * exp(-abs(K_DISTANCE_REDUCTION*da)); + + max_speed = control.max_spd; + if (control.status_bits & SLOWGO_BIT) { + max_speed *= EMERGENCY_SLOW_GO_RATIO; + } + + ang_spd = control.speeds.angular_speed; + lin_spd = control.speeds.linear_speed; + + control.speeds.angular_speed = calcSpeed(ang_spd, dda, + max_speed * control.rot_spd_ratio, 0); + control.speeds.linear_speed = calcSpeed(lin_spd, ddd, + max_speed, 0); + + ret = 0; + if (abs(dd) < ERROR_POS) { + ret |= POS_REACHED; + } + if (abs(da) < ERROR_ANGLE) { + ret |= ANG_REACHED; + } + + return ret; +} + +float calcSpeed(float init_spd, float dd, float max_spd, float final_speed) { + float dd_abs, acc_spd, dec_spd, target_spd; + int d_sign; + dd_abs = abs(dd); + d_sign = sign(dd); + + init_spd *= d_sign; + acc_spd = init_spd + (control.max_acc*DT); + dec_spd = sqrt(pow(final_speed, 2) + 2*control.max_acc*dd_abs); + target_spd = min(max_spd, min(acc_spd, dec_spd))*d_sign; + return target_spd; +} + +void stopRobot(void) { + int sign; + float speed; + + sign = sign(control.speeds.angular_speed); + speed = abs(control.speeds.angular_speed); + speed -= control.max_acc * DT; + speed = max(0, speed); + control.speeds.angular_speed = speed; + + sign = sign(control.speeds.linear_speed); + speed = abs(control.speeds.linear_speed); + speed -= control.max_acc * DT; + speed = max(0, speed); + control.speeds.linear_speed = sign*speed; + + if (abs(wheels_spd.left) + abs(wheels_spd.right) < SPD_TO_STOP) { + allStop(); + } else { + applyPID(); + } +} + +void allStop(void) { + control.speeds.pwm_left = 0; + control.speeds.pwm_right = 0; + control.speeds.linear_speed = 0; + control.speeds.angular_speed = 0; +} + +void applyPwm(void) { + set_pwm_left(control.speeds.pwm_left); + set_pwm_right(control.speeds.pwm_right); +} + +void applyPID(void) { + float left_spd, right_spd; + float left_ds, right_ds; + left_spd = control.speeds.linear_speed - control.speeds.angular_speed; + right_spd = control.speeds.linear_speed + control.speeds.angular_speed; + left_ds = left_spd - wheels_spd.left; + right_ds = right_spd - wheels_spd.right; + control.speeds.pwm_left = PIDCompute(&PID_left, left_ds); + control.speeds.pwm_right = PIDCompute(&PID_right, right_ds); +} diff --git a/arduino/asserv/control.h b/arduino/asserv/control.h new file mode 100644 index 0000000..44fbc98 --- /dev/null +++ b/arduino/asserv/control.h @@ -0,0 +1,48 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 29/11/13 * + ****************************************/ +#ifndef CONTROL_H +#define CONTROL_H + +#include "PID.h" +#include "parameters.h" + +#define PAUSE_BIT (1<<0) +#define EMERGENCY_BIT (1<<1) +#define SLOWGO_BIT (1<<2) +#define TIME_ORDER_BIT (1<<3) + +#define ANY 0 +#define FORWARD 1 +#define BACKWARD -1 + +typedef struct control { + struct speeds { + int pwm_left, pwm_right; + float angular_speed, linear_speed; + } speeds; + float max_acc, max_spd, rot_spd_ratio; + int reset, last_finished_id; + int order_started; + int status_bits; +} control_t; + +extern control_t control; +extern PID_t PID_left, PID_right; + +void ControlPrepareNewGoal(void); +void ControlReset(void); +void ControlSetStop(int mask); +void ControlUnsetStop(int mask); + +#ifdef __cplusplus +extern "C" void ControlInit(void); +extern "C" void ControlCompute(void); +#else +void ControlInit(void); +void ControlCompute(void); +#endif + +#endif diff --git a/arduino/asserv/emergency.c b/arduino/asserv/emergency.c new file mode 100644 index 0000000..1feffc9 --- /dev/null +++ b/arduino/asserv/emergency.c @@ -0,0 +1,73 @@ +#include "emergency.h" +#include "pins.h" +#include "compat.h" +#include "parameters.h" +#include "Arduino.h" + +emergency_status_t emergency_status[2] = { + {.phase = NO_EMERGENCY, .in_use = USE_SHARP, .total_time = 0}, + {.phase = NO_EMERGENCY, .in_use = USE_SHARP, .total_time = 0}, +}; + +void ComputeEmergencyOnPin(int pin, emergency_status_t *status); + +void EmergencySetStatus(int enable) { + emergency_status[EM_FORWARD].in_use = enable; + emergency_status[EM_BACKWARD].in_use = enable; + if (!enable) { + emergency_status[EM_FORWARD].phase = NO_EMERGENCY; + emergency_status[EM_BACKWARD].phase = NO_EMERGENCY; + } +} + +void ComputeEmergency(void) { +#ifdef PIN_SHARP_FORWARD + ComputeEmergencyOnPin(PIN_SHARP_FORWARD, &emergency_status[EM_FORWARD]); +#endif +#ifdef PIN_SHARP_BACKWARD + ComputeEmergencyOnPin(PIN_SHARP_BACKWARD, &emergency_status[EM_BACKWARD]); +#endif +} + +void ComputeEmergencyOnPin(int pin, emergency_status_t *status) { + float analog, distance, voltage; + long now; + + if (!status->in_use) + return; + + now = timeMillis(); + analog = analogRead(pin); + voltage = 5.0*analog/1023.0; + if (voltage == 0) { + distance = 1000; + } else { + distance = 0.123/voltage; + } + switch (status->phase) { + case NO_EMERGENCY: + if (distance < EMERGENCY_STOP_DISTANCE) { + if (status->start_detection_time < 0) { + status->start_detection_time = now; + } else if (now - status->start_detection_time > 300) { + status->start_detection_time = -1; + status->phase = FIRST_STOP; + } + } + break; + case FIRST_STOP: + status->total_time += DT; + if (distance > EMERGENCY_STOP_DISTANCE) { + status->phase = NO_EMERGENCY; + } + if ((status->total_time) > (EMERGENCY_WAIT_TIME)) { + status->phase = SLOW_GO; + } + break; + case SLOW_GO: + if (distance > EMERGENCY_STOP_DISTANCE) { + status->phase = NO_EMERGENCY; + } + break; + } +} diff --git a/arduino/asserv/emergency.h b/arduino/asserv/emergency.h new file mode 100644 index 0000000..1f14822 --- /dev/null +++ b/arduino/asserv/emergency.h @@ -0,0 +1,30 @@ +#ifndef EMERGENCY_H +#define EMERGENCY_H + +#define EM_FORWARD 0 +#define EM_BACKWARD 1 + +typedef enum emergency_phase { + NO_EMERGENCY, + FIRST_STOP, + SLOW_GO +} emergency_phase_t; + +typedef struct emergency_status { + int in_use; + long start_detection_time; + double total_time; + emergency_phase_t phase; +} emergency_status_t; + +extern emergency_status_t emergency_status[2]; + +#ifdef __cplusplus +extern "C" void EmergencySetStatus(int enable); +extern "C" void ComputeEmergency(void); +#else +void EmergencySetStatus(int enable); +void ComputeEmergency(void); +#endif + +#endif diff --git a/arduino/asserv/encoder.c b/arduino/asserv/encoder.c new file mode 100644 index 0000000..47e978b --- /dev/null +++ b/arduino/asserv/encoder.c @@ -0,0 +1,119 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/04/15 * + ****************************************/ +#include "encoder.h" +#include "compat.h" +#include "pins.h" + +volatile long left_ticks = 0; +volatile long right_ticks = 0; + +int left_last_value_A = 0; +int left_last_value_B = 0; +int right_last_value_A = 0; +int right_last_value_B = 0; + +void left_encoder_reset(void) { + left_ticks = 0; +} + +void right_encoder_reset(void) { + right_ticks = 0; +} + +#if ENCODER_EVAL == 4 +inline void interruptA(volatile long *ticks, int *last_value_A, int last_value_B, int pin) { + int new_value; + new_value = digitalRead(pin); + if(new_value == 1) + if(last_value_B == 1) + (*ticks)--; + else + (*ticks)++; + + else + if(last_value_B == 1) + (*ticks)++; + else + (*ticks)--; + *last_value_A = new_value; +} + +inline void interruptB(volatile long *ticks, int *last_value_B, int last_value_A, int pin) { + bool new_value; + new_value = digitalRead(pin); + if(new_value == 1) + if(last_value_A == 1) + (*ticks)++; + else + (*ticks)--; + + else + if(last_value_A == 1) + (*ticks)--; + else + (*ticks)++; + *last_value_B = new_value; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, &left_last_value_A, + left_last_value_B, PIN_ENC_LEFT_A); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, &right_last_value_A, + right_last_value_B, PIN_ENC_RIGHT_A); +} + +void leftInterruptB(void) { + interruptA(&left_ticks, &left_last_value_B, + left_last_value_A, PIN_ENC_LEFT_A); +} + +void rightInterruptB(void) { + interruptA(&right_ticks, &right_last_value_B, + right_last_value_A, PIN_ENC_RIGHT_A); +} +#elif ENCODER_EVAL == 2 +void interruptA(volatile long *ticks, int pin_a, int pin_b){ + int value_A, value_B; + value_A = digitalRead(pin_a); + value_B = digitalRead(pin_b); + if(value_A == 1) + if(value_B == 1) + (*ticks)--; + else + (*ticks)++; + else + if(value_B == 1) + (*ticks)++; + else + (*ticks)--; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, PIN_ENC_LEFT_A, PIN_ENC_LEFT_B); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, PIN_ENC_RIGHT_A, PIN_ENC_RIGHT_B); +} +#elif ENCODER_EVAL == 1 +void interruptA(volatile long *ticks, int pin_b){ + if(digitalRead(pin_b) == 1) + (*ticks)--; + else + (*ticks)++; +} + +void leftInterruptA(void) { + interruptA(&left_ticks, PIN_ENC_LEFT_B); +} + +void rightInterruptA(void) { + interruptA(&right_ticks, PIN_ENC_RIGHT_B); +} +#endif diff --git a/arduino/asserv/encoder.h b/arduino/asserv/encoder.h new file mode 100644 index 0000000..9a16f73 --- /dev/null +++ b/arduino/asserv/encoder.h @@ -0,0 +1,38 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/04/15 * + ****************************************/ +#ifndef ENCODER_H +#define ENCODER_H + +#include "parameters.h" + +extern volatile long left_ticks; +extern volatile long right_ticks; + +void left_encoder_reset(void); +void right_encoder_reset(void); + +extern inline void encoders_reset(void) { + left_encoder_reset(); + right_encoder_reset(); +} + +#ifdef __cplusplus +extern "C" void leftInterruptA(void); +extern "C" void rightInterruptA(void); +#if ENCODER_EVAL == 4 +void leftInterruptB(void); +void rightInterruptB(void); +#endif +#else +void leftInterruptA(void); +void rightInterruptA(void); +#if ENCODER_EVAL == 4 +void leftInterruptB(void); +void rightInterruptB(void); +#endif +#endif + +#endif diff --git a/arduino/asserv/goals.c b/arduino/asserv/goals.c new file mode 100644 index 0000000..668ec04 --- /dev/null +++ b/arduino/asserv/goals.c @@ -0,0 +1,57 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#include "goals.h" + +fifo_t fifo; + +void FifoInit() { + int i; + fifo.nb_goals = 0; + fifo.current_goal = 0; + fifo.last_goal = -1; + for (i=0; i= MAX_GOALS) { + return -1; + } + + fifo.last_goal = (fifo.last_goal + 1) % MAX_GOALS; + new_goal = &fifo.fifo[fifo.last_goal]; + + new_goal->type = type; + new_goal->data = data; + new_goal->ID = ID; + new_goal->is_reached = 0; + + fifo.nb_goals++; + return 0; + +} + +goal_t* FifoCurrentGoal() { + return &fifo.fifo[fifo.current_goal]; +} + +goal_t* FifoNextGoal() { + goal_t *current_goal = FifoCurrentGoal(); +#if KEEP_LAST_GOAL + if (current_goal->type != NO_GOAL && + fifo.nb_goals > 1) { +#else + if (current_goal->type != NO_GOAL) { +#endif + current_goal->type = NO_GOAL; + current_goal->is_reached = 0; + fifo.current_goal = (fifo.current_goal + 1) % MAX_GOALS; + fifo.nb_goals--; + } + return FifoCurrentGoal(); +} diff --git a/arduino/asserv/goals.h b/arduino/asserv/goals.h new file mode 100644 index 0000000..1de0622 --- /dev/null +++ b/arduino/asserv/goals.h @@ -0,0 +1,72 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef GOALS_H +#define GOALS_H + +#include "parameters.h" +#define MAX_GOALS 15 //nombre max de goals dans la file, évite surcharge mémoire + +#define TYPE_POS 1 +#define TYPE_ANG 2 +#define TYPE_PWM 3 +#define TYPE_SPD 4 +#define NO_GOAL -1 +#define STRUCT_NO_GOAL {.type = NO_GOAL, .is_reached=0} + +#define POS_DATA(px,py,direction) ((goal_data_t){ .pos_data=(pos_data_t){.x=px, .y=py, .d=direction}}) +#define ANG_DATA(a,m) ((goal_data_t){ .ang_data=(ang_data_t){.angle=a, .modulo=m}}) +#define PWM_DATA(l,r,t) ((goal_data_t){ .pwm_data=(pwm_data_t){.pwm_l=l, .pwm_r=r, .time=t}}) +#define SPD_DATA(l,a,t) ((goal_data_t){ .spd_data=(spd_data_t){.lin=l, .ang=a, .time=t}}) + +typedef struct pos_data { + int x, y, d; +} pos_data_t; + +typedef struct ang_data { + float angle; + int modulo; +} ang_data_t; + +typedef struct pwm_data { + float time; + int pwm_l, pwm_r; +} pwm_data_t; + +typedef struct spd_data { + float time; + int lin, ang; +} spd_data_t; + +typedef union goal_data { + pos_data_t pos_data; + ang_data_t ang_data; + pwm_data_t pwm_data; + spd_data_t spd_data; +} goal_data_t; + +typedef struct goal { + goal_data_t data; + int type; + int ID; + int is_reached; +} goal_t; + +typedef struct fifo { + goal_t fifo[MAX_GOALS]; + int nb_goals; + int current_goal; + int last_goal; + +} fifo_t; + +extern fifo_t fifo; +void FifoInit(); +int FifoPushGoal(int ID, int type, goal_data_t data); +goal_t* FifoCurrentGoal(); +goal_t* FifoNextGoal(); +extern inline void FifoClearGoals() { FifoInit(); } + +#endif diff --git a/arduino/asserv/local_math.c b/arduino/asserv/local_math.c new file mode 100644 index 0000000..024da2d --- /dev/null +++ b/arduino/asserv/local_math.c @@ -0,0 +1,28 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 25/10/13 * + ****************************************/ + +#include "local_math.h" +#include + +float moduloTwoPI(float angle){ + if(angle >= 0) + while(angle > M_PI) + angle -= 2.0*M_PI; + else + while(angle <= -M_PI) + angle += 2.0*M_PI; + return angle; +} + +float moduloPI(float angle){ + if(angle >= 0) + while(angle > M_PI/2) + angle -= M_PI; + else + while(angle <= -M_PI/2) + angle += M_PI; + return angle; +} diff --git a/arduino/asserv/local_math.h b/arduino/asserv/local_math.h new file mode 100644 index 0000000..2f412da --- /dev/null +++ b/arduino/asserv/local_math.h @@ -0,0 +1,15 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 25/10/13 * + ****************************************/ + +#ifndef LOCAL_MATH_H +#define LOCAL_MATH_H + +#define MAX(a,b) a > b ? a : b + +float moduloTwoPI(float angle); +float moduloPI(float angle); + +#endif diff --git a/arduino/asserv/pins.h b/arduino/asserv/pins.h new file mode 100644 index 0000000..ebb4ebf --- /dev/null +++ b/arduino/asserv/pins.h @@ -0,0 +1,59 @@ +#ifndef PINS_H +#define PINS_H + +// nano +#ifdef __AVR_ATmega328P__ +#define MOTOR1_EN 7 +#define MOTOR2_EN 12 + +#define MOTOR1_SPD 9 +#define MOTOR2_SPD 10 + +#define MOTOR1_BRK 8 +#define MOTOR2_BRK 11 + +#define PIN_ENC_LEFT_A 2 +#define PIN_ENC_LEFT_B 5 +#define PIN_ENC_RIGHT_A 3 +#define PIN_ENC_RIGHT_B 4 + +#define INTERRUPT_ENC_LEFT_A 0 +#define INTERRUPT_ENC_RIGHT_A 1 + +#define LED_MAINLOOP 14 +#define LED_DEBUG 13 +#define LED_BLOCKED 14 +#endif + +// mega +#ifdef __AVR_ATmega2560__ +#define MOTOR1_EN 30 +#define MOTOR2_EN 34 + +#define MOTOR1_SPD 3 +#define MOTOR2_SPD 2 + +#define MOTOR1_RDY 32 +#define MOTOR2_RDY 36 + +#define PIN_ENC_LEFT_A 19 +#define PIN_ENC_LEFT_B 18 +#define PIN_ENC_RIGHT_A 20 +#define PIN_ENC_RIGHT_B 21 + +#define PIN_SHARP_FORWARD 15 + +#define INTERRUPT_ENC_LEFT_A 4 +#define INTERRUPT_ENC_LEFT_B 5 +#define INTERRUPT_ENC_RIGHT_A 3 +#define INTERRUPT_ENC_RIGHT_B 2 + +#define PIN_JACK 52 +#define LED_JACK 51 + +#define LED_DEBUG 14 +#define LED_BLOCKED 15 +#define LED_MAINLOOP 16 +#endif + +#endif diff --git a/arduino/asserv/protocol.c b/arduino/asserv/protocol.c new file mode 100644 index 0000000..b6f0614 --- /dev/null +++ b/arduino/asserv/protocol.c @@ -0,0 +1,106 @@ +#include +#include "protocol.h" +#include "compat.h" +#include "serial_switch.h" +#include "robotstate.h" +#include "control.h" + +int sendResponse(char order, char *buf, int size, int ID){ + char message[MAX_COMMAND_LEN]; + int wsize = 0; + message[wsize++] = order; + message[wsize++] = ';'; + wsize += sprintf(&message[wsize], "%d", ID); + memcpy(message+wsize, buf, size*sizeof(char)); + wsize += size; + message[wsize++] = '\n'; + message[wsize++] = '\0'; + serial_print(message); + return wsize; +} + +void clean_current_command(char *buffer, int* end_of_cmd) { + memcpy(buffer, buffer+*end_of_cmd, MAX_COMMAND_LEN-*end_of_cmd); + *end_of_cmd = 0; +} + +void autoSendStatus(void) { + char message[MAX_COMMAND_LEN]; + int index = 0; + index += sprintf(message, "%c;%i;%i;%i;%i", + AUTO_SEND, + control.last_finished_id, + (int)current_pos.x, + (int)current_pos.y, + (int)(current_pos.angle*FLOAT_PRECISION)); +#if DEBUG_TARGET_SPEED + index += sprintf(message+index, ";%i;%i;%i;%i", + (int)wheels_spd.left, + (int)wheels_spd.right, + (int)(control.speeds.linear_speed - control.speeds.angular_speed), + (int)(control.speeds.linear_speed + control.speeds.angular_speed)); +#endif + message[index] = '\n'; + message[index+1] = '\0'; + serial_print(message); +} + +void ProtocolAutoSendStatus(int bytes_left) { +#if AUTO_STATUS_HZ + static int i=0; + if (++i % (HZ / AUTO_STATUS_HZ) == 0) { + if (bytes_left >= MAX_AUTOSEND_SIZE) { + autoSendStatus(); + i = 0; + } else { + i--; + } + } +#endif +} + +int ProtocolExecuteCmd(char data) { + static char current_command[MAX_COMMAND_LEN]; + static int index = 0; + if (data == '\r') data = '\n'; + current_command[index++] = data; + if (index >= MAX_COMMAND_LEN) { + // epic fail, this MUST NEVER happen + // if ever this happens, the order will be corrupted + // decrease index so the arduino keep going on + // that means we overwrite the last received char + index = MAX_COMMAND_LEN-1; + } + if (data == '\n') { + // end of current command + char order = current_command[0]; + char response[MAX_RESPONSE_LEN]; + int id, end_of_id, response_size, sent_size; + current_command[index] = '\0'; + end_of_id = ID_START_INDEX; // start after first ';' + while (current_command[end_of_id] != ';') { + end_of_id++; + if (end_of_id >= MAX_ID_LEN+ID_START_INDEX) { + char message[MAX_COMMAND_LEN]; + int msg_index = 0; + clean_current_command(current_command, &index); + if (order != '\n') { + message[0] = order; + message[1] = ';'; + msg_index = 2; + } + sent_size = sprintf(message+msg_index, "%s\n", FAILED_MSG); + serial_print(message); + return sent_size; + } + } + current_command[end_of_id] = '\0'; + sscanf(¤t_command[ID_START_INDEX], "%i", &id); + + switchOrdre(order, id, ¤t_command[end_of_id+1], response, &response_size); + sent_size = sendResponse(order, response, response_size, id); + clean_current_command(current_command, &index); + return sent_size; + } + return 0; +} diff --git a/arduino/asserv/protocol.h b/arduino/asserv/protocol.h new file mode 100644 index 0000000..da6e84b --- /dev/null +++ b/arduino/asserv/protocol.h @@ -0,0 +1,79 @@ +#ifndef PROTOCOL_H +#define PROTOCOL_H + +#include "parameters.h" + +// COMMANDS : +// 'ordre;id;arg1;arg2;argn' +// For example : +// 'c;3;120;1789;31400' +// 'j;0;' +// issues the order GOTOA with +// ID 3 to X=120, Y=1789 and angle = 3.14 +// +// WARNING : order should be +// ONE ascii char long +// +// float are transmitted as integer +// therefore any number refered as +// "decimal" is actually an int +// multiplied by FLOAT_PRECISION + +// BEGIN_ORDERS - Do not remove this comment +#define GOTOA 'c' // x(int);y(int);a(decimal);direction(int) - (mm and radian), direction is optionnal : 1 is forward, -1 is backward, 0 is any +#define GOTO 'd' // x(int);y(int);direction(int) - (mm), direction is optionnal : 1 is forward, -1 is backward, 0 is any +#define ROT 'e' // a(decimal) - (radian), can't turn more than 1 turn +#define ROTNOMODULO 'a' // a(decimal) - radian, can turn more than 1 turn +#define KILLG 'f' // no args, go to next order +#define CLEANG 'g' // no args, cancel all orders +#define PIDLEFT 'h' // p(decimal);i(decimal);d(decimal) - set left PID +#define PIDRIGHT 'i' // p(decimal);i(decimal);d(decimal) - set right PID +#define PIDALL 'u' // p(decimal);i(decimal);d(decimal) - set both PID +#define GET_CODER 'j' // no args, response : l(long);r(long) +#define PWM 'k' // l(int);r(int);duration(int) - set left and right pwm for duration ms +#define SPD 'b' // l(int);a(int);duration(int) - set linear and angular spd for duration ms +#define ACCMAX 'l' // a(int) - set max acceleration (mm/s-2) +#define SPDMAX 'x' // v(int),r(decimal) - set max spd (mm/s) and rotation ratio +#define SET_POS 'm' // x(int);y(int);a(decimal) - set pos (mm / radians) +#define GET_POS 'n' // no args, response : x(int);y(int);a(decimal) - get current pos (mm and radians) +#define GET_SPD 'y' // no args, respond : l(int);r(int) - get wheels speed (mm/s) +#define GET_TARGET_SPD 'v' // no args, respond : l(int);r(int) - get target wheels speed (mm/s) +#define GET_POS_ID 'o' // no args, response : x(int);y(int);a(decimal);id(int) - get current pos and last id (mm and radians) +#define GET_LAST_ID 't' // no args, response : id(int) +#define PAUSE 'q' // no args, pauses control +#define RESUME 'r' // no args, resumes control +#define RESET_ID 's' // no args, reset last finished id to 0 +#define PINGPING 'z' // no args, switch led state +#define WHOAMI 'w' // no args, answers 'ASSERV' or 'PAP' +#define SETEMERGENCYSTOP 'A'// enable(int) +// END_ORDERS - Do not remove this comment + +#define AUTO_SEND '~' // x(int);y(int);a(decimal) +#define JACK 'J' + +#define JACK_SEND_NR 5 +#define FLOAT_PRECISION 1000.0 +#define FAILED_MSG "FAILED" +#define MAX_COMMAND_LEN 60 +#define MAX_ID_VAL 32767 +#define MAX_ID_LEN 5 +#define ID_START_INDEX 2 +#define MAX_RESPONSE_LEN 50 + +#define MAX_BYTES_PER_IT (0.9*BAUDRATE/(HZ*10)) + +#ifdef DEBUG_TARGET_SPEED +#define MAX_AUTOSEND_SIZE (48) +#else +#define MAX_AUTOSEND_SIZE (24) +#endif + +#ifdef __cplusplus +extern "C" int ProtocolExecuteCmd(char data); +extern "C" void ProtocolAutoSendStatus(int bytes_left); +#else +int ProtocolExecuteCmd(char data); +void ProtocolAutoSendStatus(int bytes_left); +#endif + +#endif diff --git a/arduino/asserv/robotstate.c b/arduino/asserv/robotstate.c new file mode 100644 index 0000000..1dad41d --- /dev/null +++ b/arduino/asserv/robotstate.c @@ -0,0 +1,85 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#include "robotstate.h" +#include "compat.h" +#include "local_math.h" +#include "encoder.h" +#include + +#define FC (2) +#define RC (1/(2*PI*FC)) +#define ALPHA (DT / (RC + DT)) + +pos_t current_pos; +wheels_spd_t wheels_spd; + +void PosUpdateAngle() { + if (current_pos.angle > M_PI) { + current_pos.angle -= 2.0*M_PI; + current_pos.modulo_angle++; + } + else if (current_pos.angle <= -M_PI) { + current_pos.angle += 2.0*M_PI; + current_pos.modulo_angle--; + } +} + +void RobotStateInit() { + current_pos.x = 0; + current_pos.y = 0; + current_pos.angle = 0; + current_pos.modulo_angle = 0; + wheels_spd.left = 0; + wheels_spd.right = 0; + encoders_reset(); +} + +void RobotStateSetPos(float x, float y, float angle) { + current_pos.x = x; + current_pos.y = y; + current_pos.angle = angle; + PosUpdateAngle(); +} + +void lowPass(wheels_spd_t *old_spd, wheels_spd_t *new_spd, float a) { + new_spd->left = old_spd->left + a * (new_spd->left - old_spd->left); + new_spd->right = old_spd->right + a * (new_spd->right - old_spd->right); +} + +void RobotStateUpdate() { + static long left_last_ticks = 0, right_last_ticks = 0; + static float last_angle = 0; + float dd, dl, dr, d_angle; + long lt, rt; + wheels_spd_t old_wheels_spd = wheels_spd; + + lt = left_ticks; + rt = right_ticks; + + dl = (lt - left_last_ticks)*TICKS_TO_MM_LEFT; + dr = (rt - right_last_ticks)*TICKS_TO_MM_RIGHT; + wheels_spd.left = dl * HZ; + wheels_spd.right = dr * HZ; + + // low pass filter on speed + lowPass(&old_wheels_spd, &wheels_spd, ALPHA); + + //d_angle = atan2((dr - dl), ENTRAXE_ENC); //sans approximation tan + d_angle = (dr - dl)/ENTRAXE_ENC; // approximation tan + current_pos.angle += d_angle; +#if MODULO_TWOPI + PosUpdateAngle(); +#endif + + dd = (dr + dl) / 2.0; + current_pos.x += dd*cos((current_pos.angle + last_angle)/2.0); + current_pos.y += dd*sin((current_pos.angle + last_angle)/2.0); + + // prepare la prochaine update + right_last_ticks = rt; + left_last_ticks = lt; + last_angle = current_pos.angle; +} diff --git a/arduino/asserv/robotstate.h b/arduino/asserv/robotstate.h new file mode 100644 index 0000000..9883d6b --- /dev/null +++ b/arduino/asserv/robotstate.h @@ -0,0 +1,44 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 13/10/13 * + ****************************************/ +#ifndef ROBOTSTATE_H +#define ROBOTSTATE_H + +#include "parameters.h" +#include "encoder.h" +#include + +#if ENCODER_EVAL == 4 + #define TICKS_PER_TURN (ENC_RESOLUTION * 4) +#elif ENCODER_EVAL == 2 + #define TICKS_PER_TURN (ENC_RESOLUTION * 2) +#elif ENCODER_EVAL == 1 + #define TICKS_PER_TURN ENC_RESOLUTION +#endif +#define TICKS_TO_MM_LEFT ((float)((2.0*M_PI*ENC_LEFT_RADIUS)/(TICKS_PER_TURN)))// = mm/ticks +#define MM_TO_TICKS_LEFT ((float)(1/ENC_TICKS_TO_MM_LEFT)) +#define TICKS_TO_MM_RIGHT ((float)((2.0*M_PI*ENC_RIGHT_RADIUS)/(TICKS_PER_TURN)))// = mm/ticks +#define MM_TO_TICKS_RIGHT ((float)(1/ENC_TICKS_TO_MM_RIGHT)) + +typedef struct pos { + float x; + float y; + float angle; + int modulo_angle; +} pos_t; + +typedef struct wheels_spd { + float left, right; +} wheels_spd_t; + +extern pos_t current_pos; +extern wheels_spd_t wheels_spd; + +void RobotStateInit(); +void RobotStateUpdate(); +void RobotStateSetPos(float x, float y, float angle); +extern inline void RobotStateReset(void) { RobotStateInit(); }; + +#endif diff --git a/arduino/asserv/serial_switch.c b/arduino/asserv/serial_switch.c new file mode 100644 index 0000000..108e4b3 --- /dev/null +++ b/arduino/asserv/serial_switch.c @@ -0,0 +1,186 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/12/13 * + ****************************************/ + +#include +#include "serial_switch.h" +#include "robotstate.h" +#include "protocol.h" +#include "control.h" +#include "encoder.h" +#include "compat.h" +#include "pins.h" +#include "goals.h" +#include "emergency.h" + +//La fonction renvoit le nombre d'octet dans ret, chaine de caractère de réponse. Si doublon, ne pas executer d'ordre mais renvoyer les données à renvoyer +int switchOrdre(char ordre, int id, char *argv, char *ret, int *ret_size){ + *ret_size = 0; + switch(ordre){ + case PINGPING: + digitalWrite(LED_DEBUG, HIGH); + delay(1); + digitalWrite(LED_DEBUG, LOW); + break; + case GET_CODER: + *ret_size = sprintf(ret, "%li;%li", left_ticks, right_ticks); + break; + case GOTO: { + int x, y, direction; + direction = 0; + sscanf(argv, "%i;%i;%i", &x, &y, &direction); + FifoPushGoal(id, TYPE_POS, POS_DATA(x, y, direction)); + } + break; + case GOTOA: { + int x, y, a_int, direction; + float a; + direction = 0; + sscanf(argv, "%i;%i;%i;%i", &x, &y, &a_int, &direction); + a = a_int / (float)FLOAT_PRECISION; + FifoPushGoal(id, TYPE_POS, POS_DATA(x,y,direction)); + FifoPushGoal(id, TYPE_ANG, ANG_DATA(a,1)); + } + break; + case ROT: { + int a_int; + float a; + sscanf(argv, "%i", &a_int); + a = a_int / (float)FLOAT_PRECISION; + FifoPushGoal(id, TYPE_ANG, ANG_DATA(a,1)); + } + break; + case ROTNOMODULO: { + long a_int; + float a; + sscanf(argv, "%li", &a_int); + a = a_int / (float)FLOAT_PRECISION; + FifoPushGoal(id, TYPE_ANG, ANG_DATA(a,0)); + } + break; + case PWM:{ + int l, r, t; + sscanf(argv, "%i;%i;%i", &l, &r, &t); + FifoPushGoal(id, TYPE_PWM, PWM_DATA(l, r, t)); + } + break; + case SPD:{ + int l, a, t; + sscanf(argv, "%i;%i;%i", &l, &a, &t); + FifoPushGoal(id, TYPE_SPD, SPD_DATA(l, a, t)); + } + break; + case PIDALL: + case PIDRIGHT: + case PIDLEFT:{ + long p_int, i_int, d_int; + float p, i, d; + sscanf(argv, "%li;%li;%li", &p_int, &i_int, &d_int); + p = p_int / (float)FLOAT_PRECISION; + i = i_int / (float)FLOAT_PRECISION; + d = d_int / (float)FLOAT_PRECISION; + if (ordre == PIDLEFT) + PIDSet(&PID_left, p, i, d, LEFT_BIAS); + else if (ordre == PIDRIGHT) + PIDSet(&PID_right, p, i, d, RIGHT_BIAS); + else { + PIDSet(&PID_left, p, i, d, LEFT_BIAS); + PIDSet(&PID_right, p, i, d, RIGHT_BIAS); + } + } + break; + case KILLG: + FifoNextGoal(); + ControlPrepareNewGoal(); + break; + case CLEANG:{ + FifoClearGoals(); + ControlPrepareNewGoal(); + } + break; + case RESET_ID: + control.last_finished_id = 0; + break; + case SET_POS:{ + int x, y, a_int; + float angle; + sscanf(argv, "%i;%i;%i", &x, &y, &a_int); + angle = a_int / (float)FLOAT_PRECISION; + RobotStateSetPos(x, y, angle); + } + break; + case GET_POS:{ + int x, y, a_int; + float a; + a = current_pos.angle; + x = round(current_pos.x); + y = round(current_pos.y); + a_int = a * (float)FLOAT_PRECISION; + *ret_size = sprintf(ret, "%i;%i;%i", x, y, a_int); + } + break; + case GET_SPD: { + int l, r; + l = wheels_spd.left; + r = wheels_spd.right; + *ret_size = sprintf(ret, "%i;%i", l, r); + } + break; + case GET_TARGET_SPD: { + int left_spd, right_spd; + left_spd = control.speeds.linear_speed - control.speeds.angular_speed; + right_spd = control.speeds.linear_speed + control.speeds.angular_speed; + *ret_size = sprintf(ret, "%i;%i", left_spd, right_spd); + } + break; + case GET_POS_ID:{ + int x, y, a_int; + float a; + a = current_pos.angle; + x = round(current_pos.x); + y = round(current_pos.y); + a_int = a * (float)FLOAT_PRECISION; + *ret_size = sprintf(ret, "%i;%i;%i;%i", x, y, a_int, control.last_finished_id); + } + break; + case SPDMAX:{ + int r_int, s; + float r; + sscanf(argv, "%i;%i", &s, &r_int); + r = r_int / (float)FLOAT_PRECISION; + control.max_spd = s; + control.rot_spd_ratio = r; + } + break; + case ACCMAX:{ + int a; + sscanf(argv, "%i", &a); + control.max_acc = a; + } + break; + case GET_LAST_ID: { + *ret_size = sprintf(ret, "%i", control.last_finished_id); + break; + } + case PAUSE: + ControlSetStop(PAUSE_BIT); + break; + case RESUME: + ControlUnsetStop(PAUSE_BIT); + break; + case WHOAMI: + *ret_size = sprintf(ret, ARDUINO_ID); + break; + case SETEMERGENCYSTOP: { + int enable; + sscanf(argv, "%i", &enable); + EmergencySetStatus(enable); + } + break; + default: + return -1;//commande inconnue + } + return 0; +} diff --git a/arduino/asserv/serial_switch.h b/arduino/asserv/serial_switch.h new file mode 100644 index 0000000..6b7d46d --- /dev/null +++ b/arduino/asserv/serial_switch.h @@ -0,0 +1,11 @@ +/**************************************** + * Author : Quentin C * + * Mail : quentin.chateau@gmail.com * + * Date : 18/12/13 * + ****************************************/ +#ifndef SERIAL_SWITCH_H +#define SERIAL_SWITCH_H + +int switchOrdre(char ordre, int id, char *argv, char *ret, int *ret_size);//Si doublon, ne pas executer d'ordre mais renvoyer les données à renvoyer + +#endif diff --git a/arduino/check_protocol b/arduino/check_protocol new file mode 100644 index 0000000..6ebeaf1 --- /dev/null +++ b/arduino/check_protocol @@ -0,0 +1,38 @@ +#!/usr/bin/python + +orders = [] +used_chars = '' +ret = 0 +in_vars = 0 + +f = open('protocol.h') +for l in f: + if in_vars and l[0:7] == "#define": + o = l.split()[1:]; + if o[1][0] != "'" or o[1][2] != "'": + print("Order '"+o[0]+"' seem incorrectly defined. Expected : #define ORDER 'x'") + ret= 1 + continue + else: + o[1] = o[1][1] + + if len(o[1]) == 1: + orders.append(o) + else: + print("Order '"+o[0]+"' seem to be defined with more than 1 char") + ret = 1 + + elif "BEGIN_ORDERS" in l: + in_vars = 1 + elif "END_ORDERS" in l: + in_vars = 0 + +print("Protocol orders :") +for o in orders: + print(o[0]+" : "+o[1]) + if o[1] in used_chars: + print("!!! Order '"+o[0]+"' uses a code already in use") + ret = 1 + used_chars += o[1] + +exit(ret); diff --git a/arduino/dependencies b/arduino/dependencies new file mode 100644 index 0000000..5089f9e --- /dev/null +++ b/arduino/dependencies @@ -0,0 +1,5 @@ +scons +avr-binutils +avr-libc +avr-gcc +avrdude diff --git a/arduino/modele/calcSpeed.m b/arduino/modele/calcSpeed.m new file mode 100644 index 0000000..6180648 --- /dev/null +++ b/arduino/modele/calcSpeed.m @@ -0,0 +1,12 @@ +function [ Vtarget ] = calcSpeed(Vi, Vm, Acc, dd, t) + Dsign = sign(dd); + dd = abs(dd); + Vi = Vi*Dsign; + + Vacc = t*Acc + Vi; + Vdec = -t*Acc + sqrt(2*Acc*dd); + Vmax = Vm*ones(1,length(t)); + Vtarget = min(vertcat(Vacc, Vmax, Vdec)); + Vtarget = Vtarget*Dsign; +end + diff --git a/arduino/modele/plot.png b/arduino/modele/plot.png new file mode 100644 index 0000000..ae31f06 Binary files /dev/null and b/arduino/modele/plot.png differ diff --git a/arduino/modele/plotSimulation.m b/arduino/modele/plotSimulation.m new file mode 100644 index 0000000..242f0bb --- /dev/null +++ b/arduino/modele/plotSimulation.m @@ -0,0 +1,16 @@ +function [] = plotSimulation(Vi, Vm, Acc, dd, HZ, inaccuracy_max, color) + [t, V, d] = simulate(Vi, Vm, Acc, dd, HZ, inaccuracy_max); + subplot(1,2,1); + hold on; + xlabel('Time (s)'); + ylabel('Speed (m/s)'); + plot(t, V, 'Color', color, 'LineWidth', 2); + + subplot(1,2,2); + xlabel('Time (s)'); + ylabel('Position (m)'); + hold on; + plot(t, d(1) - d, 'Color', color, 'LineWidth', 2 ); + line([t(1), t(end)], [d(1), d(1)], 'Color', 'b', 'LineStyle', '--'); +end + diff --git a/arduino/modele/plotSimulation2D.m b/arduino/modele/plotSimulation2D.m new file mode 100644 index 0000000..a6dea5a --- /dev/null +++ b/arduino/modele/plotSimulation2D.m @@ -0,0 +1,39 @@ +function [] = plotSimulation2D(init_pos, Vm, Acc, target_pos, HZ, entraxe, inaccuracy_max, delay, max_angle_to_rotate, K_distance_reduction) + [t, pos, Vl, Vr, Va, Vt] = simulate2D(init_pos, Vm, Acc, target_pos, HZ, entraxe, inaccuracy_max, delay, max_angle_to_rotate, K_distance_reduction ); + figure('Position', [1000, 100, 800, 800]); + indexes = 1:length(t); + subplot(3,2,1); + hold on; + plot(t, Vt(indexes)-Va(indexes), 'r'); + plot(t, Vl(indexes)); + legend('Command', 'Response','Location','northwest'); + hold off; + title('Left wheel speed over time'); + subplot(3,2,2); + hold on; + plot(t, Vt(indexes)+Va(indexes), 'r'); + plot(t, Vr(indexes)); + legend('Command', 'Response','Location','northwest'); + hold off; + title('Right wheel speed over time'); + subplot(3,2,3); + plot(t, pos(indexes,3)); + title('Orientation over time'); + subplot(3,2,4); + hold on; + plot(init_pos(1), init_pos(2), 'or'); + legend('Initial position'); + plot(pos(indexes,1), pos(indexes,2)); + axis(axis); + axis equal; + title('Position (trajectory)'); + subplot(3,2,5); + plot(t, Va(indexes)); + axis(1.1*axis) + title('Angular speed over time'); + subplot(3,2,6); + plot(t, Vt(indexes)); + axis(1.1*axis); + title('Linear speed over time'); +end + diff --git a/arduino/modele/predictMovement.m b/arduino/modele/predictMovement.m new file mode 100644 index 0000000..1d32f78 --- /dev/null +++ b/arduino/modele/predictMovement.m @@ -0,0 +1,14 @@ +function [t, Vtarget, d] = predictMovement(Vi, Vm, Acc, dd, HZ) + + tend = sqrt(2*Acc*abs(dd))/Acc; + t = linspace(0, tend, HZ*(tend)+1); + % Target speed calculation + Vtarget = calcSpeed(Vi, Vm, Acc, dd, t); + + d = zeros(1,1); + for v=Vtarget + d = [d (d(end)+v*(1/HZ))]; + end + d = d(1:end-1); +end + diff --git a/arduino/modele/simulate.m b/arduino/modele/simulate.m new file mode 100644 index 0000000..f6ee0a0 --- /dev/null +++ b/arduino/modele/simulate.m @@ -0,0 +1,20 @@ +function [ t, V, d ] = simulate(Vi, Vm, Acc, dd, HZ, inaccuracy_max) + d = zeros(1, 1); + V = zeros(1, 1); + t = zeros(1, 1); + d(1) = dd; + V(1) = Vi; + t(1) = 0; + dt = 1/HZ; + i = 1; + MAX_TIME = 30; %s + while (true) + Vtarget = calcSpeed(V(i), Vm, Acc, d(i), [0 dt]); + if t(i) > MAX_TIME; break; end; + V = [V (Vtarget(2) + (2*(0.5-rand)*inaccuracy_max))]; + d = [d (d(i) - V(i)*dt)]; + t = [t (i*dt)]; + i = i+1; + end +end + diff --git a/arduino/modele/simulate2D.m b/arduino/modele/simulate2D.m new file mode 100644 index 0000000..df67f64 --- /dev/null +++ b/arduino/modele/simulate2D.m @@ -0,0 +1,61 @@ +function [ t, pos, Vlreal, Vrreal, Vat, Vt ] = simulate2D( init_pos, Vm, Acc, target_pos, HZ, entraxe, inaccuracy_max, delay, max_angle_to_rotate, K_distance_reduction ) + if nargin < 7 + inaccuracy_max = 0; + end + if nargin < 8 + delay = 0; + end + if nargin < 9 + max_angle_to_rotate = pi; + end + if nargin < 10 + K_distance_reduction = 10; + end + delay = round(delay*HZ); + pos = init_pos; + Vrtarget = zeros(1,delay); Vltarget = zeros(1,delay); + Vlreal = 0; Vrreal = 0; + dt = 1/HZ; + Vat = 0; Vt = 0; + i = 1; t = 0; + MAX_TIME = 60; %s + while (true) + % calculate errors + t = [t i*dt]; + dx = target_pos(1) - pos(end, 1); + dy = target_pos(2) - pos(end, 2); + da = wrapToPi(atan2(dy, dx) - pos(end,3)); + dd = sqrt(dx*dx + dy*dy); + if abs(da) > max_angle_to_rotate + dd = -dd; + da = wrapToPi(da - pi); + end + ddd = dd * exp(-abs(K_distance_reduction*da)); + % convert angle to distance per wheel + dda = da * (entraxe/2); + % calculate speed + Vat = [Vat, calcSpeed(Vat(end), Vm, Acc, dda, dt)]; + if (abs(da) > 180/180*pi) + Vt = [Vt 0]; + else + Vt = [Vt calcSpeed(Vt(end), Vm, Acc, ddd, dt)]; + end + % calculate wheels speed + Vltarget = [Vltarget, Vt(end) - Vat(end)]; + Vrtarget = [Vrtarget, Vt(end) + Vat(end)]; + % calculate real speed + Vlreal = [Vlreal (Vltarget(end-delay) + (2*(0.5-rand)*inaccuracy_max))]; + Vrreal = [Vrreal (Vrtarget(end-delay) + (2*(0.5-rand)*inaccuracy_max))]; + % apply movement + v = (Vrreal(end) + Vlreal(end)) / 2; + va = (Vrreal(end) - Vlreal(end)) / (entraxe*2); + pos(i+1,:) = pos(i,:) + [v*cos(pos(i,3)), v*sin(pos(i,3)), va ]./HZ; + pos(i+1,3) = wrapToPi(pos(i+1,3)); + i = i + 1; + + if (abs(dd) < 0.001) || (t(i) > MAX_TIME) + break; + end; + end +end + diff --git a/arduino/modele/simulation.m b/arduino/modele/simulation.m new file mode 100644 index 0000000..734a09d --- /dev/null +++ b/arduino/modele/simulation.m @@ -0,0 +1,33 @@ +%% SIMULATION %% +% inaccuraccy is max difference between V and Vtarget at any time (m/s) + +Vi = 0; % m/s +Vm = 1; % m/s +Acc = 0.4; % m/s2 +dd = 5; % m +HZ = 100; + +encoder_radius = 30; % mm +ticks_per_turn = 1024; + +max_inaccuracy = (HZ * (2*pi*encoder_radius) / ticks_per_turn) / 1000 + +figure; +plotSimulation(Vi, Vm, Acc, dd, HZ, max_inaccuracy, 'r'); +plotSimulation(Vi, Vm, Acc, dd, HZ, 0, 'g'); +drawnow; + +figure; +plotSimulation(Vi, Vm, Acc, dd, HZ, 0.01, 'r'); +plotSimulation(Vi, Vm, Acc, dd, HZ, 0.005, 'b'); +plotSimulation(Vi, Vm, Acc, dd, HZ, 0, 'g'); +drawnow; + +Vi = 0.3; % m/s +dd = -0.5; % m + +figure; +plotSimulation(Vi, Vm, Acc, dd, HZ, 0.01, 'r'); +plotSimulation(Vi, Vm, Acc, dd, HZ, 0.005, 'b'); +plotSimulation(Vi, Vm, Acc, dd, HZ, 0, 'g'); +drawnow; \ No newline at end of file diff --git a/arduino/modele/simulation2D.jpg b/arduino/modele/simulation2D.jpg new file mode 100644 index 0000000..9fae8ea Binary files /dev/null and b/arduino/modele/simulation2D.jpg differ diff --git a/arduino/modele/simulation2D.m b/arduino/modele/simulation2D.m new file mode 100644 index 0000000..8d4e374 --- /dev/null +++ b/arduino/modele/simulation2D.m @@ -0,0 +1,18 @@ +Vm = 1; % m/s +Acc = 1; % m/s2 +dd = 5; % m +HZ = 100; +K_distance_reduction = 10; % the higher the mor the robot will rotate before going to its destination +max_angle_to_rotate = 3*pi/4; % above that angle, the robot will go backwards +delay = 5/HZ; % control rise time + +inital_pos = [0, 0, 0]; +target_pos = [-1, 2]; + +entraxe = 0.1; %m +encoder_radius = 30; % mm +ticks_per_turn = 1024; + +max_inaccuracy = (HZ * (2*pi*encoder_radius) / ticks_per_turn) / 1000; % intrinsic + +plotSimulation2D(inital_pos, Vm, Acc, target_pos, HZ, entraxe, max_inaccuracy, delay, max_angle_to_rotate, K_distance_reduction ); \ No newline at end of file diff --git a/arduino/pr_others/AFMotor.cpp b/arduino/pr_others/AFMotor.cpp new file mode 100644 index 0000000..611eaa2 --- /dev/null +++ b/arduino/pr_others/AFMotor.cpp @@ -0,0 +1,714 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #if defined(__AVR__) + #include + #endif + #include "WProgram.h" +#endif + +#include "AFMotor.h" + + +static uint8_t latch_state; + +#if (MICROSTEPS == 8) +uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; +#elif (MICROSTEPS == 16) +uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; +#endif + +AFMotorController::AFMotorController(void) { + TimerInitalized = false; +} + +void AFMotorController::enable(void) { + // setup the latch + /* + LATCH_DDR |= _BV(LATCH); + ENABLE_DDR |= _BV(ENABLE); + CLK_DDR |= _BV(CLK); + SER_DDR |= _BV(SER); + */ + pinMode(MOTORLATCH, OUTPUT); + pinMode(MOTORENABLE, OUTPUT); + pinMode(MOTORDATA, OUTPUT); + pinMode(MOTORCLK, OUTPUT); + + latch_state = 0; + + latch_tx(); // "reset" + + //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! + digitalWrite(MOTORENABLE, LOW); +} + + +void AFMotorController::latch_tx(void) { + uint8_t i; + + //LATCH_PORT &= ~_BV(LATCH); + digitalWrite(MOTORLATCH, LOW); + + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + + for (i=0; i<8; i++) { + //CLK_PORT &= ~_BV(CLK); + digitalWrite(MOTORCLK, LOW); + + if (latch_state & _BV(7-i)) { + //SER_PORT |= _BV(SER); + digitalWrite(MOTORDATA, HIGH); + } else { + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + } + //CLK_PORT |= _BV(CLK); + digitalWrite(MOTORCLK, HIGH); + } + //LATCH_PORT |= _BV(LATCH); + digitalWrite(MOTORLATCH, HIGH); +} + +static AFMotorController MC; + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(9, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase + OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC4RS = 0x0000; + OC4R = 0x0000; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(10, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase + OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC5RS = 0x0000; + OC5R = 0x0000; + #else + // If we are not using PWM for pin 11, then just do digital + digitalWrite(11, LOW); + #endif +#elif defined(__AVR_ATmega32U4__) + pinMode(11, OUTPUT); + digitalWrite(11, LOW); +#else + #error "This chip is not supported!" +#endif + #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) + pinMode(11, OUTPUT); + #endif +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 + OC4RS = s; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 + OC5RS = s; + #else + // If we are not doing PWM output for M1, then just use on/off + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } + #endif +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase + OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC1RS = 0x0000; + OC1R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(3, OUTPUT); + digitalWrite(3, LOW); +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#elif defined(__PIC32MX__) + // Set the OC1 (pin3) PMW duty cycle from 0 to 255 + OC1RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(3, HIGH); + } + else + { + digitalWrite(3, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM3(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A / PD6 (pin 6) + TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A + //TCCR0B = freq & 0x7; + OCR0A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a + TCCR4B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR4A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase + OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC3RS = 0x0000; + OC3R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(6, OUTPUT); + digitalWrite(6, LOW); +#else + #error "This chip is not supported!" +#endif + pinMode(6, OUTPUT); +} + +inline void setPWM3(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR4A = s; +#elif defined(__PIC32MX__) + // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 + OC3RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(6, HIGH); + } + else + { + digitalWrite(6, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + + + +inline void initPWM4(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0B / PD5 (pin 5) + TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a + //TCCR0B = freq & 0x7; + OCR0B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 5 is now PE3 (OC3A) + TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a + TCCR3B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR3A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase + OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC2RS = 0x0000; + OC2R = 0x0000; +#elif defined(__AVR_ATmega32U4__) + pinMode(5, OUTPUT); + digitalWrite(5, LOW); +#else + #error "This chip is not supported!" +#endif + pinMode(5, OUTPUT); +} + +inline void setPWM4(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR3A = s; +#elif defined(__PIC32MX__) + // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 + OC2RS = s; +#elif defined(__AVR_ATmega32U4__) + if (s > 127) + { + digitalWrite(5, HIGH); + } + else + { + digitalWrite(5, LOW); + } +#else + #error "This chip is not supported!" +#endif +} + +AF_DCMotor::AF_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + + MC.enable(); + + switch (num) { + case 1: + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM1(freq); + break; + case 2: + latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM2(freq); + break; + case 3: + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM3(freq); + break; + case 4: + latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM4(freq); + break; + } +} + +void AF_DCMotor::run(uint8_t cmd) { + uint8_t a, b; + switch (motornum) { + case 1: + a = MOTOR1_A; b = MOTOR1_B; break; + case 2: + a = MOTOR2_A; b = MOTOR2_B; break; + case 3: + a = MOTOR3_A; b = MOTOR3_B; break; + case 4: + a = MOTOR4_A; b = MOTOR4_B; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + case BACKWARD: + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx(); + break; + case RELEASE: + latch_state &= ~_BV(a); // A and B both low + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + } +} + +void AF_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + case 3: + setPWM3(speed); break; + case 4: + setPWM4(speed); break; + } +} + +/****************************************** + STEPPERS +******************************************/ + +AF_Stepper::AF_Stepper(uint16_t steps, uint8_t num) { + MC.enable(); + + revsteps = steps; + steppernum = num; + currentstep = 0; + + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(11, OUTPUT); + pinMode(3, OUTPUT); + digitalWrite(11, HIGH); + digitalWrite(3, HIGH); + + // use PWM for microstepping support + initPWM1(STEPPER1_PWM_RATE); + initPWM2(STEPPER1_PWM_RATE); + setPWM1(255); + setPWM2(255); + + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(5, OUTPUT); + pinMode(6, OUTPUT); + digitalWrite(5, HIGH); + digitalWrite(6, HIGH); + + // use PWM for microstepping support + // use PWM for microstepping support + initPWM3(STEPPER2_PWM_RATE); + initPWM4(STEPPER2_PWM_RATE); + setPWM3(255); + setPWM4(255); + } +} + +void AF_Stepper::setSpeed(uint16_t rpm) { + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); + steppingcounter = 0; +} + +void AF_Stepper::release(void) { + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + } +} + +void AF_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { + uint32_t uspers = usperstep; + uint8_t ret = 0; + + if (style == INTERLEAVE) { + uspers /= 2; + } + else if (style == MICROSTEP) { + uspers /= MICROSTEPS; + steps *= MICROSTEPS; +#ifdef MOTORDEBUG + Serial.print("steps = "); Serial.println(steps, DEC); +#endif + } + + while (steps--) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + if (style == MICROSTEP) { + while ((ret != 0) && (ret != MICROSTEPS)) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + } +} + +uint8_t AF_Stepper::onestep(uint8_t dir, uint8_t style) { + uint8_t a, b, c, d; + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + if (steppernum == 1) { + a = _BV(MOTOR1_A); + b = _BV(MOTOR2_A); + c = _BV(MOTOR1_B); + d = _BV(MOTOR2_B); + } else if (steppernum == 2) { + a = _BV(MOTOR3_A); + b = _BV(MOTOR4_A); + c = _BV(MOTOR3_B); + d = _BV(MOTOR4_B); + } else { + return 0; + } + + // next determine what sort of stepping procedure we're up to + if (style == SINGLE) { + if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } + else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next even step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } + else { + currentstep -= MICROSTEPS; + } + } + } else if (style == DOUBLE) { + if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next odd step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == INTERLEAVE) { + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } + + if (style == MICROSTEP) { + if (dir == FORWARD) { + currentstep++; + } else { + // BACKWARDS + currentstep--; + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + + ocra = ocrb = 0; + if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { + ocra = microstepcurve[MICROSTEPS - currentstep]; + ocrb = microstepcurve[currentstep]; + } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { + ocra = microstepcurve[currentstep - MICROSTEPS]; + ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; + } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { + ocra = microstepcurve[MICROSTEPS*3 - currentstep]; + ocrb = microstepcurve[currentstep - MICROSTEPS*2]; + } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { + ocra = microstepcurve[currentstep - MICROSTEPS*3]; + ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; + } + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + +#ifdef MOTORDEBUG + Serial.print("current step: "); Serial.println(currentstep, DEC); + Serial.print(" pwmA = "); Serial.print(ocra, DEC); + Serial.print(" pwmB = "); Serial.println(ocrb, DEC); +#endif + + if (steppernum == 1) { + setPWM1(ocra); + setPWM2(ocrb); + } else if (steppernum == 2) { + setPWM3(ocra); + setPWM4(ocrb); + } + + + // release all + latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 + + //Serial.println(step, DEC); + if (style == MICROSTEP) { + if ((currentstep >= 0) && (currentstep < MICROSTEPS)) + latch_state |= a | b; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) + latch_state |= b | c; + if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) + latch_state |= c | d; + if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) + latch_state |= d | a; + } else { + switch (currentstep/(MICROSTEPS/2)) { + case 0: + latch_state |= a; // energize coil 1 only + break; + case 1: + latch_state |= a | b; // energize coil 1+2 + break; + case 2: + latch_state |= b; // energize coil 2 only + break; + case 3: + latch_state |= b | c; // energize coil 2+3 + break; + case 4: + latch_state |= c; // energize coil 3 only + break; + case 5: + latch_state |= c | d; // energize coil 3+4 + break; + case 6: + latch_state |= d; // energize coil 4 only + break; + case 7: + latch_state |= d | a; // energize coil 1+4 + break; + } + } + + + MC.latch_tx(); + return currentstep; +} + diff --git a/arduino/pr_others/AFMotor.h b/arduino/pr_others/AFMotor.h new file mode 100644 index 0000000..34ec0b4 --- /dev/null +++ b/arduino/pr_others/AFMotor.h @@ -0,0 +1,203 @@ +// Adafruit Motor shield library +// copyright Adafruit Industries LLC, 2009 +// this code is public domain, enjoy! + +/* + * Usage Notes: + * For PIC32, all features work properly with the following two exceptions: + * + * 1) Because the PIC32 only has 5 PWM outputs, and the AFMotor shield needs 6 + * to completely operate (four for motor outputs and two for RC servos), the + * M1 motor output will not have PWM ability when used with a PIC32 board. + * However, there is a very simple workaround. If you need to drive a stepper + * or DC motor with PWM on motor output M1, you can use the PWM output on pin + * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for + * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting + * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the + * two #defines below to activate the PWM on either pin 9 or pin 10. You will + * then have a fully functional microstepping for 2 stepper motors, or four + * DC motor outputs with PWM. + * + * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and + * the operation of DC motors and stepper motors as of 9/2012. This issue + * will get fixed in future MPIDE releases, but at the present time it means + * that the Motor Party example will NOT work properly. Any time you attach + * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will + * stop working. Thus no steppers or DC motors. + * + */ +// 09/15/2012 Modified for use with chipKIT boards + + +#ifndef _AFMotor_h_ +#define _AFMotor_h_ + +#include +#if defined (__AVR_ATmega32U4__) + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ 0 + #define MOTOR12_39KHZ 0 + #define MOTOR12_19KHZ 0 + #define MOTOR12_8KHZ 0 + #define MOTOR12_4_8KHZ 0 + #define MOTOR12_2KHZ 0 + #define MOTOR12_1KHZ 0 + + #define MOTOR34_64KHZ 0 + #define MOTOR34_39KHZ 0 + #define MOTOR34_19KHZ 0 + #define MOTOR34_8KHZ 0 + #define MOTOR34_4_8KHZ 0 + #define MOTOR34_2KHZ 0 + #define MOTOR34_1KHZ 0 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 +#elif defined(__AVR__) + #include + + //#define MOTORDEBUG 1 + + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ _BV(CS20) // no prescale + #define MOTOR12_8KHZ _BV(CS21) // divide by 8 + #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 + #define MOTOR12_1KHZ _BV(CS22) // divide by 64 + + #define MOTOR34_64KHZ _BV(CS00) // no prescale + #define MOTOR34_8KHZ _BV(CS01) // divide by 8 + #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 + +#elif defined(__PIC32MX__) + //#define MOTORDEBUG 1 + + // Uncomment the one of following lines if you have put a jumper from + // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. + // Either will enable PWM for M1 + //#define PIC32_USE_PIN9_FOR_M1_PWM + //#define PIC32_USE_PIN10_FOR_M1_PWM + + #define MICROSTEPS 16 // 8 or 16 + + // For PIC32 Timers, define prescale settings by PWM frequency + #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz + + #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz + + // PWM rate for DC motors. + #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ + // Note: for PIC32, both of these must be set to the same value + // since there's only one timebase for all 4 PWM outputs + #define STEPPER1_PWM_RATE MOTOR12_39KHZ + #define STEPPER2_PWM_RATE MOTOR34_39KHZ + +#endif + +// Bit positions in the 74HCT595 shift register output +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +// Constants that the user passes in to the motor calls +#define FORWARD 1 +#define BACKWARD 2 +#define BRAKE 3 +#define RELEASE 4 + +// Constants that the user passes in to the stepper calls +#define SINGLE 1 +#define DOUBLE 2 +#define INTERLEAVE 3 +#define MICROSTEP 4 + +/* +#define LATCH 4 +#define LATCH_DDR DDRB +#define LATCH_PORT PORTB + +#define CLK_PORT PORTD +#define CLK_DDR DDRD +#define CLK 4 + +#define ENABLE_PORT PORTD +#define ENABLE_DDR DDRD +#define ENABLE 7 + +#define SER 0 +#define SER_DDR DDRB +#define SER_PORT PORTB +*/ + +// Arduino pin names for interface to 74HCT595 latch +#define MOTORLATCH 12 +#define MOTORCLK 4 +#define MOTORENABLE 7 +#define MOTORDATA 8 + +class AFMotorController +{ + public: + AFMotorController(void); + void enable(void); + friend class AF_DCMotor; + void latch_tx(void); + uint8_t TimerInitalized; +}; + +class AF_DCMotor +{ + public: + AF_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +class AF_Stepper { + public: + AF_Stepper(uint16_t, uint8_t); + void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); + void setSpeed(uint16_t); + uint8_t onestep(uint8_t dir, uint8_t style); + void release(void); + uint16_t revsteps; // # steps per revolution + uint8_t steppernum; + uint32_t usperstep, steppingcounter; + private: + uint8_t currentstep; + +}; + +uint8_t getlatchstate(void); + +#endif diff --git a/arduino/pr_others/Servo PR/sketch_apr25a/sketch_apr25a.ino b/arduino/pr_others/Servo PR/sketch_apr25a/sketch_apr25a.ino new file mode 100644 index 0000000..e099f0e --- /dev/null +++ b/arduino/pr_others/Servo PR/sketch_apr25a/sketch_apr25a.ino @@ -0,0 +1,59 @@ + +#include + +Servo myservo1, myservo2,myservo3,myservo4,myservo5; // create servo object to control a servo + +void setup() +{ + Serial.begin(115200); + myservo1.attach(7); + myservo2.attach(9); + myservo3.attach(4); + myservo4.attach(5); + myservo5.attach(2); +} + +void loop() +{ + int order; + if (Serial.available()){ + delay(100); + while(Serial.available()>0){ + order=Serial.read(); //reads the value sent from Visual Basic + if(order=='0') // Ouvrir haur + { + myservo1.write(61); + myservo2.write(70); + } + else if(order=='1') // Fermer haut + { + myservo1.write(137); + myservo2.write(5); + } + else if(order=='2') // Ouvrir milieu moyen + { + myservo3.write(120); + myservo4.write(70); + } + else if(order=='3') // Fermer milieu + { + myservo3.write(140); + myservo4.write(50); + } + else if(order=='4') // Ouvrir gobelet + { + myservo5.write(120); + } + else if(order=='5') // Fermer gobelet + { + myservo5.write(70); + } + else if(order=='6') // Ouvrir milieu + { + myservo3.write(80); + myservo4.write(110); + } + } + } +} + diff --git a/arduino/pr_others/ax12.cpp b/arduino/pr_others/ax12.cpp new file mode 100644 index 0000000..0a4f011 --- /dev/null +++ b/arduino/pr_others/ax12.cpp @@ -0,0 +1,527 @@ +/* + ax12.cpp - arbotiX Library for AX-12 Servos + Copyright (c) 2008,2009 Michael E. Ferguson. All right reserved. + Modificada el 15/11/09 por Pablo Gindel. + versin 2.0 - 10/02/10 + versin 2.1 - 27/06/10 + versin 2.2 - 19/10/10 + versin 2.3 - 30/12/10 + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +/**** ATENCION: si se cambia el orden de los #includes, sigue compilando pero no funciona! ****/ + +#include "ax12.h" +#include // we need this for the serial port defines. +#include +#include + +/****************************************************************************** + * Hardware Serial Level, this uses the same stuff as Serial, therefore + * you should not use the Arduino Serial library. + ******************************************************************************/ + +/** helper functions to emulate half-duplex */ +void AX12::setTX () { +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) + bitClear(UCSR0B, RXCIE0); // deshabilita la interrupcin de recepcin + bitClear(UCSR0B, RXEN0); // deshabilila la recepcin + bitSet(UCSR0B, TXEN0); // habilita la trasmisin +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) + bitClear(UCSR1B, RXCIE1); // deshabilita la interrupcin de recepcin + bitClear(UCSR1B, RXEN1); // deshabilila la recepcin + bitSet(UCSR1B, TXEN1); // habilita la trasmisin +#elif defined (__AVR_ATmega8__) + bitClear(UCSRB, RXCIE); // deshabilita la interrupcin de recepcin + bitClear(UCSRB, RXEN); // deshabilila la recepcin + bitSet(UCSRB, TXEN); // habilita la trasmisin +#endif +} + +void AX12::setRX () { +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) + bitClear(TIMSK0, TOIE0); // deshabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) + bitClear(UCSR0B, TXEN0); // deshabilita la trasmisin + bitSet(UCSR0B, RXEN0); // habilita la recepcin + bitSet(UCSR0B, RXCIE0); // habilita la interrupcin de recepcin +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) + bitClear(TIMSK0, TOIE0); // deshabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) + bitClear(UCSR1B, TXEN1); // deshabilita la trasmisin + bitSet(UCSR1B, RXEN1); // habilita la recepcin + bitSet(UCSR1B, RXCIE1); // habilita la interrupcin de recepcin +#elif defined (__AVR_ATmega8__) + bitClear(TIMSK0, TOIE0); // deshabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) + bitClear(UCSRB, TXEN); // deshabilita la trasmisin + bitSet(UCSRB, RXEN); // habilita la recepcin + bitSet(UCSRB, RXCIE); // habilita la interrupcin de recepcin +#endif + ax_rx_Pointer = 0; // resetea el puntero del buffer +} + +void AX12::setNone () { +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) + bitClear(UCSR0B, RXCIE0); // deshabilita la interrupcin de recepcin + bitClear(UCSR0B, RXEN0); // deshabilila la recepcin + bitClear(UCSR0B, TXEN0); // deshabilita la trasmisin + bitSet(TIMSK0, TOIE0); // rehabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) + bitClear(UCSR1B, RXCIE1); // deshabilita la interrupcin de recepcin + bitClear(UCSR1B, RXEN1); // deshabilila la recepcin + bitClear(UCSR1B, TXEN1); // deshabilita la trasmisin + bitSet(TIMSK0, TOIE0); // rehabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) +#elif defined (__AVR_ATmega8__) + bitClear(UCSRB, RXCIE); // deshabilita la interrupcin de recepcin + bitClear(UCSRB, RXEN); // deshabilila la recepcin + bitClear(UCSRB, TXEN); // deshabilita la trasmisin + bitSet(TIMSK0, TOIE0); // rehabilita la interrupcin del timer0 (nota: esto es slo para entornos Arduino) +#endif +} + +/** Sends a character out the serial port */ +byte AX12::writeByte (byte data) { +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) + while (bit_is_clear(UCSR0A, UDRE0)); // espera que el micro est pronto para trasmitir + UDR0 = data; // escribe el byte a trasmitir +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) + while (bit_is_clear(UCSR1A, UDRE1)); // espera que el micro est pronto para trasmitir + UDR1 = data; // escribe el byte a trasmitir +#elif defined (__AVR_ATmega8__) + while (bit_is_clear(UCSRA, UDRE)); // espera que el micro est pronto para trasmitir + UDR = data; // escribe el byte a trasmitir +#endif + return data; +} + +/** We have a one-way receive buffer, which is reset after each packet is receieved. + A wrap-around buffer does not appear to be fast enough to catch all bytes at 1Mbps. */ +volatile byte AX12::ax_rx_buffer[AX12_BUFFER_SIZE]; +volatile byte AX12::ax_rx_Pointer; +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) +ISR (USART_RX_vect) { + AX12::ax_rx_buffer[(AX12::ax_rx_Pointer++)] = UDR0; // esta es la rutina de interrupcin de recepcin +} +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) +ISR (USART1_RX_vect) { + AX12::ax_rx_buffer[(AX12::ax_rx_Pointer++)] = UDR1; // esta es la rutina de interrupcin de recepcin +} // lo que hace es meter el byte recibido en el buffer +#elif defined (__AVR_ATmega8__) +SIGNAL (SIG_UART_RECV) { + AX12::ax_rx_buffer[(AX12::ax_rx_Pointer++)] = UDR; // esta es la rutina de interrupcin de recepcin +} // lo que hace es meter el byte recibido en el buffer +#endif + +/** initializes serial transmit at baud, 8-N-1 */ +// nota: el AX12 internamente usa el Double Speed Operation (U2Xn=1). Ciertos baudrates no coinciden si no se usa este seteo +void AX12::init (long baud) { + unsigned int reg = F_CPU / (baud * 8) - 1; +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega328P__) + bitSet (UCSR0A, U2X0); + UBRR0H = reg >> 8; // setea la velocidad del USART + UBRR0L = reg & 0xFF; +#elif defined (__AVR_ATmega1280__) || defined (__AVR_ATmega128__) || defined (__AVR_ATmega2560__) + bitSet (UCSR1A, U2X1); + UBRR1H = reg >> 8; + UBRR1L = reg & 0xFF; // setea la velocidad del USART +#elif defined (__AVR_ATmega8__) + bitSet (UCSRA, U2X); + UBRRH = reg >> 8; + UBRRL = reg & 0xFF; // setea la velocidad del USART +#endif + ax_rx_Pointer = 0; + // deshabilita tanto recepcin como trasmisin + setNone(); +} + +/****************************************************************************** + * Initialization + ******************************************************************************/ + +byte AX12::autoDetect (byte* list_motors, byte num_motors) { // escanea todos los ID hasta el 253 mandando PINGs + // devuelve una lista con los ID que respondieron + byte counter = 0; + byte *data; + for (byte i=0; i<254; i++) { + sendPacket (i, 0, AX_PING, data); + byte index = readPacket (); + byte id = ax_rx_buffer [index]; + byte int_error = ax_rx_buffer [index+1]; + if (int_error==0 && id==i) { + list_motors[counter++] = i; + if (counter == num_motors) {break;} + } + } + return counter; +} + +/** constructors */ +AX12::AX12 (long baud, byte motor_id, boolean inv) { + id = motor_id; + inverse = inv; + SRL = RETURN_ALL; + init (baud); +} + +AX12::AX12 (byte motor_id, boolean inv) { + id = motor_id; + inverse = inv; + SRL = RETURN_ALL; +} + +AX12::AX12 (long baud, byte motor_id) { + id = motor_id; + inverse = false; + SRL = RETURN_ALL; + init (baud); +} + +AX12::AX12 (byte motor_id) { + id = motor_id; + inverse = false; + SRL = RETURN_ALL; +} + +AX12::AX12 () { + id = BROADCAST_ID; + inverse = false; + SRL = RETURN_NONE; +} + +/* para mandar mensajes broadcast, definir un motor broadcast, de la siguiente manera: + AX12 broadcast = AX12(); + Nota: al definir un motor de esta manera y luego asignarle un id, tener en cuenta que + el SRL (status return level) va a quedar seteado en RETURN_NONE. +*/ + +/****************************************************************************** + * Packet Level + ******************************************************************************/ + +/** send instruction packet */ +void AX12::sendPacket (byte _id, byte datalength, byte instruction, byte* data) { + byte checksum = 0; + setTX(); + writeByte (0xFF); + writeByte (0xFF); + checksum += writeByte (_id); + checksum += writeByte (datalength + 2); + checksum += writeByte (instruction); + for (byte f=0; f 1100L) { // was 3000 + timeout = 1; + break; + } + } + if (timeout) break; + if ((bcount == 0) && (ax_rx_buffer[offset] != 0xff)) offset++; + else bcount++; + } + setNone(); + // ahora decodifica el packet + // correccin de cabecera + error = 0; // cdigo interno de error + do { + error++; + offset++; + bcount--; + } while (ax_rx_buffer[offset] == 255); + if (error > 1) error = 0; // prueba de cabecera + // offset = primer byte del mensaje (sin cabecera) + // bcount = largo del mensaje leido (sin cabecera) + status_length = 2 + ax_rx_buffer[offset+1]; // largo del mensaje decodificado + if (bcount != status_length) error+=2; // prueba de coherencia de data + checksum = 0; // clculo de checksum + for (byte f=0; f 1) {values[1] = highByte(value);} + return writeData (registr, reglength, values, isReg); +} + +/** "intelligent" sync write */ +void AX12::syncInfo (byte registr, byte targetlength, byte* targets, int* values) { + byte reglength = lengthWrite (registr); + if (reglength==0) {return;} + byte valuess [targetlength][reglength]; + byte * pointers [targetlength]; + for (byte f=0; f 1) {valuess[f][1] = highByte(values[f]);} + pointers[f] = &valuess[f][0]; + } + //nota: la sync write no respeta la propiedad "inverse" + syncWrite (registr, reglength, targetlength, targets, pointers); +} + + +/****************************************************************************** + * Macro Level + ******************************************************************************/ + +void AX12::setEndlessTurnMode (boolean endless) { // prende o apaga el modo "endless turn" + writeInfo (CW_ANGLE_LIMIT, 0); + if (endless) { + writeInfo (CCW_ANGLE_LIMIT, 0); + } else { + writeInfo (CCW_ANGLE_LIMIT, 1023); + } +} + +void AX12::endlessTurn (int velocidad) { // setea la velocidad, en el modo "endless turn" + boolean direccion = sign2bin (velocidad); + writeInfo (MOVING_SPEED, abs(velocidad)|((direccion^inverse)<<10)); +} + +int AX12::presentPSL (int* PSL) { // lee position, speed & load de una sola vez + AX12data data = readData (PRESENT_POSITION, 6); + for (byte f=0; f<3; f++) { + PSL[f] = makeInt (&data.data[2*f], 2); + processValue (PRESENT_POSITION + 2*f, &PSL[f]); + } + return data.error; +} + +// nota: si no coincide el SRL declarado con el del motor, los mensajes de respuesta son malinterpretados +void AX12::setSRL (byte _srl) { + SRL = _srl; + writeInfo (STATUS_RETURN_LEVEL, SRL); +} + +void AX12::changeID (byte newID) { + if (newID > 253) {return;} + writeInfo (ID, newID); + id = newID; +} + +int AX12::setPosVel (int pos, int vel) { + processValue (GOAL_POSITION, &pos); + byte values [4]; + values [0] = lowByte(pos); + values[1] = highByte(pos); + values [2] = lowByte(vel); + values[3] = highByte(vel); + return writeData (GOAL_POSITION, 4, values); +} + +void AX12::setMultiPosVel (byte targetlength, byte* targets, int* posvalues, int* velvalues) { + byte valuess [targetlength][4]; + byte * pointers [targetlength]; + for (byte f=0; f 0 --> true; numero <= 0 --> false + return (numero > 0); +} + +char bin2sign (boolean var) { // var = 0 --> sign = -1; var = 1 --> sign = 1 + return 2*var - 1; +} + +int makeInt (byte *dir, byte reglength) { // transforma 2 bytes en un int (segn la lgica AX12) + if (reglength > 1) { + return (dir[1] << 8) | dir[0]; + } else { + return dir[0]; + } +} + +byte lengthRead (byte registr) { + byte reglength = 0; + switch (registr) { + case VERSION: case ID: case BAUD_RATE: case RETURN_DELAY_TIME: + case LIMIT_TEMPERATURE: case DOWN_LIMIT_VOLTAGE: case UP_LIMIT_VOLTAGE: + case STATUS_RETURN_LEVEL: case ALARM_LED: case ALARM_SHUTDOWN: case 19: case TORQUE_ENABLE: case LED: + case CW_COMPLIANCE_MARGIN: case CCW_COMPLIANCE_MARGIN: case CW_COMPLIANCE_SLOPE: case CCW_COMPLIANCE_SLOPE: + case PRESENT_VOLTAGE: case PRESENT_TEMPERATURE: case REGISTERED_INSTRUCTION: case MOVING: case LOCK: reglength = 1; break; + case MODEL_NUMBER: case CW_ANGLE_LIMIT: case CCW_ANGLE_LIMIT: + case MAX_TORQUE: case DOWN_CALIBRATION: case UP_CALIBRATION: + case GOAL_POSITION: case MOVING_SPEED: case TORQUE_LIMIT: + case PRESENT_POSITION: case PRESENT_SPEED: case PRESENT_LOAD: case PUNCH: reglength = 2; break; + } + return reglength; +} + +byte lengthWrite (byte registr) { + byte reglength = 0; + switch (registr) { + case ID: case BAUD_RATE: case RETURN_DELAY_TIME: + case LIMIT_TEMPERATURE: case DOWN_LIMIT_VOLTAGE: case UP_LIMIT_VOLTAGE: + case STATUS_RETURN_LEVEL: case ALARM_LED: case ALARM_SHUTDOWN: case 19: + case TORQUE_ENABLE: case LED: case CW_COMPLIANCE_MARGIN: case CCW_COMPLIANCE_MARGIN: + case CW_COMPLIANCE_SLOPE: case CCW_COMPLIANCE_SLOPE: case REGISTERED_INSTRUCTION: case LOCK: reglength = 1; break; + case CW_ANGLE_LIMIT: case CCW_ANGLE_LIMIT: + case MAX_TORQUE: case GOAL_POSITION: + case MOVING_SPEED: case TORQUE_LIMIT: case PUNCH: reglength = 2; break; + } + return reglength; +} + +AX12data AX12::returnData (byte _srl) { + AX12data returndata; + if (SRL >= _srl) { + byte index = readPacket (); + byte status_id = ax_rx_buffer [index]; + byte int_error = ax_rx_buffer [index+1]; + byte status_error = ax_rx_buffer [index+2]; + returndata.error = (int_error<<7) | status_error | ((status_id != id)<<10); // genera el mensaje de error, combinacin de error interno con error ax12 + returndata.data = (byte*) &(ax_rx_buffer [index+3]); + } else { + setNone(); + returndata.error = -1; + } + return returndata; +} + +void AX12::processValue (byte registr, int* value) { // procesa el valor para la salida segun la propiedad "inverse" + switch (registr) { + case PRESENT_POSITION: case GOAL_POSITION: + if (inverse) {*value = 1023 - *value;} + break; + case PRESENT_SPEED: case PRESENT_LOAD: + *value = ((*value)&0x03FF) * bin2sign(((*value)>0x03FF)^inverse); + break; + } +} \ No newline at end of file diff --git a/arduino/pr_others/ax12.h b/arduino/pr_others/ax12.h new file mode 100644 index 0000000..c2e93a8 --- /dev/null +++ b/arduino/pr_others/ax12.h @@ -0,0 +1,167 @@ +/* + ax12.h - arbotiX Library for AX-12 Servos + Copyright (c) 2008,2009 Michael E. Ferguson. All right reserved. + Modificada el 15/11/09 por Pablo Gindel. + versión 2.0 - 10/02/10 + versión 2.1 - 27/06/10 + versión 2.2 - 19/10/10 + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef _AX12_H +#define _AX12_H + +#define AX12_MAX_SERVOS 18 +#define AX12_BUFFER_SIZE 32 + +/** EEPROM AREA **/ +#define MODEL_NUMBER 0 +#define VERSION 2 +#define ID 3 +#define BAUD_RATE 4 +#define RETURN_DELAY_TIME 5 +#define CW_ANGLE_LIMIT 6 +#define CCW_ANGLE_LIMIT 8 +#define LIMIT_TEMPERATURE 11 +#define DOWN_LIMIT_VOLTAGE 12 +#define UP_LIMIT_VOLTAGE 13 +#define MAX_TORQUE 14 +#define STATUS_RETURN_LEVEL 16 +#define ALARM_LED 17 +#define ALARM_SHUTDOWN 18 +#define DOWN_CALIBRATION 20 +#define UP_CALIBRATION 22 + +/** RAM AREA **/ +#define TORQUE_ENABLE 24 +#define LED 25 +#define CW_COMPLIANCE_MARGIN 26 +#define CCW_COMPLIANCE_MARGIN 27 +#define CW_COMPLIANCE_SLOPE 28 +#define CCW_COMPLIANCE_SLOPE 29 +#define GOAL_POSITION 30 +#define MOVING_SPEED 32 +#define TORQUE_LIMIT 34 +#define PRESENT_POSITION 36 +#define PRESENT_SPEED 38 +#define PRESENT_LOAD 40 +#define PRESENT_VOLTAGE 42 +#define PRESENT_TEMPERATURE 43 +#define REGISTERED_INSTRUCTION 44 +#define MOVING 46 +#define LOCK 47 +#define PUNCH 48 + +/** Status Return Levels **/ +#define RETURN_NONE 0 +#define RETURN_READ 1 +#define RETURN_ALL 2 + +/** Instruction Set **/ +#define AX_PING 1 +#define READ_DATA 2 +#define WRITE_DATA 3 +#define REG_WRITE 4 +#define ACTION 5 +#define RESET 6 +#define SYNC_WRITE 131 + +/** Special IDs **/ +#define BROADCAST_ID 254 + + +typedef unsigned char boolean; +typedef unsigned char byte; + +typedef struct {int error; byte *data;} AX12data; +typedef struct {int error; int value;} AX12info; + +class AX12 +{ + public: + + AX12 (long baud, byte motor_id, boolean inv); + AX12 (byte motor_id, boolean inv); + AX12 (long baud, byte motor_id); + AX12 (byte motor_id); + AX12 (); + + byte id; + boolean inverse; + byte SRL; // status return level + + // el buffer no puede ser privado porque debe ser visible desde la ISR + static volatile byte ax_rx_buffer[AX12_BUFFER_SIZE]; // buffer de recepción + static volatile byte ax_rx_Pointer; // making these volatile keeps the compiler from optimizing loops of available() + + static void init (long baud); + static byte autoDetect (byte* list_motors, byte num_motors); + static void syncWrite (byte start, byte length, byte targetlength, byte* targets, byte** valuess); + static void syncInfo (byte registr, byte targetlength, byte* targets, int* values); + static void setMultiPosVel (byte targetlength, byte* targets, int* posvalues, int* velvalues); + + int ping (); + int reset (); + AX12data readData (byte start, byte length); + int writeData (byte start, byte length, byte* values, boolean isReg = false); + int action (); + AX12info readInfo (byte registr); + int writeInfo (byte registr, int value, boolean isReg = false); + void setEndlessTurnMode (boolean endless); + void endlessTurn (int velocidad); + int presentPSL (int* PSL); + void setSRL (byte _srl); + void changeID (byte newID); + int setPosVel (int pos, int vel); + + private: + + static void setTX (); + static void setRX (); + static void setNone (); + static byte writeByte (byte data); + static void sendPacket (byte _id, byte datalength, byte instruction, byte* data); + static byte readPacket (); + AX12data returnData (byte _srl); + void processValue (byte registr, int* value); + +}; + +// utils + +boolean sign2bin (int numero); +char bin2sign (boolean var); +int makeInt (byte *dir, byte reglength); +byte lengthRead (byte registr); +byte lengthWrite (byte registr); + +/** Macros **/ + +#define setPos(pos) writeInfo (GOAL_POSITION, pos) +#define regPos(pos) writeInfo (GOAL_POSITION, pos, true) +#define setVel(vel) writeInfo (MOVING_SPEED, vel) +#define setTorque(torque) writeInfo (TORQUE_LIMIT, torque) +#define setMultiPos(a, b, c) syncInfo (GOAL_POSITION, a, b, c) +#define setMultiVel(a, b, c) syncInfo (MOVING_SPEED, a, b, c) +#define setMultiTorque(a, b, c) syncInfo (TORQUE_LIMIT, a, b, c) +#define torqueOn writeInfo (TORQUE_ENABLE, 1) +#define torqueOff writeInfo (TORQUE_ENABLE, 0) +#define getPos() readInfo (PRESENT_POSITION).value +#define getSpeed() readInfo (PRESENT_SPEED).value +#define getLoad() readInfo (PRESENT_LOAD).value + + +#endif \ No newline at end of file diff --git a/arduino/pr_others/make b/arduino/pr_others/make new file mode 100644 index 0000000..5a6d1d2 --- /dev/null +++ b/arduino/pr_others/make @@ -0,0 +1,6 @@ +#!/bin/bash + +export AVR_HOME=/usr/bin ARDUINO_HOME=../arduino-1.0 +export ARDUINO_BOARD=leonardo + +scons -f ../SConstruct $@ diff --git a/arduino/pr_others/pr_others.ino b/arduino/pr_others/pr_others.ino new file mode 100644 index 0000000..3fda1f1 --- /dev/null +++ b/arduino/pr_others/pr_others.ino @@ -0,0 +1,139 @@ +#include +#include +#include "AFMotor.h" + + +AF_Stepper stepper(200, 1); +Servo H_servo1; //vue de face ==> gauche +Servo H_servo2; //vue de face ==> droite +Servo M_servo1; //vue de face ==> gauche +Servo M_servo2; //vue de face ==> droite +Servo G_servo; +Servo C_servo; + + +int i=0; +char maChaine[64]; + +void annalyse_chaine(); + +void setup() +{ + Serial.begin(57600); + Serial.write('O'); + + stepper.setSpeed(60); + // /!\ Pour éviter les conflits, il ne faut pas utiliser les PIN deja prisent par la shield !!! + H_servo1.attach(5); //2 5 6 9 10 OK + H_servo2.attach(13); //13=led BOF BOF + M_servo1.attach(2); //3 4=vibre 7 8 11 12 !OK + M_servo2.attach(9); + G_servo.attach(10); + C_servo.attach(6); + + H_servo1.write(50); + H_servo2.write(50); + M_servo1.write(70); + M_servo2.write(50); + G_servo.write(70); + C_servo.write(50); +} + + +void loop() +{ + if (Serial.available()) { + maChaine[i]= Serial.read(); + + if (maChaine[i]=='\n' || maChaine[i]=='\r'){ + maChaine[i]='\0'; + annalyse_chaine(); + i=0; + } + else{ + i = ++i % 64; + } + } + delay(10); +} + + +void annalyse_chaine(){ + switch(maChaine[0]) + { + case 'H': + { + int H_angle1, H_angle2; + sscanf(maChaine,"H;%d;%d",&H_angle1,&H_angle2); + H_servo1.write(H_angle1); + H_servo2.write(H_angle2); + Serial.print('H'); + } + break; + + case 'M': + { + int M_angle1, M_angle2; + sscanf(maChaine,"M;%d;%d",&M_angle1,&M_angle2); + M_servo1.write(M_angle1); + M_servo2.write(M_angle2); + Serial.print('M'); + } + break; + + case 'G': + { + int angle_gobelet; + sscanf(maChaine,"G;%d",&angle_gobelet); + G_servo.write(angle_gobelet); + Serial.print('G'); + } + break; + + case 'O': + { + Serial.print('O'); + } + break; + + + case 'C': + { + int angle_clap; + sscanf(maChaine,"C;%d",&angle_clap); + C_servo.write(angle_clap); + Serial.print('C'); + } + break; + + case 'S': + { + int pas; + sscanf(maChaine,"S;%d",&pas); + if(pas==0){ + stepper.release(); + } + if(pas<0){ + stepper.setSpeed(60); + stepper.step(-pas,FORWARD, DOUBLE); + } + else{ + stepper.setSpeed(120); + stepper.step(pas,BACKWARD, DOUBLE); + } + Serial.print('S'); + //Serial.print("S:"); + //Serial.println(pas); + } + break; + default: + Serial.print('U'); + } +} + + + + + + + diff --git a/arduino/test_angles_servos/make b/arduino/test_angles_servos/make new file mode 100644 index 0000000..5a6d1d2 --- /dev/null +++ b/arduino/test_angles_servos/make @@ -0,0 +1,6 @@ +#!/bin/bash + +export AVR_HOME=/usr/bin ARDUINO_HOME=../arduino-1.0 +export ARDUINO_BOARD=leonardo + +scons -f ../SConstruct $@ diff --git a/arduino/test_angles_servos/test_angles_servos.ino b/arduino/test_angles_servos/test_angles_servos.ino new file mode 100644 index 0000000..9a6c6a5 --- /dev/null +++ b/arduino/test_angles_servos/test_angles_servos.ino @@ -0,0 +1,102 @@ +#include +#include + + +Servo H_servo1; //vue de face ==> gauche +Servo H_servo2; //vue de face ==> droite +Servo M_servo1; //vue de face ==> gauche +Servo M_servo2; //vue de face ==> droite +Servo G_servo; +Servo C_servo; + + +int i=0; +char maChaine[64]; + +void annalyse_chaine(); + +void setup() +{ + Serial.begin(57600); + Serial.write('O'); + + H_servo1.attach(2); //2 5 6 9 10 OK + H_servo2.attach(5); //13=led BOF BOF + M_servo1.attach(6); //3 4=vibre 7 8 11 12 !OK + M_servo2.attach(9); + G_servo.attach(10); + C_servo.attach(13); +} + + +void loop() +{ + if (Serial.available()) { + maChaine[i]= Serial.read(); + + if (maChaine[i]=='\n' || maChaine[i]=='\r'){ + maChaine[i]='\0'; + annalyse_chaine(); + i=0; + } + else{ + i = ++i % 64; + } + } + delay(10); +} + + +void annalyse_chaine(){ + switch(maChaine[0]) + { + case '2': + { + int angle; + sscanf(maChaine,"2;%d",&angle); + H_servo1.write(angle); + } + break; + case '5': + { + int angle; + sscanf(maChaine,"5;%d",&angle); + H_servo2.write(angle); + } + break; + case '6': + { + int angle; + sscanf(maChaine,"6;%d",&angle); + M_servo1.write(angle); + } + break; + case '9': + { + int angle; + sscanf(maChaine,"9;%d",&angle); + M_servo2.write(angle); + } + break; + case '0': + { + int angle; + sscanf(maChaine,"0;%d",&angle); + G_servo.write(angle); + } + break; + case '3': + { + int angle; + sscanf(maChaine,"3;%d",&angle); + C_servo.write(angle); + } + } +} + + + + + + + diff --git a/ax12/elevator.js b/ax12/elevator.js new file mode 100644 index 0000000..9dfab10 --- /dev/null +++ b/ax12/elevator.js @@ -0,0 +1,140 @@ +var ffi = require('ffi'); +var log4js = require('log4js'); +var logger = log4js.getLogger('test.ascenseur'); + +var libusb2ax = ffi.Library('../libs/dynamixel/src_lib/libusb2ax', { + 'dxl_initialize': ['int', ['int', 'int']], + 'dxl_write_word': ['void', ['int', 'int', 'int']], + 'dxl_read_word': ['int', ['int', 'int']], + 'dxl_terminate': ['void', ['void']], + 'dxl_get_result': ['int', ['void']] +}) + +var P_GOAL_POSITION_L = 30; +var P_POSITION = 36; +var P_SPEED = 0x26; +var P_COUPLE = 34; +var MARGE_POS = 40; +var MARGE_POS_MVT = 5; + +var ax12s = { + '2':{ + id: 2, + obj: 0, pos: 0, arrived: false + }, + '3':{ + id: 3, + obj: 0, pos: 0, arrived: false + } +}; + +function loopAX12() { + var speed; + for(var i in ax12s) { + // Si il est pas à la bonne position + if(ax12s[i].pos < ax12s[i].obj - MARGE_POS || ax12s[i].pos > ax12s[i].obj + MARGE_POS) { + ax12s[i].arrived = false; + speed = libusb2ax.dxl_read_word(ax12s[i].id, P_SPEED); + // Si il bouge pas, on renvoie l'ordre + if(speed == 0) { + console.log("ordre"+i); + libusb2ax.dxl_write_word(ax12s[i].id, P_GOAL_POSITION_L, ax12s[i].obj); + } + else { + ax12s[i].pos = libusb2ax.dxl_read_word(ax12s[i].id, P_POSITION); + } + } + else { + if(!ax12s[i].arrived) { + ax12s[i].arrived = true; + logger.info(new Date().getTime()+" "+ax12s[i].id+" arrivé !"); + } + } + } + setTimeout(loopAX12, 50); +} + +function degToAx12(deg) { + return parseInt((deg+150)*1024/300); +} +function openAx12Down() { + ax12s['2'].obj = degToAx12(0); + ax12s['3'].obj = degToAx12(0); +} +function closeAx12Down() { + ax12s['2'].obj = degToAx12(-75); + ax12s['3'].obj = degToAx12(75); +} +// var t = true; +// function loop() { +// if(t) { +// closeAx12Down(); +// } else { +// openAx12Down(); +// } +// t = !t; +// setTimeout(loop, 3000); +// } +// loop(); + +// AX12 +if(libusb2ax.dxl_initialize(1, 1) <= 0) { + logger.error("Impossible de se connecter à l'USB2AX"); + process.exit(1); +} +libusb2ax.dxl_write_word(2, P_COUPLE, 700); +libusb2ax.dxl_write_word(3, P_COUPLE, 700); +openAx12Down(); +loopAX12(); + +// Servos +var five = require("johnny-five"); +var board = new five.Board({ + port: '/dev/ttyACM0', + repl: false +}); +var servo1, servo2; +function ouvrirServos() { + servod.to(80); + servog.to(10); +} +function fermerServos() { + servod.to(55); + servog.to(35); +} +board.on("ready", function() { + servod = new five.Servo(7); + servog = new five.Servo(9); + fermerServos(); +}); + +// Ascenseur +var Elevator = require('../clients/pr/elevator.class.js'); +var elevator = new Elevator('/dev/ttyACM2'); +function descendreAscenseur() { + elevator.move1Down(); +} +function monterAscenseur() { + elevator.move1Up(); +} + +// Orders +var orders = [closeAx12Down, ouvrirServos, monterAscenseur, fermerServos, openAx12Down, descendreAscenseur]; +// var orders = [closeAx12Down, ouvrirServos, fermerServos, openAx12Down]; +var order_id = -1; +function nextOrder() { + order_id++; + if(order_id >= orders.length) { + order_id = 0; + } + orders[order_id](); +} + +// Programme principal +var keypress = require('keypress'); +keypress(process.stdin); +process.stdin.on('keypress', function (ch, key) { + if (key.name == 'enter') { + nextOrder(); + } +}); \ No newline at end of file diff --git a/ax12/pr.js b/ax12/pr.js new file mode 100644 index 0000000..ac043cc --- /dev/null +++ b/ax12/pr.js @@ -0,0 +1,11 @@ +var log4js = require('log4js'); +var logger = log4js.getLogger('clientpr.acts'); + +var five = require("johnny-five"); +var board = new five.Board();//{repl: false}); + +stepper = new five.Stepper({ + type: five.Stepper.TYPE.FOUR_WIRE, + stepsPerRev: 200, + pins: [ 8, 9, 10, 11 ] +}); \ No newline at end of file diff --git a/ax12/test.js b/ax12/test.js new file mode 100644 index 0000000..e86da3d --- /dev/null +++ b/ax12/test.js @@ -0,0 +1,87 @@ +var ffi = require('ffi'); +var log4js = require('log4js'); +var logger = log4js.getLogger('Test AX12'); + +var libusb2ax = ffi.Library('../libs/dynamixel/src_lib/libusb2ax', { + 'dxl_initialize': ['int', ['int', 'int']], + 'dxl_write_word': ['void', ['int', 'int', 'int']], + 'dxl_read_word': ['int', ['int', 'int']], + 'dxl_terminate': ['void', ['void']], + 'dxl_get_result': ['int', ['void']] +}) + +var P_GOAL_POSITION_L = 30; +var P_POSITION = 36; +var P_SPEED = 0x26; +var P_COUPLE = 34; +var MARGE_POS = 40; +var MARGE_POS_MVT = 5; + +var ax12s = { + '2':{ + id: 2, + obj: 0, pos: 0, arrived: false + }, + '3':{ + id: 3, + obj: 0, pos: 0, arrived: false + } +}; + +function loopAX12() { + var speed; + for(var i in ax12s) { + // Si il est pas à la bonne position + if(ax12s[i].pos < ax12s[i].obj - MARGE_POS || ax12s[i].pos > ax12s[i].obj + MARGE_POS) { + ax12s[i].arrived = false; + speed = libusb2ax.dxl_read_word(ax12s[i].id, P_SPEED); + // Si il bouge pas, on renvoie l'ordre + if(speed == 0) { + console.log("ordre"+i); + libusb2ax.dxl_write_word(ax12s[i].id, P_GOAL_POSITION_L, ax12s[i].obj); + } + else { + ax12s[i].pos = libusb2ax.dxl_read_word(ax12s[i].id, P_POSITION); + } + } + else { + if(!ax12s[i].arrived) { + ax12s[i].arrived = true; + logger.info(new Date().getTime()+" "+ax12s[i].id+" arrivé !"); + } + } + } + setTimeout(loopAX12, 50); +} + +function degToAx12(deg) { + return parseInt((deg+150)*1024/300); +} +function openAx12Down() { + ax12s['2'].obj = degToAx12(0); + ax12s['3'].obj = degToAx12(0); +} +function closeAx12Down() { + ax12s['2'].obj = degToAx12(-80); + ax12s['3'].obj = degToAx12(80); +} + +if(libusb2ax.dxl_initialize(0, 1) <= 0) { + logger.error("Impossible de se connecter à l'USB2AX"); + process.exit(1); +} +libusb2ax.dxl_write_word(2, P_COUPLE, 1000); +libusb2ax.dxl_write_word(3, P_COUPLE, 1000); +loopAX12(); + +var t = true; +function loop() { + if(t) { + closeAx12Down(); + } else { + openAx12Down(); + } + t = !t; + setTimeout(loop, 3000); +} +loop(); diff --git a/clients/gr/actuators.class.js b/clients/gr/actuators.class.js new file mode 100644 index 0000000..49f925e --- /dev/null +++ b/clients/gr/actuators.class.js @@ -0,0 +1,124 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('gr.acts'); + var serialPort = require("serialport"); + var SerialPort = serialPort.SerialPort; + var fifo = new (require('../shared/fifo.class.js'))(); + + var servos = null; + var asserv = null; + var date = new Date(); + var lastSendStatus = date.getTime(); + + function Acts(client, sendChildren) { + this.client = client; + this.sendChildren = sendChildren; + this.start(); + } + + Acts.prototype.start = function(){ + + }; + + Acts.prototype.clean = function(){ + fifo.clean(); // A priori déjà vide + asserv.clean(); + }; + + Acts.prototype.connectTo = function(struct){ + if (!struct.servos) { + logger.fatal("Lancement des servos gr en mode simu !"); + servos = new (require('./servos.simu.class.js'))(); + } else { + servos = new (require('./servos.class.js'))(struct.servos, this.sendStatus); + } + if (!struct.asserv) { + logger.fatal("Lancement de l'asserv gr en mode simu !"); + asserv = new (require('../shared/asserv.simu.class.js'))(this.client, 'gr', fifo); + } else { + asserv = new (require('../shared/asserv.class.js'))( + new SerialPort(struct.asserv, { + baudrate: 57600, + parser:serialPort.parsers.readline('\n'), + }), this.client, 'gr', this.sendStatus, fifo + ); + } + }; + + Acts.prototype.sendStatus = function() { + if(lastSendStatus < date.getTime()-1000){ + this.sendChildren(this.getStatus); + lastSendStatus = date.getTime(); + } + }; + + Acts.prototype.getStatus = function(){ + var data = { + "status": "", + "children": [] + }; + + data.status = "everythingIsAwesome"; + + if(servos && servos.ready){ + data.children.push("Arduino servos"); + }else + data.status = "ok"; + + if(asserv && asserv.ready){ + data.children.push("Arduino asserv"); + }else + data.status = "error"; + + return data; + }; + + Acts.prototype.quit = function(){ + if (!!servos && servos.ready) + servos.disconnect(); + if (!!asserv && asserv.ready) + asserv.disconnect(); + }; + + // Order switch + Acts.prototype.orderHandler = function (from, name, params, callback) { + // logger.info("Just received an order `" + name + "` from " + from + " with params :"); + logger.info(name, params); + + switch (name){ + // Others + case "acheter": + servos.acheter(callback); + break; + case "vendre": + servos.vendre(callback); + break; + // Asserv + case "pwm": + asserv.pwm(params.left, params.right, params.ms,callback); + break; + case "setvit": + asserv.setVitesse(params.v, params.r, callback); + break; + case "clean": + asserv.clean(callback); + break; + case "goa": + asserv.goa(params.a,callback); + break; + case "goxy": + asserv.goxy(params.x, params.y, "avant",callback); + break; + case "setpos": + asserv.setPos(params,callback); + break; + case "setpid": + asserv.setPid(params.p, params.i, params.d,callback); + break; + default: + logger.warn("Order name " + name + " " + from + " not understood"); + callback(); + } + }; + + return Acts; +})(); diff --git a/clients/gr/detect.class.js b/clients/gr/detect.class.js new file mode 100644 index 0000000..3880b58 --- /dev/null +++ b/clients/gr/detect.class.js @@ -0,0 +1,38 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('pr.detect'); + var serialPort = require("serialport"); + var SerialPort = serialPort.SerialPort; + + var sp = []; + + function Detect(callback) { + this.devicesFound = { + asserv: null, + servos: null + }; + this.callback = callback; + this.searchArduinos(); + } + + Detect.prototype.sendSP = function (){ + // Sent to acts + this.callback(this.devicesFound); + }; + + Detect.prototype.searchArduinos = function() { + // On check tous les ports disponibles + serialPort.list(function (err, ports) { + for(var i in ports) { + if(ports[i].comName.indexOf('ttyUSB') >= 0) { + this.devicesFound.servos = ports[i].comName; + } else if(ports[i].comName.indexOf('ttyACM') >= 0) { + this.devicesFound.asserv = ports[i].comName; + } + } + this.sendSP(); + }.bind(this)); + }; + + return Detect; +})(); \ No newline at end of file diff --git a/clients/gr/main.js b/clients/gr/main.js new file mode 100644 index 0000000..93349c3 --- /dev/null +++ b/clients/gr/main.js @@ -0,0 +1,151 @@ +(function () { + "use strict"; + // Requires + var log4js = require('log4js'); + var logger = log4js.getLogger('gr'); + + // logger.info("Started NodeJS client with pid " + process.pid); + + var SocketClient = require('../../server/socket_client.class.js'); + var server = require('../../config.js').server; + var client = new SocketClient({ + server_ip: server, + type: "gr" + }); + + var lastStatus = { + "status": "waiting" + }; + sendChildren(lastStatus); + + var acts = new (require('./actuators.class.js'))(client, sendChildren); + var detect = null; // new (require('./detect.class.js'))(devicesDetected); + + var queue = []; + var orderInProgress = null; + + start(); + + // On message + client.order(function (from, name, params){ + // logger.info("Recieved an order "+name); + switch (name){ + case "collision": + queue = []; + acts.clean(); + orderInProgress = false; + break; + case "stop": + acts.clean(); + logger.fatal("Stop GR"); + process.exit(); + break; + + // useless // + case "start": + queue = []; + start(); + break; + default: + addOrder2Queue(from, name, params); + } + }); + + function start(){ + logger.info("Starting :)"); + sendChildren({ + status: "starting", + children:[] + }); + detect = new (require('./detect.class.js'))(devicesDetected); + } + + function stop(){ + acts.quit(); + + // Send struct to server + sendChildren({ + status: "waiting", + children:[] + }); + } + + function devicesDetected(struct){ + // Verify content + if (!struct.servos) + logger.warn("Missing servos Nano"); + if (!struct.asserv) + logger.warn("Missing asserv Nano"); + + // Connect to what's detected + acts.connectTo(struct); + + // Send struct to server + sendChildren(acts.getStatus()); + } + + // Sends status to server + function sendChildren(status){ + lastStatus = status; + + client.send("server", "server.childrenUpdate", lastStatus); + } + + function isOk(){ + if(lastStatus.status != "waiting") + lastStatus = acts.getStatus(); + + client.send("ia", "isOkAnswer", lastStatus); + client.send("server", "server.childrenUpdate", lastStatus); + } + + // Push the order (enfiler) + function addOrder2Queue(f, n, p){ + if(queue.length < 50) { + // Adds the order to the queue + queue.push({ + from: f, + name: n, + params: p + }); + // logger.info("Order added to queue ! : "); + // logger.info(queue); + executeNextOrder(); + } + } + + // Execute order + function executeNextOrder(){ + if ((queue.length > 0) && (!orderInProgress)){ + var order = queue.shift(); + orderInProgress = order.name; + + logger.info("Going to do '" + orderInProgress + "' "+order.params.toString()); + acts.orderHandler(order.from, order.name, order.params, actionFinished); + + executeNextOrder(); + } + } + + function actionFinished(){ + logger.info(orderInProgress + " just finished !"); + + orderInProgress = false; + executeNextOrder(); + } + + function quit () { + logger.info("Please wait while exiting..."); + // acts.quit(); + process.exit(0); + } + + + // Exiting : + //do something when app is closing + //process.on('exit', quit); + // catches ctrl+c event + //process.on('SIGINT', quit); + // //catches uncaught exceptions + // process.on('uncaughtException', quit); +})(); diff --git a/clients/gr/servos.class.js b/clients/gr/servos.class.js new file mode 100644 index 0000000..8231866 --- /dev/null +++ b/clients/gr/servos.class.js @@ -0,0 +1,58 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('gr.servos'); + var five = require("johnny-five"); + var board = null; + var servo_gauche, servo_droit; + + function Servos(sp, sendStatus) { + this.ready = false; + this.sendStatus = sendStatus; + this.connect(sp); + } + + Servos.prototype.connect = function(sp) { + board = new five.Board({ + port: sp, + repl: false + }); + + board.on("ready", function() { + logger.info("Board servos Ready"); + servo_gauche = new five.Servo(2); + servo_droit = new five.Servo(3); + this.ready = true; + this.sendStatus(); + this.acheter(); + }.bind(this)); + }; + + Servos.prototype.acheter = function(callback) { + servo_gauche.to(170); + servo_droit.to(0); + setTimeout(callback, 500); + }; + Servos.prototype.vendre = function(callback) { + servo_gauche.to(50); + servo_droit.to(120); + setTimeout(function(){ // remontée lente + servo_gauche.to(100, 1000); + servo_droit.to(70, 1000); + }, 500); + setTimeout(function(){ + servo_gauche.to(50); + servo_droit.to(120); + }, 2000); + setTimeout(function(){ // remontée lente + servo_gauche.to(100, 1000); + servo_droit.to(70, 1000); + }, 2500); + setTimeout(function(){ + servo_gauche.to(50); + servo_droit.to(120); + }, 4000); + setTimeout(callback, 6000); + }; + + return Servos; +})(); \ No newline at end of file diff --git a/clients/gr/servos.simu.class.js b/clients/gr/servos.simu.class.js new file mode 100644 index 0000000..b012742 --- /dev/null +++ b/clients/gr/servos.simu.class.js @@ -0,0 +1,17 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('gr.servos'); + + function Servos(sp) { + + } + + Servos.prototype.acheter = function(callback) { + setTimeout(callback, 500); + }; + Servos.prototype.vendre = function(callback) { + setTimeout(callback, 500); + }; + + return Servos; +})(); \ No newline at end of file diff --git a/clients/pr/actuators.class.js b/clients/pr/actuators.class.js new file mode 100644 index 0000000..dbf6053 --- /dev/null +++ b/clients/pr/actuators.class.js @@ -0,0 +1,532 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('pr.acts'); + var serialPort = require("serialport"); + var SerialPort = serialPort.SerialPort; + var spawn = require('child_process').spawn; + var fifo = new (require('../shared/fifo.class.js'))(); + + var others = null; + var asserv = null; + var ax12 = null; + var date = new Date(); + var lastSendStatus = date.getTime(); + + function Acts(client, sendChildren) { + this.client = client; + this.sendChildren = sendChildren; + this.start(); + this.nb_plots = 0; + this.new_has_ball = false; + this.has_gobelet = false; + } + + Acts.prototype.start = function(){ + + }; + + Acts.prototype.clean = function(){ + fifo.clean(); // A priori déjà vide + asserv.clean(); + ax12.ouvrir(); + others.ouvrirStabilisateurGrand(); + others.ouvrirBloqueurGrand(); + }; + + Acts.prototype.connectTo = function(struct){ + if (!struct.others) { + logger.fatal("Lancement de others pr en mode simu !"); + others = new (require('./others.simu.class.js'))(fifo); + } else { + others = new (require('./others.class.js'))( + new SerialPort(struct.others, { baudrate: 57600 }), + this.sendStatus, + fifo + ); + } + + if (!struct.asserv) { + logger.fatal("Lancement de l'asserv pr en mode simu !"); + asserv = new (require('../shared/asserv.simu.class.js'))(this.client, 'pr', fifo); + } else { + asserv = new (require('../shared/asserv.class.js'))( + new SerialPort(struct.asserv, { + baudrate: 57600, + parser:serialPort.parsers.readline('\n') + }), this.client, 'pr', this.sendStatus, fifo + ); + } + + if (!struct.ax12) { + logger.fatal("Lancement de l'usb2ax pr en mode simu !"); + ax12 = new (require('./ax12.simu.class.js'))(fifo); + } else { + ax12 = new (require('./ax12.class.js'))(struct.ax12, this.sendStatus, fifo); + } + + // Initialisation + setTimeout(function() { + others.ouvrirStabilisateurGrand(); + others.lacherGobelet(); + others.ouvrirBloqueurGrand(); + others.descendreAscenseur(); + ax12.ouvrir(function() { + logger.fatal('BAZOOKA'); + }); + }, 1000); + }; + + Acts.prototype.sendStatus = function() { + if(lastSendStatus < date.getTime()-1000){ + this.sendChildren(this.getStatus); + lastSendStatus = date.getTime(); + } + }; + + Acts.prototype.getStatus = function(){ + var data = { + "status": "", + "children": [] + }; + + data.status = "everythingIsAwesome"; + + if(others && !!others.ready) + data.children.push("Arduino others"); + else + data.status = "ok"; + + if(ax12 && !!ax12.ready) + data.children.push("USB2AX"); + else + data.status = "ok"; + + if(asserv && !!asserv.ready) + data.children.push("Arduino asserv"); + else + data.status = "error"; + + return data; + }; + + Acts.prototype.quit = function(){ + if (ax12 && ax12.ready) + ax12.disconnect(); + }; + + function fake() {} + Acts.prototype.delay = function(ms, callback){ + fifo.newOrder(function() { + setTimeout(function() { + if(callback !== undefined) + callback(); + fifo.orderFinished(); + }, ms); + }, 'delay'); + } + + Acts.prototype.prendre_plot = function(callback, monter){ + if(callback === undefined) { + callback = function() {}; + } + if (monter === undefined) { + monter = 1; + } + var that = this; + if (that.new_has_ball) { + that.new_has_ball = false; + others.descendreUnPeuAscenseur(); + ax12.ouvrir(); + others.descendreAscenseur(); + that.prendre_plot(callback); + } + else if (that.nb_plots>=3 || !monter){ + ax12.ouvrir(); + others.fermerStabilisateur(); + ax12.fermer(); + others.monterUnPeuAscenseur(function() { + that.client.send('ia', 'pr.plot++'); + callback(); + }); + others.ouvrirBloqueurMoyen(); + others.fermerBloqueur(); + } + else if (that.nb_plots==0) { + ax12.ouvrir(); + others.ouvrirBloqueurMoyen(); + ax12.fermer(); + others.monterAscenseur(function() { + that.client.send('ia', 'pr.plot++'); + setTimeout(callback, 200); + }); + others.fermerBloqueur(); + ax12.ouvrir(); + others.ouvrirStabilisateurMoyen(function() {}, 0); + others.descendreAscenseur(); + } + else if(that.nb_plots==1){ + ax12.ouvrir(); + others.ouvrirStabilisateurMoyen(); + ax12.fermer(); + others.ouvrirBloqueurMoyen(); + others.monterAscenseur(function() { + that.client.send('ia', 'pr.plot++'); + setTimeout(callback, 200); + }); + others.fermerBloqueur(); + ax12.ouvrir(function() {}); + others.descendreAscenseur(); + } + else { + ax12.ouvrir(); + others.ouvrirStabilisateurMoyen(); + ax12.fermer(); + others.ouvrirBloqueurMoyen(); + others.monterAscenseur(function() { + that.client.send('ia', 'pr.plot++'); + setTimeout(callback, 200); + }); + others.fermerStabilisateur(fake,0); + others.fermerBloqueur(); + ax12.ouvrir(function() {}); + others.descendreAscenseur(); + } + that.nb_plots++; + } + + Acts.prototype.pousser_plot = function(callback){ + if(callback === undefined) { + callback = function() {}; + } + ax12.ouvrir(); + asserv.goxy(1150, 200, "avant"); + asserv.goxy(1100, 250, "arriere", callback); + } + + Acts.prototype.prendre_plot2 = function(callback){ + if(callback === undefined) { + callback = function() {}; + } + var that = this; + asserv.speed(300, 0, 750); + if (that.new_has_ball) { + that.new_has_ball = false; + others.descendreUnPeuAscenseur(); + ax12.ouvrir(); + others.descendreAscenseur(); + that.prendre_plot(callback); + } + else { + + ax12.ouvrir(); + ax12.fermer(); + others.monterUnPeuAscenseur(function() { + that.client.send('ia', 'pr.plot++'); + callback(); + }); + } + + that.nb_plots++; + } + + Acts.prototype.monter_plot2 = function(callback){ + if(callback === undefined) { + callback = function() {}; + } + var that = this; + + callback(); + if (that.nb_plots == 1) { + others.ouvrirBloqueurMoyen(); + others.monterAscenseur(); + others.fermerBloqueur(); + others.fermerStabilisateur(); + ax12.ouvrir(); + others.descendreAscenseur(); + } else { + others.ouvrirStabilisateurMoyen(); + others.ouvrirBloqueurMoyen(); + others.monterAscenseur(); + others.fermerBloqueur(); + others.fermerStabilisateur(); + ax12.ouvrir(); + others.descendreAscenseur(); + } + } + + // Order switch + Acts.prototype.orderHandler = function (from, name, params, callback) { + // logger.info("Just received an order `" + name + "` from " + from + " with params :"); + // logger.info(params); + var that = this; + // TODO : add a callback parameter to all functions (and call it) + switch (name){ + // others + case "placer": + asserv.setPid(0.25, 130, 13); + asserv.goxy(500, 940); + asserv.goa(-0.62); + this.orderHandler('ia','fermer_tout', {}, callback); + break; + case "prendre_plot": + this.prendre_plot(callback); + break; + case "pousser_plot": + this.pousser_plot(callback); + break; + case "prendre_plot2": + this.prendre_plot2(callback); + break; + case "monter_plot2": + this.monter_plot2(callback); + break; + case "reset_nb_plots": + this.nb_plots = 0; + callback(); + break; + case "prendre_plot_rear_left": + asserv.goxy(160, 1800, "avant"); + that.prendre_plot(undefined, 0); + asserv.goxy(270, 1800, "arriere", callback); + break; + case "prendre_plot_rear_left_calage": + asserv.goxy(250, 1450, "arriere"); + asserv.goa(1.5708); + asserv.pwm(-70, -70, 1500); + asserv.calageY(1288, 1.5708); + asserv.goxy(250, 1450, 500); + asserv.goa(0); + asserv.pwm(-70, -70, 1500); + asserv.calageX(65, 0); + asserv.goxy(120, 1450, "avant"); + asserv.goxy(120, 1790, "avant"); + that.prendre_plot(); + asserv.goa(-1); + asserv.speed(500, 0, 750, callback); + break; + case "prendre_gobelet_et_2_plots_front": + others.lacherGobelet(fake,0); + asserv.goxy(275, 260, "arriere"); + others.prendreGobelet(function() { + that.has_gobelet = true; + that.client.send('ia', 'pr.gobelet1'); + }); + asserv.speed(500, 0, 500); + asserv.goxy(165, 270, "avant"); //100 au lieu de 90 pos plot + that.prendre_plot(undefined, 1); + //asserv.speed(-300, -300, 600); + asserv.goxy(285, 310, "arriere"); + asserv.goxy(155, 180, "avant"); + that.prendre_plot(callback); + break; + + case "deposer_pile_left_2": + asserv.goxy(650, 1000, "avant"); + others.descendreAscenseur(); + ax12.ouvrir(); + others.ouvrirBloqueurMoyen(fake,0); + others.ouvrirStabilisateurMoyen(); + that.delay(500); + others.ouvrirBloqueurGrand(fake,0); + others.ouvrirStabilisateurGrand(function() { + that.nb_plots = 0; + that.client.send('ia', 'pr.plot0'); + }); + asserv.speed(-300, 0, 1000); + break; + + case "recalage_stairs": + asserv.goxy(250, 1450, "arriere"); + asserv.goa(1.5708); + asserv.pwm(-80, -80, 1500); + asserv.calageY(1288, 1.5708); + asserv.goxy(250, 1450, 500); + asserv.goa(0); + asserv.pwm(-80, -80, 1500); + asserv.calageY(65, 0); + asserv.goxy(120, 1450, "avant"); + break; + + case "prendre_2_plots_stairs": + asserv.goxy(800, 1740, "avant"); + that.prendre_plot(undefined, 1); + that.delay(1000); + asserv.goxy(830, 1850, "avant"); + that.prendre_plot(undefined, 0); + asserv.speed(-300, 0, 1000, callback); + break; + + case "prendre_gobelet": + others.lacherGobelet(fake,0); + asserv.speed(-300, 0, 500); + others.prendreGobelet(function() { + that.client.send('ia', 'pr.gobelet1'); + that.has_gobelet = true; + callback(); + }); + break; + + case "deposer_pile_et_gobelet": + asserv.goxy(250, 1000, "avant"); + others.descendreUnPeuAscenseur(); + ax12.ouvrir(); + others.ouvrirBloqueurMoyen(fake,0); + others.ouvrirStabilisateurMoyen(); + that.delay(500); + others.ouvrirBloqueurGrand(fake,0); + others.ouvrirStabilisateurGrand(function() { + that.nb_plots = 0; + that.client.send('ia', 'pr.plot0'); + }); + asserv.speed(-300, 0, 1000); + asserv.goa(0); + others.lacherGobelet(function() { + that.client.send('ia', 'pr.gobelet0'); + }); + that.delay(300); + asserv.speed(300, 0, 750, callback); + others.ouvrirStabilisateurMoyen(fake,0); + others.ouvrirBloqueurMoyen(fake,0); + others.descendreAscenseur(); + break; + + case "deposer_pile_gobelet_prendre_balle_gauche": + asserv.goxy(300, 1000, "avant"); + //asserv.goa(-2.3562); + others.descendreUnPeuAscenseur(); + ax12.ouvrir(); + others.ouvrirBloqueurGrand(fake,0); + others.ouvrirStabilisateurMoyen(); + that.delay(500); + others.ouvrirStabilisateurGrand(); + asserv.speed(-300, 0, 1000); + asserv.goa(0); + others.lacherGobelet(); + that.delay(300); + asserv.speed(300, 0, 750); + /*asserv.goxy(700, 1300, "arriere", function() { + that.client.send('ia', 'data.add_dynamic', {pos:{x:450, y:880}, d:8}); + }); + others.ouvrirBloqueurMoyen(fake,0); + others.ouvrirStabilisateurMoyen(fake,0); + others.monterMoyenAscenseur(); + + asserv.goxy(260, 1050, "osef"); + asserv.goa(3.1416/2); + asserv.pwm(50, 50, 1500); + asserv.calageY(1126, 3.1416/2); + asserv.goxy(260, 1000, "arriere"); + asserv.goa(3.1416); + + asserv.goxy(200, 1000, "avant"); + asserv.goa(3.1416); + asserv.pwm(50, 50, 1500); + asserv.calageX(145, 3.1416); + ax12.fermerBalle(); + asserv.goxy(260, 1000, "arriere"); + others.descendreMoyenAscenseur(); + ax12.fermerBalle2(); + asserv.goxy(260, 1000, "arriere"); + others.monterAscenseur(); + asserv.goxy(330, 1000, "avant"); + asserv.goa(0.1863); + others.lacherGobelet(); + that.delay(100); + that.new_has_ball = true; + that.client.send('ia', 'pr.plot0'); + that.client.send('ia', 'pr.gobelet0'); + that.nb_plots = 0; + that.has_gobelet = false; + asserv.goxy(600, 1050, "avant", callback);*/ + break; + + case "deposer_gobelet": + asserv.goa(3.1416); + that.delay(100); + others.lacherGobelet(); + that.client.send('ia', 'pr.gobelet0'); + asserv.speed(500, 0, 800, callback); + break; + + case "fermer_tout": + others.lacherGobelet(); + others.fermerBloqueur(); + others.descendreAscenseur(); + others.rangerClap(); + others.fermerStabilisateur(); + ax12.fermer(callback); + break; + case "ouvrir_ax12": + ax12.ouvrir(callback); + break; + case "monter_ascenseur": + others.monterAscenseur(callback); + break; + case "clap_1": + others.sortirClap(); + asserv.goxy(330, 140, "osef", function() { + callback(); + others.rangerClap(); + }); + break; + case "clap_3": + others.sortirClap(); + asserv.goxy(1000, 140, "osef", function() { + callback(); + others.rangerClap(); + }); + break; + case "clap_5": + others.sortirClap(); + asserv.goxy(2300, 140, "osef", function() { + callback(); + others.rangerClap(); + }); + break; + + // Asserv + case "pwm": + asserv.pwm(params.left, params.right, params.ms, callback); + break; + case "setvit": + asserv.setVitesse(params.v, params.r, callback); + break; + case "clean": + asserv.clean(callback); + break; + case "goa": + asserv.goa(params.a, callback, true); + break; + case "goxy": + asserv.goxy(params.x, params.y, params.sens, callback, true); + break; + case "setpos": + asserv.setPos(params, callback); + break; + case "setacc": + asserv.setAcc(params.acc, callback); + break; + case "setpid": + asserv.setPid(params.p, params.i, params.d, callback); + break; + case "sync_git": + spawn('/root/sync_git.sh', [], { + detached: true + }); + break; + + + // Debug + case "AX12_close": + ax12.fermer(callback); + break; + case "AX12_open": + ax12.ouvrir(callback); + break; + default: + logger.warn("Order name " + name + " " + from + " not understood"); + callback(); + } + }; + + return Acts; +})(); diff --git a/clients/pr/ax12.class.js b/clients/pr/ax12.class.js new file mode 100644 index 0000000..11a71b0 --- /dev/null +++ b/clients/pr/ax12.class.js @@ -0,0 +1,178 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('pr.ax12'); + var ffi = require('ffi'); + var libusb2ax = ffi.Library('./libs/dynamixel/lib/libusb2ax', { + 'dxl_initialize': ['int', ['int', 'int']], + 'dxl_write_word': ['void', ['int', 'int', 'int']], + 'dxl_read_word': ['int', ['int', 'int']], + 'dxl_terminate': ['void', ['void']], + 'dxl_get_result': ['int', ['void']] + }); + + // Constants + var AX12_COUPLE = 800; + var P_GOAL_POSITION_L = 30; + var P_POSITION = 36; + var P_SPEED = 0x26; + var P_COUPLE = 34; + var MARGE_POS = 80; + var MARGE_POS_MVT = 5; + var ax12s = { + '2':{ + id: 2, + obj: 0, pos: 0, arrived: false + }, + '3':{ + id: 3, + obj: 0, pos: 0, arrived: false + } + }; + + function Ax12(sp, sendStatus, fifo) { + // sp is Serial Port NAME + this.ready = false; + this.sendStatus = sendStatus; + this.orders_sent = []; + this.fifo = fifo; + + this.connect(sp); + } + + + // ====== General actions ====== + + POS_OPENED = 40; + POS_CLOSED = 10; + POS_BALL_1 = 0; + POS_BALL_2 = 0; + + + Ax12.prototype.connect = function(sp) { + if(libusb2ax.dxl_initialize(sp.substring("/dev/ttyACM".length), 1) <= 0) { + logger.error("Impossible de se connecter à l'USB2AX"); + } else { + logger.info("Connecté à l'USB2AX !"); + } + this.ready = true; + this.sendStatus(); + this.ax12s = {}; + this.type_callback = null; + + libusb2ax.dxl_write_word(2, P_COUPLE, 400); + libusb2ax.dxl_write_word(3, P_COUPLE, 400); + + this.ouvrir(); + this.loopAX12(); + }; + + Ax12.prototype.disconnect = function(x) { + this.ready = false; + this.sendStatus(); + }; + + Ax12.prototype.loopAX12 = function() { + var speed; + for(var i in ax12s) { + // S'il n'est pas à la bonne position + ax12s[i].pos = libusb2ax.dxl_read_word(ax12s[i].id, P_POSITION); + speed = libusb2ax.dxl_read_word(ax12s[i].id, P_SPEED); + + if (!ax12s[i].started) { + libusb2ax.dxl_write_word(ax12s[i].id, P_COUPLE, AX12_COUPLE); + libusb2ax.dxl_write_word(ax12s[i].id, P_GOAL_POSITION_L, ax12s[i].obj); + if (Math.abs(speed) > MARGE_POS_MVT) { + ax12s[i].started = true; + } + } else { + speed = libusb2ax.dxl_read_word(ax12s[i].id, P_SPEED); + if (Math.abs(speed) < MARGE_POS_MVT) { + ax12s[i].arrived = true; + } + } + if(Math.abs(ax12s[i].pos - ax12s[i].obj) < MARGE_POS) { + ax12s[i].arrived = true; + } + } + // logger.debug(ax12s['2'].pos + ' ; ' + ax12s['3'].pos); + if(ax12s['2'].started && ax12s['3'].started && this.type_callback == "ouvrir" || + ax12s['2'].arrived && ax12s['3'].arrived && this.type_callback) { + this.type_callback = null; + this.callback(); + this.fifo.orderFinished(); + } + + setTimeout(function() { this.loopAX12(); }.bind(this), 50); + }; + + Ax12.prototype.degToAx12 = function(deg) { + return parseInt((deg+150)*1024/300); + }; + + Ax12.prototype.ouvrir = function(callback) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + ax12s['2'].obj = this.degToAx12(0); + ax12s['3'].obj = this.degToAx12(0); + ax12s['2'].arrived = false; + ax12s['2'].started = false; + ax12s['3'].arrived = false; + ax12s['3'].started = false; + this.callback = callback; + this.type_callback = 'ouvrir'; + }.bind(this), 'AX12-Ouvrir'); + }; + + Ax12.prototype.fermer = function(callback) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + ax12s['2'].obj = this.degToAx12(-85); + ax12s['3'].obj = this.degToAx12(85); + // logger.debug(ax12s['2'].obj); + ax12s['2'].arrived = false; + ax12s['2'].started = false; + ax12s['3'].arrived = false; + ax12s['3'].started = false; + this.callback = callback; + this.type_callback = 'fermer'; + }.bind(this), 'AX12-Fermer'); + }; + Ax12.prototype.fermerBalle = function(callback) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + ax12s['2'].obj = this.degToAx12(-50); + ax12s['3'].obj = this.degToAx12(50); + // logger.debug(ax12s['2'].obj); + ax12s['2'].arrived = false; + ax12s['2'].started = false; + ax12s['3'].arrived = false; + ax12s['3'].started = false; + this.callback = callback; + this.type_callback = 'fermer'; + }.bind(this), 'AX12-Fermer balle'); + }; + Ax12.prototype.fermerBalle2 = function(callback) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + ax12s['2'].obj = this.degToAx12(-75); + ax12s['3'].obj = this.degToAx12(75); + // logger.debug(ax12s['2'].obj); + ax12s['2'].arrived = false; + ax12s['2'].started = false; + ax12s['3'].arrived = false; + ax12s['3'].started = false; + this.callback = callback; + this.type_callback = 'fermer'; + }.bind(this), 'AX12-Fermer balle 2'); + }; + + return Ax12; +})(); diff --git a/clients/pr/ax12.simu.class.js b/clients/pr/ax12.simu.class.js new file mode 100644 index 0000000..6da5604 --- /dev/null +++ b/clients/pr/ax12.simu.class.js @@ -0,0 +1,39 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('pr.ax12'); + + function Ax12(fifo) { + this.fifo = fifo; + } + + Ax12.prototype.disconnect = function(x) { + + }; + + Ax12.prototype.callCallback = function(callback, ms) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + setTimeout(function() { + callback(); + this.fifo.orderFinished(); + }.bind(this), parseInt(ms/5)); + }.bind(this)); + } + + Ax12.prototype.ouvrir = function(callback) { + this.callCallback(callback, 1000); + }; + + Ax12.prototype.fermer = function(callback) { + this.callCallback(callback, 1000); + }; + Ax12.prototype.fermerBalle = function(callback) { + this.callCallback(callback, 800); + }; + Ax12.prototype.fermerBalle2 = function(callback) { + this.callCallback(callback, 900); + }; + + return Ax12; +})(); diff --git a/clients/pr/detect.class.js b/clients/pr/detect.class.js new file mode 100644 index 0000000..c589d85 --- /dev/null +++ b/clients/pr/detect.class.js @@ -0,0 +1,71 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('pr.detect'); + var serialPort = require("serialport"); + var SerialPort = serialPort.SerialPort; + + var sp = []; + + function Detect(callback) { + this.devicesFound = { + asserv: null, + others: null, + ax12: null + }; + this.callback = callback; + this.searchArduinos(); + } + + Detect.prototype.sendSP = function (){ + // Close opened ports & detect other devices + serialPort.list(function (err, ports) { + for(var i in ports) { + if(ports[i].comName.indexOf('ttyUSB') >= 0) { + this.devicesFound.asserv = ports[i].comName; + } else if((ports[i].comName.indexOf('ttyACM') >= 0) && ports[i].comName != this.devicesFound.others) { + // logger.debug(ports[i].comName); + this.devicesFound.ax12 = ports[i].comName; + } + if(sp[i].readable){ + logger.info("Closing "+ports[i].comName); + // this.devicesFound.ax12 = ports[i].comName; + sp[i].close(); + } + } + + // Sent to acts + this.callback(this.devicesFound); + }.bind(this)); + }; + + Detect.prototype.searchArduinos = function() { + // On check tous les ports disponibles + serialPort.list(function (err, ports) { + var nb_found = 0; + for(var i in ports) { + sp[i] = new SerialPort(ports[i].comName, { baudrate: 57600 }); + sp[i].on('open', function (i) { + sp[i].write('O\n'); + }.bind(this, i)); + + sp[i].on("data", function (i, data) { + data = data.toString(); + console.log(ports[i].comName, data); + if (data == 'O' && !this.devicesFound.others){ // Stepper + this.devicesFound.others = ports[i].comName; + clearTimeout(timeout); + this.sendSP(); + } + sp[i].close(); + }.bind(this, i)); + + sp[i].on("error", function() {}); // Node JS Error if it doesn't exist (and if an "error" event is sent) + } + }.bind(this)); + + // On check tous les ports qui ne sont pas enregistrés + timeout = setTimeout(function(){this.sendSP(); }.bind(this), 3000); + }; + + return Detect; +})(); \ No newline at end of file diff --git a/clients/pr/elevator.class.js b/clients/pr/elevator.class.js new file mode 100644 index 0000000..fd94ca9 --- /dev/null +++ b/clients/pr/elevator.class.js @@ -0,0 +1,83 @@ +module.exports = (function () { + var log4js = require('log4js'); + var logger = log4js.getLogger('pr.elevator'); + var serialPort = require("serialport"); + var SerialPort = serialPort.SerialPort; + + var ORDER_ACHIEVED = 'K'; // Like Ok + var ORDER_UNKNOWN = 'U'; // Like Unknown + + var ELEV_MOVE_UP = 'u'; + var ELEV_MOVE_DOWN = 'd'; + var ELEV_RELEASE = 'r'; + var ELEV_CHOUILLA = 'c'; + + function Elevator(sp, sendStatus) { + // sp is Serial Port OBJECT + this.sp = sp; + this.ready = false; + this.sendStatus = sendStatus; + this.pos = 'down'; + this.orders_sent = []; + + this.sp.on("data", function(data) { + this.ready = true; + this.sendStatus(); + this.parseOrder(data.toString()); + }.bind(this)); + } + + // Fonctions for sending orders to the Arduino + Elevator.prototype.sendOrder = function(order) { + this.sp.write(order); + this.orders_sent.push(order); + }; + + Elevator.prototype.disconnect = function() { + this.sp.close(); + this.ready = false; + this.sendStatus(); + }; + + + // ====== General actions ====== + + Elevator.prototype.monter = function() { + this.sendOrder(ELEV_MOVE_UP); + }; + + Elevator.prototype.monterChouilla = function() { + this.sendOrder(ELEV_CHOUILLA); + }; + + Elevator.prototype.descendre = function() { + this.sendOrder(ELEV_MOVE_DOWN); + this.lacher(); + }; + + Elevator.prototype.release = function() { + this.sendOrder(ELEV_RELEASE); + }; + + Elevator.prototype.parseOrder = function(order) { + if(order == ORDER_ACHIEVED) { + switch(this.orders_sent.shift()) { + case ELEV_MOVE_UP: + this.pos = 'up'; + logger.info("Elevator is up"); + break; + case ELEV_MOVE_DOWN: + this.pos1 = 'down'; + logger.info("Elevator is down"); + break; + } + } else if (order == ORDER_UNKNOWN) { + oldest_order = this.orders_sent.shift(); + logger.warn("Order sent unknown: "+this.orders_sent.shift()); + } else { + logger.warn("Order received unknown: "+order); + } + }; + + return Elevator; +})(); \ No newline at end of file diff --git a/clients/pr/main.js b/clients/pr/main.js new file mode 100644 index 0000000..7df3c5a --- /dev/null +++ b/clients/pr/main.js @@ -0,0 +1,174 @@ +(function () { + "use strict"; + + // Requires + var logger = require('log4js').getLogger('pr'); + + // logger.info("Started NodeJS client with pid " + process.pid); + + var SocketClient = require('../../server/socket_client.class.js'); + var server = require('../../config.js').server + var client = new SocketClient({ + server_ip: server, + type: "pr" + }); + + var lastStatus = { + "status": "waiting" + }; + sendChildren(lastStatus); + + var acts = new (require('./actuators.class.js'))(client, sendChildren); + var detect = null; + + var queue = []; + var orderInProgress = false; + + start(); + + // On message + client.order(function (from, name, params){ + // logger.info("Recieved an order "+name); + switch (name){ + case "collision": + queue = []; + acts.clean(); + orderInProgress = false; + break; + case "stop": + acts.clean(); + setTimeout(function() { + logger.fatal("Stop PR"); + process.exit(); + }, 800); + break; + + // useless // + case "start": + queue = []; + start(); + break; + default: + addOrder2Queue(from, name, params); + } + }); + + function start(){ + logger.info("Starting :)"); + sendChildren({ + status: "starting", + children:[] + }); + detect = new (require('./detect.class.js'))(devicesDetected); + } + + // function stop(){ + // acts.quit(); + + // // Send struct to server + // sendChildren({ + // status: "waiting", + // children:[] + // }); + // } + + function devicesDetected(struct){ + // Verify content + if (!struct.others) + logger.warn("Missing others Mega"); + + // if (!struct.servos) + // logger.warn("Missing servos Nano"); + + if (!struct.asserv) + logger.warn("Missing asserv Nano"); + + if (!struct.ax12) + logger.warn("Missing USB2AX"); + + // Connect to what's detected + acts.connectTo(struct); + + // Send struct to server + sendChildren(acts.getStatus()); + } + + // Sends status to server + function sendChildren(status){ + lastStatus = status; + + client.send("server", "server.childrenUpdate", lastStatus); + } + + function isOk(){ + if(lastStatus.status != "waiting") + lastStatus = acts.getStatus(); + + client.send("ia", "isOkAnswer", lastStatus); + client.send("server", "server.childrenUpdate", lastStatus); + } + + + // Push the order (enfiler) + function addOrder2Queue(f, n, p){ + // if(n == 'clean') { + // logger.info(n+" : Begin"); + // acts.orderHandler(f, n, p, actionFinished); + // } else + if(queue.length < 100){ + // Adds the order to the queue + queue.push({ + from: f, + name: n, + params: p + }); + // logger.info("Order added to queue ! : "); + // logger.info(queue); + + executeNextOrder(); + } + } + + // Execute order + function executeNextOrder(){ + if ((queue.length > 0) && (!orderInProgress)) { + var order = queue.shift(); + if(order.name == "send_message") { + // logger.debug("Send message %s", order.params.name); + client.send('ia', order.params.name, order.params || {}); + executeNextOrder(); + } else { + orderInProgress = order.name; + + logger.info(orderInProgress+" : Begin"); + // logger.debug(order.params); + acts.orderHandler(order.from, order.name, order.params, actionFinished); + + // executeNextOrder(); + } + } + } + + function actionFinished(){ + if(orderInProgress !== false) { + logger.info(orderInProgress + " : End"); + + orderInProgress = false; + executeNextOrder(); + } + } + function quit () { + logger.info("Please wait while exiting..."); + acts.quit(); + process.exit(); + } + + + // Exiting : + //do something when app is closing + // process.on('exit', quit); + // catches ctrl+c event + // process.on('SIGINT', quit); + // //catches uncaught exceptions + // process.on('uncaughtException', quit); +})(); diff --git a/clients/pr/others.class.js b/clients/pr/others.class.js new file mode 100644 index 0000000..6be901c --- /dev/null +++ b/clients/pr/others.class.js @@ -0,0 +1,178 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('Others'); + + function Others(sp, sendStatus, fifo) { + this.sp = sp; + // this.client = client; + this.ready = true; + logger.debug(sendStatus); + this.sendStatus = sendStatus; + this.fifo = fifo; + + this.sp.on("data", function(data){ + if(this.ready === false){ + this.ready = true; + this.sendStatus(); + } + this.parseCommand(data.toString()); + }.bind(this)); + this.sp.on("error", function(data){ + logger.debug(data.toString()); + }); + this.sp.on("close", function(){ + this.ready = false; + this.sendStatus(); + logger.error("Serial port close"); + }.bind(this)); + } + + Others.prototype.parseCommand = function(data) { + if(this.order_sent == data) { + this.order_sent = ''; + setTimeout(function() { + this.callback(); + this.fifo.orderFinished(); + }.bind(this), this.callback_delay); + } else { + logger.warn("Arduino others unknown: "+data+" (order_sent : "+this.order_sent+")"); + } + }; + + Others.prototype.sendCommand = function(callback, cmd, args, callback_delay){ + if(callback === undefined) { + callback = function(){}; + } + + this.fifo.newOrder(function() { + this.callback = callback; + this.callback_delay = callback_delay; + this.order_sent = cmd; + + //logger.debug([cmd].concat(args).join(";")); + this.sp.write([cmd].concat(args).join(";")+"\n"); + }.bind(this), 'sendCommandOther('+cmd+':'+args+')'); + }; + + Others.prototype.fermerStabilisateur = function(callback, time) { + if (time === undefined) { + time = 100; + } + this.sendCommand(callback, 'H', [100, 23], time); + }; + + Others.prototype.ouvrirStabilisateurMoyen = function(callback, time) { + if (time === undefined) { + time = 100; + } + this.sendCommand(callback, 'H', [90, 30], time); + }; + + Others.prototype.ouvrirStabilisateurGrand = function(callback, time) { + if (time === undefined) { + time = 400; + } + this.sendCommand(callback, 'H', [40, 80], time); + }; + + Others.prototype.fermerBloqueur = function(callback, time) { + if (time === undefined) { + time = 200; + } + this.sendCommand(callback, 'M', [32, 68], time); + }; + + Others.prototype.ouvrirBloqueurMoyen = function(callback, time) { + if (time === undefined) { + time = 200; + } + this.sendCommand(callback, 'M', [66, 29], time); + }; + + Others.prototype.ouvrirBloqueurGrand = function(callback, time) { + if (time === undefined) { + time = 400; + } + this.sendCommand(callback, 'M', [106, 1], time); + }; + + Others.prototype.prendreGobelet = function(callback, time) { + if (time === undefined) { + time = 200; + } + this.sendCommand(callback, 'G', [115], time); + }; + + Others.prototype.lacherGobelet = function(callback, time) { + if (time === undefined) { + time = 200; + } + this.sendCommand(callback, 'G', [50], time); + }; + + Others.prototype.sortirClap = function(callback, time) { + if (time === undefined) { + time = 200; + } + this.sendCommand(callback, 'C', [130], time); + }; + + Others.prototype.rangerClap = function(callback, time) { + if (time === undefined) { + time = 100; + } + this.sendCommand(callback, 'C', [40], time); + }; + + Others.prototype.monterAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [-250], time); + }; + + Others.prototype.monterUnPeuAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [-30], time); + }; + + Others.prototype.descendreUnPeuAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [30], time); + }; + + Others.prototype.monterMoyenAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [-60], time); + }; + + Others.prototype.descendreMoyenAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [60], time); + }; + + Others.prototype.relacherAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(callback, 'S', [0], time); + }; + + Others.prototype.descendreAscenseur = function(callback, time) { + if (time === undefined) { + time = 0; + } + this.sendCommand(function() { + this.relacherAscenseur(callback); + }.bind(this), 'S', [250], time); + }; + + return Others; +})(); diff --git a/clients/pr/others.simu.class.js b/clients/pr/others.simu.class.js new file mode 100644 index 0000000..81e5425 --- /dev/null +++ b/clients/pr/others.simu.class.js @@ -0,0 +1,90 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('pr.others'); + + function Others(fifo) { + this.fifo = fifo; + } + + Others.prototype.disconnect = function(x) { + + }; + + Others.prototype.callCallback = function(callback, ms) { + if(callback === undefined) { + callback = function(){}; + } + this.fifo.newOrder(function() { + setTimeout(function() { + callback(); + this.fifo.orderFinished(); + }.bind(this), parseInt(ms/5)); + }.bind(this)); + }; + + Others.prototype.fermerStabilisateur = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.ouvrirStabilisateurMoyen = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.ouvrirStabilisateurGrand = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.fermerBloqueur = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.ouvrirBloqueurMoyen = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.ouvrirBloqueurGrand = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.sortirClap = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.rangerClap = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.prendreGobelet = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.lacherGobelet = function(callback) { + this.callCallback(callback, 200); + }; + + Others.prototype.monterAscenseur = function(callback) { + this.callCallback(callback, 1000); + }; + + Others.prototype.monterUnPeuAscenseur = function(callback) { + this.callCallback(callback, 300); + }; + + Others.prototype.descendreUnPeuAscenseur = function(callback) { + this.callCallback(callback, 300); + }; + + Others.prototype.monterMoyenAscenseur = function(callback) { + this.callCallback(callback, 500); + }; + + Others.prototype.descendreMoyenAscenseur = function(callback) { + this.callCallback(callback, 500); + }; + + Others.prototype.descendreAscenseur = function(callback) { + this.callCallback(callback, 1000); + }; + + + return Others; +})(); diff --git a/clients/shared/asserv.class.js b/clients/shared/asserv.class.js new file mode 100644 index 0000000..0955155 --- /dev/null +++ b/clients/shared/asserv.class.js @@ -0,0 +1,540 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('asserv'); + var COMMANDS = require('./defineParser.js')('./arduino/asserv/protocol.h'); + var DETECT_SERIAL_TIMEOUT = 100; //ms, -1 to disable + + // function Asserv(sp, client) { + // this.client = client; + // this.getPos(); + // this.sp = sp; + // this.pos = {}; + // this.sentCommands = {}; + // this.currentId = 0; + // } + + function Asserv(sp, client, who, sendStatus, fifo) { + this.ready = true; + this.sendStatus = sendStatus; + this.sp = sp; + this.client = client; + this.pos = {}; + this.who = who; + this.currentId = 0; + this.color = "yellow"; + this.fifo = fifo; + + this.sp.on("data", function(data){ + if(this.ready === false){ + this.ready = true; + this.sendStatus(); + } + this.parseCommand(data.toString()); + }.bind(this)); + this.sp.on("error", function(data){ + this.ready = false; + this.sendStatus(); + logger.debug("error", data.toString()); + }.bind(this)); + this.sp.on("close", function(data){ + this.ready = false; + this.sendStatus(); + logger.error("Serial port close"); + }.bind(this)); + + setTimeout(function() { + this.getPos(); + }.bind(this), 2000); + } + Asserv.prototype.convertColorX = function(x) { + if(this.color == "yellow") { + return x; + } else { + return 3000-x; + } + } + Asserv.prototype.convertColorY = function(y) { + if(this.color == "yellow") { + return y; + } else { + return y; + } + } + Asserv.prototype.convertColorA = function(a) { + if(this.color == "yellow") { + return convertA(a); + } else { + return convertA(Math.PI - a); + } + } + + function convertA(a) { return Math.atan2(Math.sin(a), Math.cos(a)); } + Asserv.prototype.setA = function(a) { + // logger.debug(a, convertA(a)); + this.pos.a = convertA(a); + } + Asserv.prototype.Pos = function(pos) { + this.pos.x = pos.x; + this.pos.y = pos.y; + this.setA(pos.a); + } + Asserv.prototype.setPos = function(pos, callback) { + logger.debug(pos); + if(pos.color !== undefined) + this.color = pos.color; + this.sendCommand(COMMANDS.SET_POS, [ + parseInt(this.convertColorX(pos.x)), + parseInt(this.convertColorY(pos.y)), + myWriteFloat(this.convertColorA(pos.a)) + ], false, callback); + } + Asserv.prototype.getPos = function(pos) { + this.client.send('ia', this.who+'.getpos'); + } + Asserv.prototype.sendPos = function() { + this.client.send('ia', this.who+'.pos', this.pos); + } + + Asserv.prototype.setPosCalage = function(pos, callback) { + this.sendCommand(COMMANDS.SET_POS, [ + parseInt(this.convertColorX(pos.x)), + parseInt(this.convertColorY(pos.y)), + myWriteFloat(this.convertColorA(pos.a)) + ], false, function() { + callback(); + this.fifo.orderFinished(); + }.bind(this), true); + } + + Asserv.prototype.calageX = function(x, a, callback) { + if(callback === undefined) + callback = function(){}; + this.fifo.newOrder(function() { + this.setPosCalage({x: x, y: this.pos.y, a: a}, callback); + }.bind(this), 'calageX'); + + } + Asserv.prototype.calageY = function(y, a, callback) { + if(callback === undefined) + callback = function(){}; + this.fifo.newOrder(function() { + this.setPosCalage({x: this.pos.x, y: y, a: a}, callback, true); + }.bind(this), 'calageY'); + } + + // For float + function myWriteFloat(f){ return Math.round(f*COMMANDS.FLOAT_PRECISION); } + function myParseFloat(f){ return parseInt(f)/COMMANDS.FLOAT_PRECISION; } + + + Asserv.prototype.parseCommand = function(data){ + // logger.debug(data); + var datas = data.split(';'); + var cmd = datas.shift();//, id = datas.shift(); + if(cmd == COMMANDS.AUTO_SEND && datas.length >= 4) { // periodic position update + var lastFinishedId = parseInt(datas.shift()); // TODO + this.Pos({ + x: this.convertColorX(parseInt(datas.shift())), + y: this.convertColorY(parseInt(datas.shift())), + a: this.convertColorA(myParseFloat(datas.shift())) + }); + + + this.sendPos(); + + // logger.debug(lastFinishedId); + if(this.currentId != lastFinishedId) { + // logger.fatal('finish id', lastFinishedId); + this.currentId = lastFinishedId; + var use_fifo = this.use_fifo; + this.use_fifo = true; + this.callback(); + if(use_fifo) + this.fifo.orderFinished(); + } + } else if(cmd == this.order_sent) { + this.order_sent = ''; + // logger.debug('finish', datas.shift()); + if(!this.wait_for_id) { + var use_fifo = this.use_fifo; + this.use_fifo = true; + this.callback(); + if(use_fifo) + this.fifo.orderFinished(); + } + } else if (cmd == COMMANDS.JACK) { + logger.info("JACK !"); + this.client.send("ia", "ia.jack"); + } else { + // logger.warn(datas); + logger.warn("Command return from Arduino to unknown cmd="+cmd); + } + } + Asserv.prototype.sendCommand = function(cmd, args, wait_for_id, callback, no_fifo){ + function nextOrder() { + if(callback === undefined) + callback = function(){}; + this.callback = callback; + args = args || []; + this.order_sent = cmd; + this.wait_for_id = wait_for_id; + logger.debug([cmd,this.currentId+1].concat(args).join(";")+"\n"); + this.sp.write([cmd,this.currentId+1].concat(args).join(";")+"\n"); + } + + this.use_fifo = !no_fifo; + + if(this.use_fifo) { + this.fifo.newOrder(nextOrder.bind(this), 'sendCommandAsserv('+cmd+':'+args+')'); + } else { + nextOrder.call(this); + } + } + + Asserv.prototype.setVitesse = function(v, r, callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.SPDMAX, [ + parseInt(v), + myWriteFloat(r) + ], false, callback); + }; + + Asserv.prototype.speed = function(l, a, ms, callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.SPD, [ + parseInt(l), + parseInt(a), + parseInt(ms) + ], true, callback); + }; + + Asserv.prototype.setAcc = function(acc,callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.ACCMAX, [ + parseInt(acc) + ], false, callback); + }; + + Asserv.prototype.clean = function(callback){ + this.sendCommand(COMMANDS.CLEANG, false, callback); + }; + + Asserv.prototype.pwm = function(left, right, ms, callback) { + this.sendCommand(COMMANDS.PWM, [ + parseInt(left), + parseInt(right), + parseInt(ms) + ], true, callback); + + }; + + Asserv.prototype.goxy = function(x, y, sens, callback, no_fifo){ + if(sens == "avant") sens = 1; + else if(sens == "arriere") sens = -1; + else sens = 0; + + this.sendCommand(COMMANDS.GOTO, [ + parseInt(this.convertColorX(x)), + parseInt(this.convertColorY(y)), + sens + ], true, callback, no_fifo); + }; + Asserv.prototype.goa = function(a, callback, no_fifo){ + // this.clean(); + this.sendCommand(COMMANDS.ROT, [ + myWriteFloat(this.convertColorA(a)) + ], true, callback, no_fifo); + }; + + Asserv.prototype.setPid = function(p, i, d, callback){ + // this.clean(); + this.sendCommand(COMMANDS.PIDALL, [ + myWriteFloat(p), + myWriteFloat(i), + myWriteFloat(d) + ],false, callback); + }; + + // Asserv.prototype.gotoPath = function(callback, path){ + // this.clean(); + // if(instanceof path !=== "Array") path = path.path; // not sure about Path class right now + // path.forEach(function(item)); + // }; + + return Asserv; +})(); + +/* + +module.exports = (function () { + var logger = require('log4js').getLogger('asserv'); + var COMMANDS = require('./defineParser.js')('./arduino/asserv/protocol.h'); + var DETECT_SERIAL_TIMEOUT = 100; //ms, -1 to disable + + // function Asserv(sp, client) { + // this.client = client; + // this.getPos(); + // this.sp = sp; + // this.pos = {}; + // this.sentCommands = {}; + // this.currentId = 0; + // } + + function Asserv(sp, client, who, sendStatus, fifo) { + this.ready = true; + this.sendStatus = sendStatus; + this.sp = sp; + this.client = client; + this.pos = {}; + this.who = who; + this.currentId = 0; + this.color = "yellow"; + this.fifo = fifo; + + this.data_sent = {}; + + this.sp.on("data", function(data){ + if(this.ready === false){ + this.ready = true; + this.sendStatus(); + } + this.parseCommand(data.toString()); + }.bind(this)); + this.sp.on("error", function(data){ + this.ready = false; + this.sendStatus(); + logger.debug("error", data.toString()); + }.bind(this)); + this.sp.on("close", function(data){ + this.ready = false; + this.sendStatus(); + logger.debug(data.toString()); + }.bind(this)); + + setTimeout(function() { + this.getPos(); + }.bind(this), 2000); + } + Asserv.prototype.convertColorX = function(x) { + if(this.color == "yellow") { + return x; + } else { + return 3000-x; + } + } + Asserv.prototype.convertColorY = function(y) { + if(this.color == "yellow") { + return y; + } else { + return y; + } + } + Asserv.prototype.convertColorA = function(a) { + if(this.color == "yellow") { + return convertA(a); + } else { + return convertA(Math.PI - a); + } + } + + function convertA(a) { return Math.atan2(Math.sin(a), Math.cos(a)); } + Asserv.prototype.setA = function(a) { + // logger.debug(a, convertA(a)); + this.pos.a = convertA(a); + } + Asserv.prototype.Pos = function(pos) { + this.pos.x = pos.x; + this.pos.y = pos.y; + this.setA(pos.a); + } + Asserv.prototype.setPos = function(pos, callback) { + if(pos.color !== undefined) + this.color = pos.color; + this.sendCommand(COMMANDS.SET_POS, [ + parseInt(this.convertColorX(pos.x)), + parseInt(this.convertColorY(pos.y)), + myWriteFloat(this.convertColorA(pos.a)) + ], false, callback); + } + Asserv.prototype.getPos = function(pos) { + this.client.send('ia', this.who+'.getpos'); + } + Asserv.prototype.sendPos = function() { + this.client.send('ia', this.who+'.pos', this.pos); + } + + Asserv.prototype.setPosCalage = function(pos, callback) { + this.sendCommand(COMMANDS.SET_POS, [ + parseInt(this.convertColorX(pos.x)), + parseInt(this.convertColorY(pos.y)), + myWriteFloat(this.convertColorA(pos.a)) + ], false, function() { + callback(); + this.fifo.orderFinished(); + }.bind(this), true); + } + + Asserv.prototype.calageX = function(x, a, callback) { + if(callback === undefined) + callback = function(){}; + this.fifo.newOrder(function() { + this.setPosCalage({x: x, y: this.pos.y, a: a}, callback); + }.bind(this), 'calageX'); + + } + Asserv.prototype.calageY = function(y, a, callback) { + if(callback === undefined) + callback = function(){}; + this.fifo.newOrder(function() { + this.setPosCalage({x: this.pos.x, y: y, a: a}, callback, true); + }.bind(this), 'calageY'); + } + + // For float + function myWriteFloat(f){ return Math.round(f*COMMANDS.FLOAT_PRECISION); } + function myParseFloat(f){ return parseInt(f)/COMMANDS.FLOAT_PRECISION; } + + + Asserv.prototype.parseCommand = function(data){ + // logger.debug(data); + var datas = data.split(';'); + var cmd = datas.shift();//, id = datas.shift(); + var id = parseInt(datas.shift()); + if(cmd == COMMANDS.AUTO_SEND && datas.length >= 4) { // periodic position update + this.Pos({ + x: this.convertColorX(parseInt(datas.shift())), + y: this.convertColorY(parseInt(datas.shift())), + a: this.convertColorA(myParseFloat(datas.shift())) + }); + + + this.sendPos(); + + // logger.debug(lastFinishedId); + if(this.data_sent[id] !== undefined) { + // logger.fatal('finish id', lastFinishedId); + // this.currentId = lastFinishedId; + this.data_sent[id].callback(); + if(this.data_sent[id].use_fifo) + this.fifo.orderFinished(); + // else + // this.use_fifo = this.old_use_fifo; + delete this.data_sent[id]; + } + } else if(this.data_sent[id] !== undefined && this.data_sent[id].cmd == cmd) { + // this.order_sent = ''; + // logger.debug('finish', datas.shift()); + if(!this.data_sent[id].wait_for_id) { + this.data_sent[id].callback(); + if(this.data_sent[id].use_fifo) + this.fifo.orderFinished(); + delete this.data_sent[id]; + // else + // this.use_fifo = this.old_use_fifo; + } + } else if (cmd == COMMANDS.JACK) { + logger.info("JACK !"); + this.client.send("ia", "ia.jack"); + } else { + // logger.warn(datas); + logger.warn("Command return from Arduino to unknown cmd="+cmd); + } + } + Asserv.prototype.sendCommand = function(cmd, args, wait_for_id, callback, no_fifo){ + if(callback === undefined) + callback = function(){}; + + this.currentId += 1; + var id = this.currentId.toString(); + args = args || []; + + function nextOrder() { + this.sp.write([cmd,id].concat(args).join(";")+"\n"); + } + this.data_sent[id] = { + callback: callback, + cmd: cmd, + wait_for_id: wait_for_id, + use_fifo: !no_fifo + }; + + if(!no_fifo) { + this.fifo.newOrder(nextOrder.bind(this), 'sendCommandAsserv('+cmd+':'+args+')'); + } else { + nextOrder.call(this); + } + } + + Asserv.prototype.setVitesse = function(v, r, callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.SPDMAX, [ + parseInt(v), + myWriteFloat(r) + ], false, callback); + }; + + Asserv.prototype.speed = function(l, a, ms, callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.SPD, [ + parseInt(l), + parseInt(a), + parseInt(ms) + ], true, callback); + }; + + Asserv.prototype.setAcc = function(acc,callback) { + // logger.debug(myWriteFloat(r)); + this.sendCommand(COMMANDS.ACCMAX, [ + parseInt(acc) + ], false, callback); + }; + + Asserv.prototype.clean = function(callback){ + this.sendCommand(COMMANDS.CLEANG, false, callback); + }; + + Asserv.prototype.pwm = function(left, right, ms, callback) { + this.sendCommand(COMMANDS.PWM, [ + parseInt(left), + parseInt(right), + parseInt(ms) + ], true, callback); + + }; + + Asserv.prototype.goxy = function(x, y, sens, callback, no_fifo){ + if(sens == "avant") sens = 1; + else if(sens == "arriere") sens = -1; + else sens = 0; + + this.sendCommand(COMMANDS.GOTO, [ + parseInt(this.convertColorX(x)), + parseInt(this.convertColorY(y)), + sens + ], true, callback, no_fifo); + }; + Asserv.prototype.goa = function(a, callback, no_fifo){ + // this.clean(); + this.sendCommand(COMMANDS.ROT, [ + myWriteFloat(this.convertColorA(a)) + ], true, callback, no_fifo); + }; + + Asserv.prototype.setPid = function(p, i, d, callback){ + // this.clean(); + this.sendCommand(COMMANDS.PIDALL, [ + myWriteFloat(p), + myWriteFloat(i), + myWriteFloat(d) + ],false, callback); + }; + + // Asserv.prototype.gotoPath = function(callback, path){ + // this.clean(); + // if(instanceof path !=== "Array") path = path.path; // not sure about Path class right now + // path.forEach(function(item)); + // }; + + return Asserv; +})(); + +*/ diff --git a/clients/shared/asserv.simu.class.js b/clients/shared/asserv.simu.class.js new file mode 100644 index 0000000..5fffda5 --- /dev/null +++ b/clients/shared/asserv.simu.class.js @@ -0,0 +1,205 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('asserv'); + + // For simu + var SIMU_FACTOR_VIT = 1; + var SIMU_FACTOR_A = 0.3; // Facteur + // var SIMU_PWM_REF = 255; // à une PWM de 80 + function SIMU_DIST(pwm, dt, vitesse) { return (vitesse*SIMU_FACTOR_VIT)*dt; } + function SIMU_DIST_ROT(a) { return Math.abs(a)*100; } // Rayon aproximatif de 10cm + function SIMU_ROT_TIME(a, vitesse) { return SIMU_DIST_ROT(a)/(vitesse*SIMU_FACTOR_VIT*SIMU_FACTOR_A); } + var FPS = 30; + + var timeouts = []; + + function Asserv(client, who, fifo) { + this.client = client; + this.who = who; + this.pos = { + x:0,y:0,a:0 + }; + this.fifo = fifo; + this.vitesse = 800; + this.getPos(); + } + + Asserv.prototype.newOrder = function(callback, ms, no_fifo, delay_order_finished){ + if(callback === undefined) + callback = function(){}; + var use_fifo = !no_fifo; + if(ms === undefined) ms = 0; + if(delay_order_finished === undefined) delay_order_finished = 0; + + function nextOrder() { + timeouts.push(setTimeout(function() { + callback(); + timeouts.push(setTimeout(function() { + this.fifo.orderFinished(); + }.bind(this), delay_order_finished)); + }.bind(this), ms)); + } + + if(use_fifo) { + // logger.debug('use_fifo'); + this.fifo.newOrder(nextOrder.bind(this)); + } else { + // logger.debug('no_fifo'); + timeouts.push(setTimeout(callback, ms));//.call(this)); + } + } + + function convertA(a) { return Math.atan2(Math.sin(a), Math.cos(a)); } + Asserv.prototype.setA = function(a) { + this.pos.a = convertA(a); + } + Asserv.prototype.Pos = function(pos) { + this.pos.x = pos.x; + this.pos.y = pos.y; + this.setA(pos.a); + } + Asserv.prototype.setPos = function(pos, callback) { + this.Pos(pos); + this.sendPos(); + if(callback !== undefined) + callback(); + } + Asserv.prototype.getPos = function(pos) { + this.client.send('ia', this.who+'.getpos'); + } + Asserv.prototype.sendPos = function() { + this.client.send('ia', this.who+'.pos', this.pos); + } + + Asserv.prototype.clean = function(){ + logger.debug('cleaning %d timeouts', timeouts.length); + while(timeouts.length > 0) { + clearTimeout(timeouts.shift()); + } + }; + Asserv.prototype.avancerPlot = function(callback) { + this.newOrder(callback, 1200); + } + + Asserv.prototype.setVitesse = function(v, r, callback) { + this.vitesse = parseInt(v); + if(callback !== undefined) + callback(); + }; + Asserv.prototype.calageX = function(x, a, callback) { + this.setPos({x: x, y: this.pos.y, a: a}, callback); + } + Asserv.prototype.calageY = function(y, a, callback) { + this.setPos({x: this.pos.x, y: y, a: a}, callback); + } + + Asserv.prototype.simu_speed = function(vit, x, y, a, dt) { + return function() { + this.pos = { + x: x + Math.cos(a) * vit*dt/1000, + y: y + Math.sin(a) * vit*dt/1000, + a: a + } + this.sendPos(); + }.bind(this); + } + Asserv.prototype.speed = function(l, a, ms,callback) { + this.newOrder(function() { + // this.simu.pwm(callback, l/3, l/3, ms); + for(var t = 0; t < ms; t += 1000/FPS) { + timeouts.push(setTimeout(this.simu_speed(l, this.pos.x, this.pos.y, this.pos.a, t), t)); + } + timeouts.push(setTimeout(this.simu_speed(l, this.pos.x, this.pos.y, this.pos.a, ms), ms)); + timeouts.push(setTimeout(callback, ms)); + }.bind(this), 0, false, ms); + }; + + Asserv.prototype.simu_pwm = function(pwm, x, y, a, dt) { + return function() { + this.pos = { + x: x + Math.cos(a) * SIMU_DIST(pwm, dt/1000, this.vitesse), + y: y + Math.sin(a) * SIMU_DIST(pwm, dt/1000, this.vitesse), + a: a + } + this.sendPos(); + }.bind(this); + } + Asserv.prototype.pwm = function(left, right, ms, callback) { + this.newOrder(function() { + var pwm = (left+right)/2; + for(var t = 0; t < ms; t += 1000/FPS) { + timeouts.push(setTimeout(this.simu_pwm(pwm, this.pos.x, this.pos.y, this.pos.a, t), t)); + } + timeouts.push(setTimeout(this.simu_pwm(pwm, this.pos.x, this.pos.y, this.pos.a, ms), ms)); + timeouts.push(setTimeout(callback, ms)); + }.bind(this), 0, false, ms); + }; + + Asserv.prototype.simu_goxy = function(x, y) { + return function() { + this.pos.x = x; + this.pos.y = y; + this.sendPos(); + }.bind(this); + } + Asserv.prototype.goxy = function(x, y, sens, callback, no_fifo) { + + var dx = x-this.pos.x; + var dy = y-this.pos.y; + var dist = Math.sqrt(Math.pow(dx,2) + Math.pow(dy,2)); + var tf = (dist / (this.vitesse*SIMU_FACTOR_VIT))*1000; // *1000 s->ms + + angle_avant = convertA(Math.atan2(dy,dx)-this.pos.a); + angle_arriere = convertA(angle_avant+Math.PI); + + if(sens == "avant") angle_depart = angle_avant + else if(sens == "arriere") angle_depart = angle_arriere; + else if (Math.abs(angle_avant) < Math.abs(angle_arriere)) angle_depart = angle_avant; + else angle_depart = angle_arriere; + + // logger.debug("dx: ", dx); + // logger.debug("dy: ", dy); + // logger.debug("angle: ", this.pos.a); + // logger.debug("angle avant: ", angle_avant); + // logger.debug("angle arriere: ", angle_arriere); + // logger.debug("angle depart: ", angle_depart); + + this.goa(angle_depart+this.pos.a, function() { + this.newOrder(function() { + for(var t = 0; t < tf; t += 1000/FPS) { + timeouts.push(setTimeout(this.simu_goxy(this.pos.x+dx*t/tf, this.pos.y+dy*t/tf), t)); + } + timeouts.push(setTimeout(this.simu_goxy(x, y), tf)); + timeouts.push(setTimeout(callback, tf)); + }.bind(this), 0, no_fifo, tf); + }.bind(this), no_fifo); + }; + Asserv.prototype.simu_goa = function(a) { + return function() { + this.setA(a); + this.sendPos(); + }.bind(this); + } + Asserv.prototype.goa = function(a, callback, no_fifo){ + a = convertA(a); + da = convertA(a-this.pos.a); + // logger.debug("depart:", this.pos.a); + // logger.debug("arrivee:", a); + // logger.debug("delta:", da); + + var tf = SIMU_ROT_TIME(da, this.vitesse)*1000; // *1000 s->ms + this.newOrder(function() { + for(var t = 0; t < tf; t += 1000/FPS) { + // logger.debug(this.pos.a+da*t/tf); + timeouts.push(setTimeout(this.simu_goa(this.pos.a+da*t/tf), t)); + } + timeouts.push(setTimeout(this.simu_goa(a), tf)); + timeouts.push(setTimeout(callback, tf)); + }.bind(this), 0, no_fifo, tf); + }; + + Asserv.prototype.setPid = function(p, i, d, callback){ + this.newOrder(callback); + }; + + return Asserv; +})(); diff --git a/clients/shared/defineParser.js b/clients/shared/defineParser.js new file mode 100644 index 0000000..7989047 --- /dev/null +++ b/clients/shared/defineParser.js @@ -0,0 +1,45 @@ + + +/* Converts : + * #define VALUEA + * #define VALUEB 'b' + * #define VALUEC 42 + * In : + * { + * VALUEA : null, + * VALUEB : "b", + * VALUEC : 42 + * } + */ + + + +module.exports = (function(){ + var logger = require('log4js').getLogger('DefineParser'); + var fs = require('fs'); + + var reg = /#define[ \t]+(\S+)[ \t]+(\S+)/g; + + + return function parse(file){ + var parsed = {}; + fs.readFile(file, 'utf8', function (err,data) { + if(err) { + logger.fatal("cant read file:\""+file+"\""); + return ; + } + var nb = 0; + while( findings = reg.exec(data) ) { + try { + parsed[findings[1]] = eval(findings[2]); + } catch(e) {} + // try to evaluate calculus... + nb++; + } + logger.info("done parsing \""+file+"\" with "+nb+" defines"); + //should be quick enough + }); + return parsed; + } + +})(); \ No newline at end of file diff --git a/clients/shared/fifo.class.js b/clients/shared/fifo.class.js new file mode 100644 index 0000000..ba515e0 --- /dev/null +++ b/clients/shared/fifo.class.js @@ -0,0 +1,36 @@ +module.exports = (function () { + var logger = require('log4js').getLogger('Fifo'); + + function Fifo() { + this.clean(); + } + Fifo.prototype.clean = function(callback) { + this.fifo = []; + this.order_in_progress = false; + } + + Fifo.prototype.orderFinished = function() { + this.order_in_progress = false; + this.nextOrder(); + } + + Fifo.prototype.newOrder = function(callback, name) { + if (name === undefined) + name = ""; + this.fifo.push({callback: callback, name: name}); + this.nextOrder(); + } + + Fifo.prototype.nextOrder = function() { + if(!this.order_in_progress && this.fifo.length > 0) { + // logger.debug(this.fifo.length); + this.order_in_progress = true; + object = this.fifo.shift(); + // logger.debug("Calling : "+object.name); + object.callback(); + } + } + + + return Fifo; +})(); diff --git a/clients/shared/test.js b/clients/shared/test.js new file mode 100644 index 0000000..a74dff0 --- /dev/null +++ b/clients/shared/test.js @@ -0,0 +1,12 @@ + + + +var defineParser = require("./defineParser.js"); +console.log(defineParser); + +var commands = defineParser("./arduino/asserv/protocol.h"); + + +setTimeout(function(){ + console.log(commands); +}, 200); \ No newline at end of file diff --git a/config.js b/config.js new file mode 100644 index 0000000..4ee90f7 --- /dev/null +++ b/config.js @@ -0,0 +1,5 @@ +module.exports = { + //"server": "127.0.0.1:3128", + "server": "192.168.0.100:3128", + "hokuyo_command": "/root/coupe15/hokuyo/bin/hokuyo" +} diff --git a/hokuyo/Makefile b/hokuyo/Makefile new file mode 100644 index 0000000..4062828 --- /dev/null +++ b/hokuyo/Makefile @@ -0,0 +1,49 @@ +DEBUG=yes +SDL=no + +CC=gcc +SRC_FILES=main.c fast_math.c analyzer.c exceptions.c frame.c +SRC_DIR=src/ +EXEC=bin/hokuyo + +LFLAGS=-Wall `urg_c-config --libs` -lm +CFLAGS=-Wall `urg_c-config --cflags` + +ifeq ($(DEBUG),yes) + CFLAGS += -D DEBUG +else + CFLAGS += -Werror + LFLAGS += -Werror +endif + +ifeq ($(SDL),yes) + SRC_FILES+= sdl.c + LFLAGS+= `sdl-config --libs` + CFLAGS+= -D SDL `sdl-config --cflags` + EXEC=bin/hokuyo_sdl +endif + +SOURCES=$(SRC_FILES:%=$(SRC_DIR)%) +OBJS=$(SOURCES:src/%.c=build/%.o) + +all: $(EXEC) + +$(EXEC): $(OBJS) + $(CC) -o $@ $^ $(LFLAGS) + +build/%.o: src/%.c build + $(CC) -o $@ $(CFLAGS) -c $< + +build: + mkdir build + +sdl.o: sdl.h global.h +main.o: fast_math.h global.h analyzer.h +analyser.o: fast_math.h global.h analyzer.h +fast_math.o: fast_math.h global.h +clean: + rm -rf build +mrproper: clean + rm $(EXEC) + +.PHONY: clean diff --git a/hokuyo/README.md b/hokuyo/README.md new file mode 100644 index 0000000..775c078 --- /dev/null +++ b/hokuyo/README.md @@ -0,0 +1,2 @@ +hokuyo +====== diff --git a/hokuyo/bin/hokuyo_sdl b/hokuyo/bin/hokuyo_sdl new file mode 100644 index 0000000..d59ef86 Binary files /dev/null and b/hokuyo/bin/hokuyo_sdl differ diff --git a/hokuyo/client_hok.js b/hokuyo/client_hok.js new file mode 100644 index 0000000..bcb8627 --- /dev/null +++ b/hokuyo/client_hok.js @@ -0,0 +1,315 @@ +// TODO : +// message/objet erreur ou pas ? + +(function (){ + "use strict"; + + /* JS which will connect to the server, and then + * will execute the C program to control the Hokuyos. + * It will transfer datas from the C Hokuyo controller to AI + */ + + var log4js = require('log4js'); + var logger = log4js.getLogger('Client'); + var spawn = require('child_process').spawn; + var fs = require('fs'); + var match_name = ""; + var child_process = require('child_process'); + var child; + var SocketClient = require('../server/socket_client.class.js'); + var config = require('../config.js'); + var lastT = Date.now(); + var startingT = lastT; + + var FREQ_ENVOI_INFO = 50; // tous les 10 infos (genre 1 seconde) + var nth = 0; + + var server = process.argv[2] || config.server; + var command = process.argv[3] || config.hokuyo_command; + + var client = new SocketClient({ + server_ip: server, + type: "hokuyo", + }); + + var started = false; + var nb_active_hokuyos = -1; + var lastStatus = { + "status": "waiting" + }; + sendChildren(lastStatus); + + logger.info("Starting with pid " + process.pid); + + client.order(function(from, name, params){ + var now = Date.now(); + logger.info("Time since last order : "+(now - lastT)); + if (now - lastT > 500) { // half a second between two orders + logger.info("Just received an order `" + name + "` from " + from + " with params :"); + logger.info(params); + + lastT = now; + switch (name){ + case "start": + if(!!params.color && !started) { + started = true; + start(params.color); + } else + logger.error("ALready started or Missing parameters !"); + break; + case "shutdown": + quitC("stop"); + spawn('sudo', ['halt']); + break; + case "stop": + started = false; + quitC("stop"); + break; + case "sync_git": + spawn('/root/sync_git.sh', [], { + detached: true + }); + break; + default: + logger.warn("Name not understood : " + data); + } + } else { + logger.warn("Received two orders too closely !"); + } + }); + + function matchLogger(name, line){ + fs.appendFile('/var/log/utcoupe/'+name+'.log', line+'\n', function (err) { + if (err) logger.error('Ecriture dans le fichier de log de match "/var/log/utcoupe/'+name+'.log" impossible'); + // logger.debug('The "data to append" was appended to file!'); + }); + } + + function quitC(code){ + if(!!child){ + logger.info("Closing child "+child.pid+" at "+code); + child.kill('SIGINT'); + child = null; + } else { + logger.info("Can't close child at "+code+" : never born :P"); + logger.info("Father's pid : " + process.pid); + // process.kill(process.pid, 'SIGINT'); + } + } + + function uException(code){ + logger.error("uException sent with code "+code); + } + + function start(color){ + // We just an order to start, with the flavour :P (color, number of robots) + + sendChildren({"status": "starting"}); + + // Generates the match name (for the log file) + var tmp = new Date(); + match_name = tmp.toJSON().replace(/T/, ' ').replace(/\..+/, ''); + var now = Date.now() - lastT; + matchLogger(match_name, now+"; color:"+color); + now = lastT; + + // If there's a child, kill it + quitC("start"); + + // Exit handlers + //do something when app is closing + process.on('exit', quitC); + // catches ctrl+c event + // process.on('SIGINT', quitC); + //catches uncaught exceptions + //process.on('uncaughtException', uException); + + // Functions + function parseRobots(string) { + var dots = []; + now = Date.now() - lastT; + + if(!!string){ + var temp = string.split("#"); + for (var i = 0; i <= temp.length - 1; i++) { + temp[i] = temp[i].split(","); + dots.push({x: 0, y: 0}); + dots[i].x = parseInt(temp[i][0]); + dots[i].y = parseInt(temp[i][1]); + + // Log them : + matchLogger(match_name, now+"; dotx:"+dots[i].x+"; doty:"+dots[i].y); + } + logger.info('[J-HOK] Robots'); + logger.info(dots); + } else { + logger.info('[J-HOK] No robot detected !'); + } + + now = lastT; + + // Send all robots + client.send("ia", "hokuyo.position_tous_robots", {dots: dots}); + } + + function parseInfo(string) { + logger.info("Read info..."); + // logger.info(string); + + var prev_n_a_h = nb_active_hokuyos; + + now = Date.now() - lastT; + + switch (string.substring(0,1)){ + case "0": + // Send error : no Hokuyo working + // client.send("ia", "nb_hokuyo", {nb: 0}); + nb_active_hokuyos = 0; + break; + case "1": + // Send warning : one Hokuyo is missing + // client.send("ia", "nb_hokuyo", {nb: 1}); + nb_active_hokuyos = 1; + break; + case "2": + // Send message : Hokuyos are ok + // client.send("ia", "nb_hokuyo", {nb: 2}); + nb_active_hokuyos = 2; + break; + default: + logger.info("Error not understood : " + string); + return; + } + + if ((prev_n_a_h != nb_active_hokuyos) || (nth == FREQ_ENVOI_INFO)){ + logger.info("Info sent to server"); + sendChildren(getStatus()); + nth = 0; + } + + matchLogger(match_name, now+"; nb_hokuyo:"+nb_active_hokuyos); + now = lastT; + nth += 1; + } + + function dataFromCHandler(input) { + // input format (XXXX type and xxxx values) : "[XXXX]xxxxxxxxx" maybe many times, seperated with \n + + var inputAr = input.toString().split('\n'); + + for (var i = 0; i <= inputAr.length - 1; i++) { + if (!!inputAr[i]){ + switch (inputAr[i].substring(1,5)){ + case "HI:)": + // send "C started" to server + logger.info('C Hokuyo software says "Hi !" :)'); + sendChildren({"status": "starting"}); + break; + case "DATA": + logger.info('C Hokuyo software sends datas'); + parseRobots(inputAr[i].substring(6)); + break; + case "INFO": + logger.info('C Hokuyo software sends information :'+inputAr[i].substring(6)); + parseInfo(inputAr[i].substring(6)); + break; + case "WARN": + logger.warn('C Hokuyo software sends a warning :'+inputAr[i].substring(6)); + parseInfo(inputAr[i].substring(6)); + break; + default: + logger.info("Data "+ inputAr[i].substring(1,5) + " not understood at line " + i + " : " + inputAr[i]); + } + } + } + } + + + // Execute C program + // var command = "/home/pi/coupe15/hokuyo/bin/hokuyo"; + var args = [color]; + // var options = // default : { cwd: undefined, env: process.env}; + logger.info('Launching : ' + command + ' ' + args); + child = child_process.spawn(command, args); + + // Events + child.stdout.on('data', function(data) { + logger.debug(data.toString()); + dataFromCHandler(data); + }); + + child.on('error', function(data) { + logger.fatal('Erreur avec le process C : ' + data.toString()); + sendChildren({"status": "error", "children":[]}); + setTimeout(function(){ + sendChildren({"status": "waiting", "children":[]}); + }, 5000); + }); + + child.stderr.on('error', function(data) { + logger.fatal(data.toString()); + sendChildren({"status": "error", "children":[]}); + setTimeout(function(){ + sendChildren({"status": "waiting", "children":[]}); + }, 5000); + }); + + child.stderr.on('data', function(data) { + logger.error(data.toString()); + }); + + + child.on('close', function(code) { + started = false; + if (code == 0) + logger.info('Child closed correctly'); + else + logger.error('Child closed with code: ' + code); + + // Send message + if (code != -1) + sendChildren({"status": "waiting", "children":[]}); + }); + } + + function getStatus(){ + var data = { + "status": "", + "children": [] + }; + + switch (nb_active_hokuyos){ + case 0: + data.status = "error"; + break; + case 1: + data.status = "ok"; + data.children = ["Lonesome hokuyo"]; + break; + case 2: + data.status = "everythingIsAwesome"; + data.children = ["Hokuyo 1", "Hokuyo 2"]; + break; + } + + return data; + } + + + // Sends status to server + function sendChildren(status){ + lastStatus = status; + + client.send("server", "server.childrenUpdate", lastStatus); + client.send("ia", "hokuyo.nb_hokuyo", { nb: nb_active_hokuyos }); + } + + function isOk(){ + if(lastStatus.status != "waiting") + lastStatus = getStatus(); + + client.send("ia", "isOkAnswer", lastStatus); + client.send("server", "server.childrenUpdate", lastStatus); + client.send("ia", "hokuyo.nb_hokuyo", { nb: nb_active_hokuyos }); + } +})(); diff --git a/hokuyo/client_hok.simu.js b/hokuyo/client_hok.simu.js new file mode 100644 index 0000000..fe27e72 --- /dev/null +++ b/hokuyo/client_hok.simu.js @@ -0,0 +1,206 @@ +// TODO : +// message/objet erreur ou pas ? + +(function (){ + "use strict"; + + /* JS which will connect to the server, and then + * will execute the C program to control the Hokuyos. + * It will transfer datas from the C Hokuyo controller to AI + */ + + var log4js = require('log4js'); + var logger = log4js.getLogger('Client'); + var spawn = require('child_process').spawn; + var fs = require('fs'); + var match_name = ""; + var child_process = require('child_process'); + var child; + var SocketClient = require('../server/socket_client.class.js'); + var config = require('../config.js'); + var lastT = Date.now(); + var startingT = lastT; + + var FREQ_ENVOI_INFO = 50; // tous les 10 infos (genre 1 seconde) + var nth = 0; + + var server = process.argv[2] || config.server; + var command = process.argv[3] || config.hokuyo_command; + + var client = new SocketClient({ + server_ip: server, + type: "hokuyo", + }); + + var nb_active_hokuyos = -1; + var lastStatus = { + "status": "waiting" + }; + sendChildren(lastStatus); + + logger.info("Starting with pid " + process.pid); + + client.order(function(from, name, params){ + var now = Date.now(); + logger.info("Time since last order : "+(now - lastT)); + if (now - lastT > 500) { // half a second between two orders + logger.info("Just received an order `" + name + "` from " + from + " with params :"); + logger.info(params); + + lastT = now; + switch (name){ + case "start": + if(!!params.color) + start(params.color); + else + logger.error("Missing parameters !"); + break; + case "shutdown": + quitC("stop"); + spawn('sudo', ['halt']); + break; + case "stop": + quitC("stop"); + break; + case "sync_git": + spawn('/root/sync_git.sh', [], { + detached: true + }); + break; + default: + logger.warn("Name not understood : " + data); + } + } else { + logger.warn("Received two orders too closely !"); + } + }); + + function matchLogger(name, line){ + fs.appendFile('/var/log/utcoupe/'+name+'.log', line+'\n', function (err) { + if (err) logger.error('Ecriture dans le fichier de log de match impossible'); + // logger.debug('The "data to append" was appended to file!'); + }); + } + + function uException(code){ + logger.error("uException sent with code "+code); + } + + function start(color){ + // We just an order to start, with the flavour :P (color, number of robots) + + sendChildren({"status": "starting"}); + + // Generates the match name (for the log file) + var tmp = new Date(); + match_name = tmp.toJSON().replace(/T/, ' ').replace(/\..+/, ''); + var now = Date.now() - lastT; + var x1 = 1500, y1 = 400; + var x2 = 1500, y2 = 600; + var x1_inc= -20, y1_inc = 50; + var x2_inc= -25, y2_inc = 35; + matchLogger(match_name, now+"; color:"+color); + now = lastT; + + // Functions + function parseRobots(string) { + var dots = []; + now = Date.now() - lastT; + + x1 += x1_inc; + y1 += y1_inc; + + if (x1 < 1000 || x1 > 1800) + x1_inc = -x1_inc; + if (y1 < 200 || y1 > 800) + y1_inc = -y1_inc; + + x2 += x2_inc; + y2 += y2_inc; + + if (x2 < 900 || x2 > 1800) + x2_inc = -x2_inc; + if (y2 < 600 || y2 > 1200) + y2_inc = -y2_inc; + + //logger.debug("x:"+x1+" y:"+y1+" x:"+x1+" y:"+y1); + + dots.push({x: x1, y: y1}); + dots.push({x: x2, y: y2}); + now = lastT; + + // Send all robots + client.send("ia", "hokuyo.position_tous_robots", {dots: dots}); + } + + function parseInfo(string) { + logger.info("Read info..."); + // logger.info(string); + + var prev_n_a_h = nb_active_hokuyos; + + nb_active_hokuyos = 2; + + now = Date.now() - lastT; + + if ((prev_n_a_h != nb_active_hokuyos) || (nth == FREQ_ENVOI_INFO)){ + logger.info("Info sent to server"); + sendChildren(getStatus()); + nth = 0; + } + + matchLogger(match_name, now+"; nb_hokuyo:"+nb_active_hokuyos); + now = lastT; + nth += 1; + } + + function dataFromCHandler(input) { + parseRobots(); + setTimeout(dataFromCHandler, 100); // call every 100ms + } + + sendChildren({"status": "starting"}); + dataFromCHandler(); + } + + function getStatus(){ + var data = { + "status": "", + "children": [] + }; + + switch (nb_active_hokuyos){ + case 0: + data.status = "error"; + break; + case 1: + data.status = "ok"; + data.children = ["Lonesome hokuyo"]; + break; + case 2: + data.status = "everythingIsAwesome"; + data.children = ["Hokuyo 1", "Hokuyo 2"]; + break; + } + + return data; + } + + + // Sends status to server + function sendChildren(status){ + lastStatus = status; + + client.send("server", "server.childrenUpdate", lastStatus); + client.send("ia", "hokuyo.nb_hokuyo", { nb: nb_active_hokuyos }); + } + + function isOk(){ + if(lastStatus.status != "waiting") + lastStatus = getStatus(); + + client.send("ia", "isOkAnswer", lastStatus); + client.send("server", "server.childrenUpdate", lastStatus); + client.send("ia", "hokuyo.nb_hokuyo", { nb: nb_active_hokuyos }); + } +})(); diff --git a/hokuyo/config.js b/hokuyo/config.js new file mode 100644 index 0000000..8d594c5 --- /dev/null +++ b/hokuyo/config.js @@ -0,0 +1,5 @@ +module.exports = { + server: "192.168.0.100:3128", // server adress + port + //server: "127.0.0.1:3128", + command: "/root/coupe15/hokuyo/bin/hokuyo" // C sw place +}; diff --git a/hokuyo/orders.json b/hokuyo/orders.json new file mode 100644 index 0000000..ed23840 --- /dev/null +++ b/hokuyo/orders.json @@ -0,0 +1,66 @@ +These are the different orders exchanged between the Hokuyos and the AI. + +Please make sure take a look at the "plan.svg" file (part Communication), +it's a summary of this part of the structure : + - the Raspberry Pi is linked to the server/AI wirelessly + - it hosts 2 programs : + ¤ client_hok.js : always running nodeJS script, which starts the + other when it is needed. + ¤ hokuyo : (compilation of the src files), connects to the Hokuyos, + recieves the spots and sends the computed robots to the nodeJS. + +"order"s between the AI and the nodeJS : + + { + "to": "IA", + "name": "position_tous_robots", + "params": {"dots": [dots]} + } + + { + "to": "IA", + "name": "nb_hokuyo", + "params": {"nb": 0/1/2} + } + + { + "to": "hokuyo", + "name": "start", + "params": { + "color": "yellow"/"green", + "nbrobots": 2/3/4 + } + } + + { + "to": "hokuyo", + "name": "stop", + "params": {} + } + + { + "to": ???, + "name": "C_started", + "params": {} + } + + { + "to": ???, + "name": "C_closed", + "params": {} + } + + +Datas (string lines on stdout) from the C to the nodeJS : + + "[HI:)] Hello !\n" + Ok C started and able to communicate + + "[DATA]12,345#233,544#1223,43#234,0\n" + Robots x,y separed with # + + "[INFO]%c\n" + %c is the character of the info code : + "2" : everything is ok ! + "1" : one hokuyo missing + "0" : two hokuyos missing (Error : blind) \ No newline at end of file diff --git a/hokuyo/socket_client.class.js b/hokuyo/socket_client.class.js new file mode 100644 index 0000000..e0c940d --- /dev/null +++ b/hokuyo/socket_client.class.js @@ -0,0 +1 @@ +server/socket_client.class.js \ No newline at end of file diff --git a/hokuyo/src/analyzer.c b/hokuyo/src/analyzer.c new file mode 100644 index 0000000..3f0093d --- /dev/null +++ b/hokuyo/src/analyzer.c @@ -0,0 +1,170 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 17/11/2013 * +*******************************/ +#include +#include +#include +#include +#include +#include + +#include "analyzer.h" +#include "fast_math.h" +#include "global.h" +#include "exceptions.h" + +int init(urg_t *urg){ + int error; + int i; + char *device = "/dev/ttyACM0"; + + //SERIAL PORT DETECTION + fprintf(stderr, "List of serial ports :\n"); + int found_port_size = urg_serial_find_port(); + if (found_port_size == 0) { + fprintf(stderr, "could not found serial ports.\n"); + exit(EXIT_FAILURE); + } + for (i = 0; i < found_port_size; ++i) { + fprintf(stderr, "%s", (char *)urg_serial_port_name(i)); + device = (char *)urg_serial_port_name(i); + } + fprintf(stderr, "\n"); + + fprintf(stderr, "Connection à %s\n", device); + error = urg_open(urg, URG_SERIAL, device, BAUDRATE); + if(error < 0){ + error_func(urg, "connection failed"); + } + else{ + fprintf(stderr, "Connection établie à %s\n", device); + urg_set_scanning_parameter(urg, urg_rad2step(urg, ANGLE_MIN), urg_rad2step(urg, ANGLE_MAX), 0);//scan en continu, on ne garde que les point entre -PI/2 et PI/2 + fprintf(stderr, "Parameters set\n"); + error = urg_start_measurement(urg, URG_DISTANCE, URG_SCAN_INFINITY, 0); + if(error < 0){ + error_func(urg, "failed to start measurement"); + } + } + get_val(calc, 0, urg);//calcule les tables de cos/sin à l'avance + return error; +} + +int get_points_2d(struct urg_params urg, struct coord *points){ + int data_max, n, i; + long *data; + + data_max = urg_max_data_size(urg.ptr);//aquisition du nombre de points + data = (long*)malloc(sizeof(long) * data_max); + if(data == NULL){ + fprintf(stderr, "data malloc error\n"); + exit(1); + } + if(points == NULL){ + fprintf(stderr, "get_points_2d : points non alloué\n"); + exit(1); + } + + n = urg_get_distance(urg.ptr, data, NULL); + + // CONVERSION EN COORDONNEES + for(i=0; i LX - MIN_DIST_TO_EDGE || p.x < MIN_DIST_TO_EDGE) + || (p.y > LY - MIN_DIST_TO_EDGE || p.y < MIN_DIST_TO_EDGE) + // ADD OTHER EXCEPTIONS HERE // + )return 1; + else return 0; +} + +int group_exception(struct points_group g){ + //LISTE D'EXEPTIONS + if( (g.size < POINTS_MIN) //si le groupe ne contiens pas au moins POINTS_MIN alors il n'est pas valide + // ADD OTHER EXCEPTIONS HERE // + ) return 1; + else return 0; +} diff --git a/hokuyo/src/exceptions.h b/hokuyo/src/exceptions.h new file mode 100644 index 0000000..811a2be --- /dev/null +++ b/hokuyo/src/exceptions.h @@ -0,0 +1,9 @@ +#ifndef EXCEPTIONS_H +#define EXCEPTIONS_H + +#include "analyzer.h" + +int ignore(struct coord p);//renvoie 1 si le point est à ignorer, 0 sinon +int group_exception(struct points_group p); //renvoit 1 si le groupe est invalide, 0 sinon + +#endif diff --git a/hokuyo/src/fast_math.c b/hokuyo/src/fast_math.c new file mode 100644 index 0000000..3ea8169 --- /dev/null +++ b/hokuyo/src/fast_math.c @@ -0,0 +1,42 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 18/11/2013 * +*******************************/ +#include +#include +#include + + +#include "fast_math.h" +#include "global.h" + +double get_val(enum action ask, int index, urg_t *urg){ + static double cos_table[DATA_MAX]; + static double sin_table[DATA_MAX]; + if(ask == calc){ + int i,n; + n=urg_get_distance(urg, NULL, NULL); + for(i=0;i<=n;i++){ + cos_table[i] = cos(urg_index2rad(urg, i)+HOKUYO_A); + sin_table[i] = sin(urg_index2rad(urg, i)+HOKUYO_A); + } + } + else if(ask == cosinus) + return cos_table[index]; + else if(ask == sinus) + return sin_table[index]; + return -1; +} + +int dist_to_edge(struct coord p){ + int x_to_edge = min(p.x, LX - p.x); + int y_to_edge = min(p.y, LY - p.y); + int res = min(x_to_edge, y_to_edge); + return res; +} + +int dist(struct coord p1, struct coord p2){ + int res = abs(p1.x - p2.x) + abs(p1.y - p2.y); //distance = abs( ( x1 - x2 ) + ( y1 - y2) ) + return res; +} diff --git a/hokuyo/src/fast_math.h b/hokuyo/src/fast_math.h new file mode 100644 index 0000000..cb10572 --- /dev/null +++ b/hokuyo/src/fast_math.h @@ -0,0 +1,29 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 18/11/2013 * +*******************************/ + +#ifndef FAST_MATH_H +#define FAST_MATH_H + +#include + +#define max(a,b) (a>=b?a:b) +#define min(a,b) (a<=b?a:b) + +#define PI 3.14159265358979323846264338327950288 + +struct coord{ + int x; + int y; +}; + +enum action{sinus, cosinus, calc}; + +double get_val(enum action ask, int index, urg_t *urg); + +int dist_to_edge(struct coord p); +int dist(struct coord p1, struct coord p2); + +#endif diff --git a/hokuyo/src/frame.c b/hokuyo/src/frame.c new file mode 100644 index 0000000..b5d8e7e --- /dev/null +++ b/hokuyo/src/frame.c @@ -0,0 +1,26 @@ +#include "frame.h" +#include "global.h" +#include "fast_math.h" +#include "analyzer.h" + +#include + +void frame(struct urg_params hokuyo){ + struct coord data[DATA_MAX], robots_pos[DETECTABLE_ROBOTS]; + int i, n_data, n_robots; + + //Recuperation des poins + n_data = get_points_2d(hokuyo, data); + //Recuperation des robots + n_robots = get_robots_2d(robots_pos, data, n_data); + + printf("[DATA]"); + for (i=0; i0) { + printf("#"); + } + printf("%d,%d", robots_pos[i].x, robots_pos[i].y); + } + printf("\n"); + fflush(stdout); +} diff --git a/hokuyo/src/frame.h b/hokuyo/src/frame.h new file mode 100644 index 0000000..ce0832c --- /dev/null +++ b/hokuyo/src/frame.h @@ -0,0 +1,10 @@ +#ifndef FRAME_H +#define FRAME_H + +#include "global.h" +#include "fast_math.h" +#include "analyzer.h" + +void frame(struct urg_params hokuyo); + +#endif diff --git a/hokuyo/src/global.h b/hokuyo/src/global.h new file mode 100644 index 0000000..83824b9 --- /dev/null +++ b/hokuyo/src/global.h @@ -0,0 +1,42 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 17/11/2013 * +*******************************/ + +#ifndef GLOBAL_H +#define GLOBAL_H + +#define LX 3000.0 //longueur de la table en mm +#define LY 2000.0 //largeur de la table en mm + +#define X_WINDOW_RESOLUTION 1050 +#define Y_WINDOW_RESOLUTION 700 + +//distance (mm) minimale entre un robot et le bord de la carte pour que +//le robot soit accepté. Cela permet d'éviter de détercter des personnes +//se promenant trop près du bord. +#define MIN_DIST_TO_EDGE 30 +#define DIST_DIFF_GROUP 100 //taille en mm entre eux robots +#define POINTS_MIN 2 + +#define DETECTABLE_ROBOTS 100 + +// postion de l'hokuyo par rapport au coin inférieur droit +// le coin est dépendant du side, c'est à dire que la valeur +// n'est pas dépendante du side +#define HOKUYO_X 3040 +#define HOKUYO_Y 1000 +#define HOKUYO_A PI +#define ANGLE_MIN -PI/2 +#define ANGLE_MAX PI/2 + + +//********** +// PRIVATE * +//********** + +#define DATA_MAX 700 +#define MAX_GROUPS 350 //résolution/2 + +#endif diff --git a/hokuyo/src/main.c b/hokuyo/src/main.c new file mode 100644 index 0000000..0196f50 --- /dev/null +++ b/hokuyo/src/main.c @@ -0,0 +1,63 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 30/09/2013 * +*******************************/ + +#include +#include +#include + +#include "global.h" +#include "fast_math.h" +#include "analyzer.h" +#include "frame.h" + +#ifdef SDL +#include "sdl.h" +#endif + +int main(int argc, char **argv){ + //INITIALISATION + urg_t urg; + struct urg_params hokuyo; + + hokuyo.ptr = &urg; + hokuyo.x = HOKUYO_X; + hokuyo.y = HOKUYO_Y; + + int error = 1; + + while (error) { + error = init(hokuyo.ptr); + } + + if(error < 0){ + fprintf(stderr, "Erreur de connection\n"); + exit(EXIT_FAILURE); + } + + //On initialise la structure hokuyo avec ses paramètres de side et de position + if(argc >= 2){ + if(strcmp(argv[1], "green") == 0) + hokuyo.side = GREEN_SIDE; + else if(strcmp(argv[1], "yellow") == 0) + hokuyo.side = YELLOW_SIDE; + } + else{ + fprintf(stderr, "Préciser un side (blue/red)\n"); + exit(EXIT_FAILURE); + } + + fprintf(stderr, "Hokuyo initialized\n"); + +#ifdef SDL + affichage_sdl(hokuyo); +#else + printf("[HI:)] Hello !"); + while(1) { + frame(hokuyo); + } +#endif + return 0; +} diff --git a/hokuyo/src/sdl.c b/hokuyo/src/sdl.c new file mode 100644 index 0000000..8dd9a58 --- /dev/null +++ b/hokuyo/src/sdl.c @@ -0,0 +1,86 @@ +/******************************* +* Quentin CHATEAU pour UTCOUPE * +* quentin.chateau@gmail.com * +* Last edition : 30/09/2013 * +*******************************/ +#ifdef SDL + +#include +#include "sdl.h" +#include "global.h" +#include "fast_math.h" +#include "analyzer.h" +#include + +void affichage_sdl(struct urg_params hokuyo){ + struct coord data[DATA_MAX], robots_pos[DETECTABLE_ROBOTS]; + int i, n_data, n_robots, continuer = 1; + + //INITIALISATION DES ELEMENTS SDL + + //Initialisation de la fenetre + SDL_Surface *ecran = NULL, *sdl_points, *sdl_robots, *sdl_hokuyo; + SDL_Rect sdl_points_position[DATA_MAX], sdl_robots_position[DETECTABLE_ROBOTS], sdl_hokuyo_pos; + SDL_Init(SDL_INIT_VIDEO); + SDL_Event event; + ecran = SDL_SetVideoMode(X_WINDOW_RESOLUTION, Y_WINDOW_RESOLUTION, 32, SDL_HWSURFACE); + if(ecran == NULL){ + fprintf(stderr, "Erreur chargement SDL"); + exit(EXIT_FAILURE); + } + SDL_WM_SetCaption("Hokuyo", NULL); + SDL_FillRect(ecran, NULL, SDL_MapRGB(ecran->format, 200, 200, 200)); + + //Creation des points et des robots + sdl_points = SDL_CreateRGBSurface(SDL_HWSURFACE, 2, 2, 32, 0, 0, 0, 0); + SDL_FillRect(sdl_points, NULL, SDL_MapRGB(ecran->format, 255, 0, 0)); + sdl_robots = SDL_CreateRGBSurface(SDL_HWSURFACE, 30, 30, 32, 0, 0, 0, 0); + SDL_FillRect(sdl_robots, NULL, SDL_MapRGB(ecran->format, 0, 255, 0)); + sdl_hokuyo = SDL_CreateRGBSurface(SDL_HWSURFACE, 20, 20, 32, 0, 0, 0, 0); + SDL_FillRect(sdl_hokuyo, NULL, SDL_MapRGB(ecran->format, 50, 50, 255)); + + printf("SDL Initialized\n"); + + //RECUPERATION ET AFFICHAGE DES POINTS + while(continuer){ + SDL_PollEvent(&event); // On doit utiliser PollEvent car il ne faut pas attendre d'évènement de l'utilisateur pour mettre à jour la fenêtre + switch(event.type) + { + case SDL_QUIT: + continuer = 0; + break; + } + //Recuperation des poins + n_data = get_points_2d(hokuyo, data); + printf("got %dpts\n", n_data); + SDL_FillRect(ecran, NULL, SDL_MapRGB(ecran->format, 200, 200, 200));//on clean l'ecran + //Recuperation des robots + n_robots = get_robots_2d(robots_pos, data, n_data); + printf("got %drob\n", n_robots); + for(i=0;i + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + haut de la table public telles que sont les constantes dans le Github, on est vert(on fait la symetry si on est jaune changer selon notre couleur) + + + + x y + + Hok1x=-25, y=-25, angle = 0 + cone_min = 0 cone_max = pi/2 + cone_min = -pi/2 cone_max = pi/2 Hok2x=3025, y=1000, angle = pi PLAN TABLE HOKUYOS c'est bizarre, on dirait que c'est inversé... (le 0 est parallèle au public en green/reden yellow (inversé), le 0 est aussi // au public + + Ordres Hokuyo + + + + pendant match avant match après match + + start C started + + stop C stoped + position_tous_robots + nb_hokuyo + [DATA] + [WARN] + [HI:)] + + NodeJS C + diff --git a/hokuyo_bak/src/communication.c b/hokuyo_bak/src/communication.c new file mode 100644 index 0000000..7141dbc --- /dev/null +++ b/hokuyo_bak/src/communication.c @@ -0,0 +1,38 @@ +#include "communication.h" +#include "fast_math.h" +#include "global.h" + +#include +#include +#include +#include +#include + +extern FILE* logfile; + +void sayHello(){ + printf("[HI:)] Hello !\n"); + fflush(stdin); +} + +void pushResults(Cluster_t *coords, int nbr, long timestamp) { + // timestamp not used !! XXX + + char message[50]; + + strcpy(message, "[DATA]"); + + for(int i=0; i +#include + +// More information about communication in ../orders.json + +void sayHello(); +void pushResults(Cluster_t *coords, int nbr, long timestamp); +void pushInfo(char info); + +#endif diff --git a/hokuyo_bak/src/compat.c b/hokuyo_bak/src/compat.c new file mode 100644 index 0000000..4ac25dd --- /dev/null +++ b/hokuyo_bak/src/compat.c @@ -0,0 +1,78 @@ +#include +#include +#include // UNIX standard function definitions +#include // File control definitions +#include // Error number definitions +#include +#include + +#include "compat.h" + +long timeMillis() { + struct timeval tv; + if(gettimeofday(&tv, NULL) != 0) return 0; + return (unsigned long)((tv.tv_sec * 1000ul) + (tv.tv_usec / 1000ul)); +} + +char serial_read(int serial) { + char data; + int nbr = 0; + do { + nbr = read (serial, &data, 1); + } while (nbr <= 0); + data &= 0xFF; + return data; +} + +int nonblocking_read(int serial, char *data) { + set_blocking(serial, 0); + int n = read (serial, data, 1); + set_blocking(serial, 1); + return n; +} + +int set_interface_attribs (int fd, int speed, int parity) { + struct termios tty; + memset (&tty, 0, sizeof tty); + if (tcgetattr (fd, &tty) != 0) + { + return -1; + } + + cfsetospeed (&tty, speed); + cfsetispeed (&tty, speed); + + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars + tty.c_iflag &= ~(IGNBRK | INLCR | IGNCR | ICRNL); // ignore break signal + tty.c_lflag = 0; // no signaling chars, no echo, no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 0; // read doesn't block + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl + + tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls, enable reading + tty.c_cflag &= ~(PARENB); // shut off parity + tty.c_cflag |= parity; + tty.c_cflag &= ~CSTOPB; + + if (tcsetattr (fd, TCSANOW, &tty) != 0) + { + return -1; + } + return 0; +} + +void set_blocking (int fd, int should_block) { + struct termios tty; + memset (&tty, 0, sizeof tty); + if (tcgetattr (fd, &tty) != 0) + { + return; + } + + tty.c_cc[VMIN] = should_block ? 1 : 0; + tty.c_cc[VTIME] = 5; // 5 seconds read timeout + + if (tcsetattr (fd, TCSANOW, &tty) != 0); +} diff --git a/hokuyo_bak/src/compat.h b/hokuyo_bak/src/compat.h new file mode 100644 index 0000000..52a5f73 --- /dev/null +++ b/hokuyo_bak/src/compat.h @@ -0,0 +1,16 @@ +#ifndef COMPAT_H +#define COMPAT_H + +typedef enum bool +{ + true = 1, false = 0 +} bool; + +long timeMillis(); + +char serial_read(int serial); +int nonblocking_read(int serial, char *data); +int set_interface_attribs (int fd, int speed, int parity); +void set_blocking (int fd, int should_block); + +#endif diff --git a/hokuyo_bak/src/fast_math.c b/hokuyo_bak/src/fast_math.c new file mode 100644 index 0000000..0d2079c --- /dev/null +++ b/hokuyo_bak/src/fast_math.c @@ -0,0 +1,88 @@ +#include +#include +#include + +#include "fast_math.h" +#include "hokuyo_config.h" + +extern FILE* logfile; + + +static inline int max(int a,int b){ return (a>b) ? a : b ; } +static inline int min(int a,int b){ return (a initFastmath...\n", PREFIX); + exit(EXIT_FAILURE); + } + + //fprintf(logfile, "Should print anles from -90 to 0\n"); + for(int i=0; i= M_PI) { + a -= 2*M_PI; + } + return a; +} diff --git a/hokuyo_bak/src/fast_math.h b/hokuyo_bak/src/fast_math.h new file mode 100644 index 0000000..aabff67 --- /dev/null +++ b/hokuyo_bak/src/fast_math.h @@ -0,0 +1,29 @@ +#ifndef FAST_MATH_H +#define FAST_MATH_H + +#include "global.h" + +typedef struct Pt { + int x, y; +} Pt_t; + + +struct fastmathTrigo { + int n; + double *cos, *sin; +}; + + +int dist_squared(Pt_t p1, Pt_t p2); +int dist_to_edge(Pt_t p, int largeurX, int largeurY); + + +struct fastmathTrigo initFastmath(int n, double *angles, double headingError); +void freeFastmath(struct fastmathTrigo s); + +double angle(Pt_t p1, Pt_t p2); +double fastCos(struct fastmathTrigo f, int index); +double fastSin(struct fastmathTrigo f, int index); +double modTwoPi(double a); + +#endif diff --git a/hokuyo_bak/src/global.h b/hokuyo_bak/src/global.h new file mode 100644 index 0000000..d5c6e2e --- /dev/null +++ b/hokuyo_bak/src/global.h @@ -0,0 +1,56 @@ +#ifndef GLOBAL_H +#define GLOBAL_H + +#include "fast_math.h" +#include +#include + +#define DEBUG 0 + +#define PREFIX "[C-HK] " +// #define SDL + +#define TABLE_X 3000 +#define TABLE_Y 2000 +#define MAX_DATA 1024 +#define MAX_CLUSTERS 50 // nombre maxi de clusters +#define MAX_ROBOTS 4 // au cas où qu'on précise pas à l'appel du programme +#define BORDER_MARGIN 50 +#define TIMEOUT 1000 + + +#define CONE_DIAM_MAX 690 // à modifier en fonction du cône utilisé +#define CONE_CALIB CONE_DIAM_MAX/2 // diamètre du cône à l'horizontale de l'hokuyo <=> ø vu par l'hokuyo lorsque l'assiette est bonne +#define CONE_HEIGHT 350// hauteur du cône +#define CONE_X_LEFT 967 +#define CONE_X_RIGHT 2033 +#define CONE_Y 1420 +#define CALIB_X (TABLE_X/2) +#define CALIB_Y (TABLE_Y/2) + +#define HOK1_SERIAL "1102605" +#define HOK1_X -40 // old : 25 +#define HOK1_Y -40 // old : 25 +#define HOK1_A 0 // orientation +#define HOK1_CONE_MIN 0 +#define HOK1_CONE_MAX (M_PI/2) + +#define HOK2_SERIAL "1320252" +#define HOK2_X 3040 // old 3025 +#define HOK2_Y 1000 +#define HOK2_A M_PI +#define HOK2_CONE_MIN (-M_PI/2) +#define HOK2_CONE_MAX (M_PI/2) + +// CLUSTERS : +#define CLUSTER_POINTS_BACKWARDS 15 +#define MAX_DIST 100 // entre deux points pour les considérer dans le même cluster, old: 200 +#define MAX_SIZE_TO_MERGE 200 +#define NB_PTS_MIN 3 // pour qu'un cluster soit gardé + +// Position zone escalier +#define STAIRS_Y_MIN 1420 +#define STAIRS_X_MIN 967 +#define STAIRS_X_MAX 2033 + +#endif diff --git a/hokuyo_bak/src/gui.c b/hokuyo_bak/src/gui.c new file mode 100644 index 0000000..b43eb5e --- /dev/null +++ b/hokuyo_bak/src/gui.c @@ -0,0 +1,137 @@ +#include "gui.h" +#include "stdio.h" +#include "fast_math.h" +#include "utils.h" +#include + +struct GUI_data{ + SDL_Surface *ecran, *terrain, *lidar, *robot, *point, *marker; + SDL_Rect posTerrain, posMarker; +}; +static struct GUI_data gui; + +//Do NOT modify +#define GUI_WINDOW_REAL_SIZE_X (TABLE_X+2*BORDER_PADDING) +#define GUI_WINDOW_REAL_SIZE_Y (TABLE_Y+2*BORDER_PADDING) + +#define GUI_TERRAIN_SIZE_X GUI_WINDOW_RESOLUTION_X*TABLE_X / GUI_WINDOW_REAL_SIZE_X +#define GUI_TERRAIN_SIZE_Y GUI_WINDOW_RESOLUTION_Y*TABLE_Y / GUI_WINDOW_REAL_SIZE_Y + +#define GUI_MARKER_SIZE_X GUI_WINDOW_RESOLUTION_X*MARKER_SIZE / GUI_WINDOW_REAL_SIZE_X +#define GUI_MARKER_SIZE_Y GUI_WINDOW_RESOLUTION_Y*MARKER_SIZE / GUI_WINDOW_REAL_SIZE_Y + +#define GUI_LIDAR_SIZE_X GUI_WINDOW_RESOLUTION_X*LIDAR_SIZE / GUI_WINDOW_REAL_SIZE_X +#define GUI_LIDAR_SIZE_Y GUI_WINDOW_RESOLUTION_Y*LIDAR_SIZE / GUI_WINDOW_REAL_SIZE_Y + +SDL_Rect +getPixelCoord(int x, int y){ + SDL_Rect p; + p.x = (BORDER_PADDING+(float)x) * GUI_WINDOW_RESOLUTION_X / GUI_WINDOW_REAL_SIZE_X; + p.y = GUI_WINDOW_RESOLUTION_Y - ((BORDER_PADDING+(float)y) * GUI_WINDOW_RESOLUTION_Y / GUI_WINDOW_REAL_SIZE_Y); + return p; +} + +SDL_Rect +getPixelCoordWithSize(int x, int y, int sx, int sy){ + SDL_Rect p; + p.x = (BORDER_PADDING+(float)(x)) * GUI_WINDOW_RESOLUTION_X / GUI_WINDOW_REAL_SIZE_X; + p.y = GUI_WINDOW_RESOLUTION_Y - ((BORDER_PADDING+(float)(y+sy)) * GUI_WINDOW_RESOLUTION_Y / GUI_WINDOW_REAL_SIZE_Y); + return p; +} + + +struct color newColor(int r, int g, int b){ + struct color c; + c.r = r; + c.g = g; + c.b = b; + return c; +} + +void +initSDL(Pt_t positionLidar){ + + gui.ecran = SDL_SetVideoMode(GUI_WINDOW_RESOLUTION_X, GUI_WINDOW_RESOLUTION_Y, 32, SDL_HWSURFACE); + if(gui.ecran == NULL) exit(EXIT_FAILURE); + + gui.terrain = SDL_CreateRGBSurface(SDL_HWSURFACE, GUI_TERRAIN_SIZE_X, GUI_TERRAIN_SIZE_Y, 32, 0, 0, 0, 0); + SDL_FillRect(gui.terrain, NULL, SDL_MapRGB(gui.ecran->format, 200, 255, 200)); + gui.posTerrain = getPixelCoordWithSize(0, 0, TABLE_X, TABLE_Y); + + gui.robot = SDL_CreateRGBSurface(SDL_HWSURFACE, GUI_ROBOT_SIZE, GUI_ROBOT_SIZE, 32, 0, 0, 0, 0); + SDL_FillRect(gui.robot, NULL, SDL_MapRGB(gui.ecran->format, 0, 0, 0)); + + gui.marker = SDL_CreateRGBSurface(SDL_HWSURFACE, GUI_MARKER_SIZE_X, GUI_MARKER_SIZE_Y, 32, 0, 0, 0, 0); + SDL_FillRect(gui.marker, NULL, SDL_MapRGB(gui.ecran->format, 0, 255, 0)); + gui.posMarker = getPixelCoordWithSize(CALIB_X-MARKER_SIZE/2, CALIB_Y-MARKER_SIZE/2, MARKER_SIZE, MARKER_SIZE); + + gui.lidar = SDL_CreateRGBSurface(SDL_HWSURFACE, GUI_LIDAR_SIZE_X, GUI_LIDAR_SIZE_Y, 32, 0, 0, 0, 0); + gui.point = SDL_CreateRGBSurface(SDL_HWSURFACE, GUI_POINT_SIZE, GUI_POINT_SIZE, 32, 0, 0, 0, 0); + + +} + +void +blitMap(Pt_t positionLidar){ + SDL_FillRect(gui.ecran, NULL, SDL_MapRGB(gui.ecran->format, 255, 255, 255)); + SDL_BlitSurface(gui.terrain, NULL, gui.ecran, &(gui.posTerrain)); + SDL_BlitSurface(gui.marker, NULL, gui.ecran, &(gui.posMarker)); +} + +void +blitLidar(Pt_t positionLidar, struct color c){ + SDL_Rect p = getPixelCoord(positionLidar.x-LIDAR_SIZE/2, positionLidar.y+LIDAR_SIZE/2); + SDL_FillRect(gui.lidar, NULL, SDL_MapRGB(gui.ecran->format, c.r, c.g, c.b)); + SDL_BlitSurface(gui.lidar, NULL, gui.ecran, &p); +} + +void +blitRobots(Cluster_t *robots, int nRobots, struct color c){ + SDL_FillRect(gui.robot, NULL, SDL_MapRGB(gui.ecran->format, c.r, c.g, c.b)); + int i, maxsize = 0; + for (i=0; i maxsize) { + maxsize = robots[i].size; + } + } + for(i=0; iformat, c.r, c.g, c.b)); + for(int i=0; i +#include +#include +#include +#include // UNIX standard function definitions +#include // File control definitions +#include // Error number definitions +#include + +extern FILE* logfile; + +char expected_serial[2][20] = { + HOK1_SERIAL, + HOK2_SERIAL, +}; + +#define NR_ACM_TRY 4 +#define VVCOMMAND "VV\n" + +int detectHokuyos(char (*paths)[SERIAL_STR_LEN], int nr) { + char base_path[] = "/dev/ttyACMx"; + int i; + for (i=0; iisWorking) { + fprintf(logfile, "%sHokuyo not connected, trying to connect to %s\n", PREFIX, hok->path); + int error = urg_connect(hok->urg, hok->path, 115200); + if (error < 0) { + fprintf(logfile, "%sCan't connect to hokuyo : %s\n", PREFIX, urg_error(hok->urg)); + hok->isWorking = 0; + } else { + hok->imin = urg_rad2index(hok->urg, hok->cone_min); + hok->imax = urg_rad2index(hok->urg, hok->cone_max); + + urg_setCaptureTimes(hok->urg, UrgInfinityTimes); + error = urg_requestData(hok->urg, URG_MD, hok->imin, hok->imax); + if (error < 0) { + fprintf(logfile, "%sCan't connect to hokuyo\n", PREFIX); + hok->isWorking = 0; + } else { + fprintf(logfile, "%sHokuyo connected\n", PREFIX); + hok->isWorking = 1; + fprintf(logfile, "%sRequesting data on indexes %d to %d from %s OK\n", PREFIX, hok->imin, hok->imax, hok->path); + + hok->nb_data = urg_dataMax(hok->urg); + double *angles = malloc(hok->nb_data * sizeof(double)); + int i; + for (i=0; inb_data; i++) { + angles[i] = modTwoPi(urg_index2rad(hok->urg, i) + hok->orientation); + } + hok->fm = initFastmath(hok->nb_data, angles, hok->error); + free(angles); + + fprintf(logfile, "%sCalculted sin/cos data for %s\n", PREFIX, hok->path); + } + } + } +} + +Hok_t applySymetry(Hok_t hok) { + hok.pt = (Pt_t) {TABLE_X - hok.pt.x, hok.pt.y}; + hok.orientation = M_PI - hok.orientation; + double temp = hok.cone_min; + hok.cone_min = -hok.cone_max; + hok.cone_max = -temp; + return hok; +} + +// Angles_t frameWizard (Hok_t *hok, int hok_pos, int symetry){ +// Pt_t pts[MAX_DATA], coneCenter; +// Angles_t results; +// Cluster_t clusters[MAX_CLUSTERS]; +// Cluster_simple_t cone; +// int nPts = 0, nClusters = 0, distToCone; + +// if (hok->isWorking) { +// // Valeurs de coneCenter et de la zone de l'hokuyo +// coneCenter.y = CONE_Y; +// if (((hok_pos == 4) && !symetry) || ((hok_pos == 3) && symetry)){ // si on est sur la gauche de la table (vu du public) +// hok->zone = (ScanZone_t){ BORDER_MARGIN, TABLE_X/2, BORDER_MARGIN, TABLE_Y-BORDER_MARGIN }; +// coneCenter.x = CONE_X_LEFT; +// } else { // si on est sur la droite +// hok->zone = (ScanZone_t){ TABLE_X/2, TABLE_X-BORDER_MARGIN, BORDER_MARGIN, TABLE_Y-BORDER_MARGIN }; +// coneCenter.x = CONE_X_RIGHT; +// } + +// // Valeur de la distance entre l'Hokuyo et le cône +// if (hok_pos == 3){ // si on est sur un côté +// distToCone = sqrt(dist_squared((Pt_t) {HOK2_X, HOK2_Y}, (Pt_t) {CONE_X_RIGHT, CONE_Y})); +// } else { // si on est dans un coin +// distToCone = sqrt(dist_squared((Pt_t) {HOK1_X, HOK1_Y}, (Pt_t) {CONE_X_LEFT, CONE_Y})); +// } + +// if (hok->isWorking) { +// nPts = getPoints(*hok, pts); +// if (nPts == -1) { +// hok->isWorking = 0; +// results.pitch = -1; +// results.heading = -1; +// return results; +// } +// } else { +// results.pitch = -1; +// results.heading = -1; +// return results; // Hokuyo disconnected +// } + +// nClusters = getClustersFromPts(pts, nPts, clusters); + +// cone = findCone(nClusters, clusters, coneCenter); + +// if ((cone.center.x ==-1) && (cone.center.y ==-1)) +// { +// results.pitch = -2; +// results.heading = -2; +// return results; // cone lost +// } + +// // Calcul des erreurs (normalement ce sont des tan-1 mais on les néglige vu l'angle) +// // --- pitch +// results.pitch = ((CONE_CALIB * CONE_HEIGHT/2)/((float)cone.size) - CONE_HEIGHT/2)/(distToCone); +// // --- heading +// results.heading = (sqrt(dist_squared(cone.center, coneCenter)))/(distToCone); + +// return results; +// } else { +// results.pitch = -1; +// results.heading = -1; +// return results; // Hokuyo disconnected +// } +// } + + +// void initWizard (Hok_t *hok1, Hok_t *hok2, int symetry){ +// Angles_t errors; + +// fprintf(logfile, "%sLaunching initWizard...\n", PREFIX); + +// // Début de l'initialisation du premier Hokuyo +// fprintf(logfile, "%sPut the 1st hokuyo on the platform which is in front of the public,\n", PREFIX); +// if (symetry == 0) // On est vert +// fprintf(logfile, "%son the left-hand side.\n", PREFIX); +// else +// fprintf(logfile, "%son the right-hand side.\n", PREFIX); +// fprintf(logfile, "%sIt must look to the other side.\n", PREFIX); +// fprintf(logfile, "%sOnce done, please plug it into the Rasp (press any key to continue)\n", PREFIX); +// getchar(); // en attendant un ENTER +// // boucle de vérification qu'il est branché +// while (!hok1->isWorking) { +// checkAndConnect(hok1); +// sleep(1); +// } +// fprintf(logfile, "%sOk, first hokuyo detected !\n", PREFIX); + +// fprintf(logfile, "%sPut the mark on the nearest corner of the stairs. (press any key to continue)\n", PREFIX); +// getchar(); // en attendant un ENTER +// // boucle de vérification de l'assiette +// do{ +// if (hok1->isWorking){ +// errors = frameWizard (hok1, 4, symetry); + +// fprintf(logfile, "%f %f\n", errors.pitch, errors.heading); + +// if ((errors.pitch == -1.) && (errors.heading == -1.)){ +// fprintf(logfile, "%sHokuyo disconnected\n", PREFIX); +// continue; +// } else { +// if ((errors.pitch == -2.) && (errors.heading == -2.)) +// fprintf(logfile, "%sCan't see the cone\n", PREFIX); +// } +// } else { +// checkAndConnect(hok1); +// } +// sleep(1); +// } while (pow(errors.pitch, 2) > 0.0125); // trigo, en estimant que nos cylindres font 10cm de haut à max 2m de distance (0.025²) 0.0125 +// // une fois l'erreur suffisement petite +// // prendre l'erreur ce cap +// hok1->error = errors.heading; +// fprintf(logfile, "%sHeading error is %f grad = %f°\n", PREFIX, hok1->error, (hok1->error*180)/M_PI); +// fprintf(logfile, "%sPitch error is %f grad = %f°\n", PREFIX, errors.pitch, (errors.pitch*180)/M_PI); +// fprintf(logfile, "%sOk, let's say that's good\n", PREFIX); +// fprintf(logfile, "%sThis hokuyo has been correctly configured (press any key to continue)\n", PREFIX); +// getchar(); + + +// // Début de l'initialisation du second Hokuyo +// fprintf(logfile, "%sPut the 2nd hokuyo on the platform which on the side of the area,\n", PREFIX); +// if (symetry == 0) // On est vert +// fprintf(logfile, "%son the right-hand side.\n", PREFIX); +// else +// fprintf(logfile, "%son the left-hand side.\n", PREFIX); +// fprintf(logfile, "%sIt must look to the other side.\n", PREFIX); +// fprintf(logfile, "%sOnce done, please plug it into the Rasp (press any key to continue)\n", PREFIX); +// getchar(); // en attendant un ENTER +// // boucle de vérification qu'il est branché +// while (!hok2->isWorking) { +// checkAndConnect(hok2); +// sleep(1); +// } +// fprintf(logfile, "%sOk, 2nd hokuyo detected !\n", PREFIX); + +// fprintf(logfile, "%sPut the mark on the nearest corner of the stairs. (press any key to continue)\n", PREFIX); +// getchar(); // en attendant un ENTER +// // boucle de vérification de l'assiette +// do{ +// if (hok2->isWorking){ +// errors = frameWizard (hok2, 3, symetry); + +// fprintf(logfile, "%f %f\n", errors.pitch, errors.heading); + +// if ((errors.pitch == -1.) && (errors.heading == -1.)){ +// fprintf(logfile, "%sHokuyo disconnected\n", PREFIX); +// continue; +// } else { +// if ((errors.pitch == -2.) && (errors.heading == -2.)) +// fprintf(logfile, "%sCan't see the cone\n", PREFIX); +// } +// } else { +// checkAndConnect(hok2); +// } +// sleep(1); +// } while (pow(errors.pitch, 2) > 0.0125); // trigo, en estimant que nos cylindres font 10cm de haut à max 2m de distance (0.025²) 0.0125 +// // une fois l'erreur suffisement petite +// // prendre l'erreur ce cap +// hok2->error = errors.heading; +// fprintf(logfile, "%sHeading error is %f grad = %f°\n", PREFIX, hok2->error, (hok2->error*180)/M_PI); +// fprintf(logfile, "%sPitch error is %f grad = %f°\n", PREFIX, errors.pitch, (errors.pitch*180)/M_PI); +// fprintf(logfile, "%sOk, let's say that's good\n", PREFIX); +// fprintf(logfile, "%sThis hokuyo has been correctly configured (press any key to continue)\n", PREFIX); +// getchar(); + +// fprintf(logfile, "%sWaiting for the match to start...\n", PREFIX); +// } diff --git a/hokuyo_bak/src/hokuyo_config.h b/hokuyo_bak/src/hokuyo_config.h new file mode 100644 index 0000000..9df5702 --- /dev/null +++ b/hokuyo_bak/src/hokuyo_config.h @@ -0,0 +1,39 @@ +#ifndef HOKUYO_CONFIG_H +#define HOKUYO_CONFIG_H + +#include "fast_math.h" +#include + +#define SERIAL_STR_LEN 20 + +typedef struct ScanZone { + int xmin, xmax, ymin, ymax; +} ScanZone_t; + +typedef struct Angles { + double pitch; // tangage + double heading; // cap² +} Angles_t; + +typedef struct Hokuyo { + urg_t* urg; + Pt_t pt; + ScanZone_t zone; + double orientation, cone_min, cone_max; //Scanne dans ori-cone;ori+cone + int imin, imax, nb_data, isWorking; + char *path; + double error; // non utilisé coupe14, utilisé pour l'erreur de cap maintenant + struct fastmathTrigo fm; // enregistre les cos et sin pour les n angles +} Hok_t; + +int detectHokuyos(char (*paths)[SERIAL_STR_LEN], int nr); +Hok_t initHokuyo(char *path, double ori, double cone_min, double cone_max, Pt_t pt); +// void initWizard(Hok_t *hok1, Hok_t *hok2, int symetry); +void checkAndConnect(Hok_t *hok); +Hok_t applySymetry(Hok_t hok); + +// IN : struct hokuyo, position (3 = côté ( -| ), 4 = coin ( _| )) +// OUT : erreur d'assiette/tangage th (angle en rad), erreur de cap +// Angles_t frameWizard(Hok_t *hok, int hok_pos, int symetry); + +#endif diff --git a/hokuyo_bak/src/main.c b/hokuyo_bak/src/main.c new file mode 100644 index 0000000..dbcdd5f --- /dev/null +++ b/hokuyo_bak/src/main.c @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include + +#include "hokuyo_config.h" +#include "utils.h" +#include "compat.h" +#include "communication.h" + +#ifdef SDL +#include "gui.h" +#endif + +// void frame(int nb_robots_to_find); +void frame(); + +static int symetry = 0; +static long timeStart = 0; +static Hok_t hok1, hok2; +FILE* logfile; + +void exit_handler() { + fprintf(logfile, "\n%sClosing lidar(s), please wait...\n", PREFIX); + if (hok1.urg != 0) + urg_disconnect(hok1.urg); + if (hok2.urg != 0) + urg_disconnect(hok2.urg); + + // XXX on ne free rien ? genre nos hok et tout ? + + fflush(logfile); + if (logfile != NULL){ + fprintf(logfile, "\n%sClosing log file and exiting, please wait...\n", PREFIX); + fclose(logfile); + } + // kill(getppid(), SIGUSR1); //Erreur envoyee au pere +} + +static void catch_SIGINT(int signal){ + exit(EXIT_FAILURE); +} + +int main(int argc, char **argv){ + // Disable buffering on stdout + setvbuf(stdout, NULL, _IOLBF, 0); + + // int nb_robots_to_find = 4; + hok1.urg = 0; + hok2.urg = 0; + + // Open log file + logfile = fopen("/var/log/hokuyo.log", "a+"); + if (logfile == NULL) { + fprintf(stderr, "Can't open log file (what do you think about beeing a sudoer ? :P )\n"); + exit(EXIT_FAILURE); + } + fprintf(logfile, "\n\n===== Starting Hokuyo =====\n"); + sayHello(); + + atexit(exit_handler); // en cas de signal de fermeture, on déconnecte proprement + + if(argc <= 1 || ( strcmp(argv[1], "green") != 0 && strcmp(argv[1], "yellow") ) ){ + // fprintf(stderr, "usage: hokuyo {green|yellow} [nbr_robots]\n"); + fprintf(stderr, "usage: hokuyo {green|yellow}\n"); + exit(EXIT_FAILURE); + } + + if (signal(SIGINT, catch_SIGINT) == SIG_ERR) { + fprintf(stderr, "An error occurred while setting a signal handler for SIGINT.\n"); + exit(EXIT_FAILURE); + } + + // if (argc >= 3) { + // nb_robots_to_find = atoi(argv[2]); + // } else { + // nb_robots_to_find = MAX_ROBOTS; + // } + + char paths[2][SERIAL_STR_LEN]; + + if (detectHokuyos(paths, 2)) { + fprintf(stderr, "Failed to detect hokuyos paths\n"); + exit(EXIT_FAILURE); + } + fprintf(logfile, "Hokuyo 1 (corner) detected on port %s\n", paths[0]); + fprintf(logfile, "Hokuyo 2 (ennemy side) detected on port %s\n", paths[1]); + fflush(logfile); + + hok1 = initHokuyo(paths[0], HOK1_A, HOK1_CONE_MIN, HOK1_CONE_MAX, (Pt_t){HOK1_X, HOK1_Y} ); + hok2 = initHokuyo(paths[1], HOK2_A, HOK2_CONE_MIN, HOK2_CONE_MAX, (Pt_t){HOK2_X, HOK2_Y} ); + + if (strcmp(argv[1], "yellow") == 0) { + // si on est jaune, on inverse les positions ! (au lieu de changer les cst) + symetry = 1; + hok1 = applySymetry(hok1); + hok2 = applySymetry(hok2); + } + + // if (strcmp(argv[2], "use_init_wizard") == 0){ + // initWizard(&hok1, &hok2, symetry); + // } + + checkAndConnect(&hok1); + checkAndConnect(&hok2); + + #ifdef SDL + initSDL(); + #endif + + fprintf(logfile, "%sStarting hokuyo :\n%sLooking for %s robots\n", PREFIX, PREFIX, argv[1]); + fflush(stdout); + timeStart = timeMillis(); + long time_last_try = 0; + while(1){ + long now = timeMillis(); + if (now - time_last_try > TIMEOUT) { + fprintf(logfile, "%sChecking hokuyos\n", PREFIX); + checkAndConnect(&hok1); + checkAndConnect(&hok2); + + // If an Hokuyo isn't connected, we scan the ports and try to reconect + if (!hok1.isWorking) { + if (detectHokuyos(paths, 2)) { + fprintf(stderr, "Failed to detect hokuyos paths (looking for Hokuyo 1)\n"); + } else { + strcpy(hok1.path, paths[0]); + + checkAndConnect(&hok1); + if (!hok1.isWorking) { + // pushInfo('1'); + fprintf(logfile, "%sHokuyo 1 not working\n", PREFIX); + } + } + } + if (!hok2.isWorking) { + if (detectHokuyos(paths, 2)) { + fprintf(stderr, "Failed to detect hokuyos paths (looking for Hokuyo 2)\n"); + } else { + strcpy(hok1.path, paths[1]); + checkAndConnect(&hok2); + if (!hok2.isWorking) { + // pushInfo('1'); + fprintf(logfile, "%sHokuyo 2 not working\n", PREFIX); + } + } + } + time_last_try = now; + } + frame(); + fflush(logfile); + } + exit(EXIT_SUCCESS); +} + + +void frame(){ + long timestamp; + //static long lastTime = 0; + Pt_t pts1[MAX_DATA], pts2[MAX_DATA]; + Cluster_t robots1[MAX_CLUSTERS], robots2[MAX_CLUSTERS], robots[MAX_ROBOTS]; + int nPts1 = 0, nPts2 = 0, nRobots1 = 0, nRobots2 = 0, nRobots; + + if (hok1.isWorking && hok2.isWorking) { + pushInfo('2'); + hok1.zone = (ScanZone_t){ BORDER_MARGIN, TABLE_X/2, BORDER_MARGIN, TABLE_Y-BORDER_MARGIN }; // l'hok1 se charge de la partie gauche (vu du public) + hok2.zone = (ScanZone_t){ TABLE_X/2, TABLE_X-BORDER_MARGIN, BORDER_MARGIN, TABLE_Y-BORDER_MARGIN }; // l'hok2 se charge de la partie droite + if (symetry) { + ScanZone_t temp = hok1.zone; + hok1.zone = hok2.zone; + hok2.zone = temp; + } + } else if (hok1.isWorking || hok2.isWorking) { // si ya qu'un des deux hok à marcher, le seul survivant scanne toute la table + pushInfo('1'); + hok1.zone = hok2.zone = (ScanZone_t){ BORDER_MARGIN, TABLE_X - BORDER_MARGIN, BORDER_MARGIN, TABLE_Y-BORDER_MARGIN }; + } + + if (hok1.isWorking || hok2.isWorking) { + //long start_t = timeMillis(); + + if (hok1.isWorking) { + nPts1 = getPoints(hok1, pts1); + if (nPts1 == -1) { + hok1.isWorking = 0; + } + } + if (hok2.isWorking) { + nPts2 = getPoints(hok2, pts2); + if (nPts2 == -1) { + hok2.isWorking = 0; + } + } + + + timestamp = timeMillis() - timeStart; + //fprintf(logfile, "%sDuration : %ld\n", PREFIX,timeMillis() - start_t); + //fprintf(logfile, "%sGot %d and %d points\n", PREFIX, nPts1, nPts2); + + nRobots1 = getClustersFromPts(pts1, nPts1, robots1); + nRobots2 = getClustersFromPts(pts2, nPts2, robots2); + + fprintf(logfile, "%sCalculated %d and %d clusters\n", PREFIX, nRobots1, nRobots2); + + // nRobots1 = sortAndSelectRobots(nRobots1, robots1, nb_robots_to_find); + // nRobots2 = sortAndSelectRobots(nRobots2, robots2, nb_robots_to_find); + + nRobots = mergeRobots(robots1, nRobots1, robots2, nRobots2, robots); + //fprintf(logfile, "%sGot %d robots\n", PREFIX, nRobots); + + #ifdef SDL + struct color l1Color = {255, 0, 0}, l2Color = {0, 0, 255}, lColor = {255, 0, 255}; + blitMap(); + blitLidar(hok1.pt, l1Color); + blitLidar(hok2.pt, l2Color); + blitRobots(robots1, nRobots1, l1Color); + blitRobots(robots2, nRobots2, l2Color); + blitRobots(robots, nRobots, lColor); + blitPoints(pts1, nPts1, l1Color); + blitPoints(pts2, nPts2, l2Color); + waitScreen(); + #endif + + pushResults(robots, nRobots, timestamp); + } else { + pushInfo('0'); + sleep(1); + } + //lastTime = timestamp; +} + diff --git a/hokuyo_bak/src/utils.c b/hokuyo_bak/src/utils.c new file mode 100644 index 0000000..58a48e4 --- /dev/null +++ b/hokuyo_bak/src/utils.c @@ -0,0 +1,352 @@ +#include "utils.h" +#include "fast_math.h" +#include +#include +#include +#include +// #include + +#define max(a, b) a>b?a:b +#define min(a, b) a zone.xmax || pt.x < zone.xmin || pt.y > zone.ymax || pt.y < zone.ymin); +} + +int getPoints(Hok_t hok, Pt_t* pt_list) { + long data[MAX_DATA]; + int n = urg_receiveData(hok.urg, data, MAX_DATA); + if (n > 0) { + int i, j=0; + for (i=hok.imin; i 0) { + int *clusters_index = malloc(nb_pts*sizeof(int)); + clusters_index[0] = 0; // numéro de cluster pour chaque point scanné + for (i=0; i MAX_CLUSTERS) { + fprintf(logfile, "%sToo many clusters\n", PREFIX); + } + } + } + + /* + Ça compte le nombre de points par clusters, ça met dans clusters[i].pts[] la liste des points associés à ce cluster + et ça vire les points foirés (rangés au cluster -1) + */ + for (i=0; i= 0) { + clusters[index].pts[clusters[index].nb++] = pt_list[i]; + } + } + + /* + On supprime les clusters de taille inférieure à NB_PTS_MIN + */ + i = 0; + while (i < nbCluster) { + if (clusters[i].nb < NB_PTS_MIN) { + for (j=i+1; j 0) { + clusters[i].center = (Pt_t) { sumx/clusters[i].nb, sumy/clusters[i].nb }; + } + } + } + return nbCluster; +} + +int sortAndSelectRobots(int n, Cluster_t *robots, int nb_robots_to_find){ + int i, nbr_robots = min(n, nb_robots_to_find); + Cluster_t *r = malloc(n*sizeof(Cluster_t)); + memcpy(r, robots, n*sizeof(Cluster_t));// on copie nos clusters, pour pouvoir mettre les tailles des clusters rangés à 0 + for(i=0; i source d'oublis de robots !! (en cas de main d'arbitre par ex) + int maxSize = 0, maxId = 0; + for(int j=0; j maxSize){ + maxSize = r[j].size; + maxId = j; + } + } + robots[i].center = r[maxId].center; // on range le ième plus grand à la ième place ! + robots[i].size = r[maxId].size; + r[maxId].size = 0; + } + return i; +} + +int mergeRobots(Cluster_t *r1, int n1, Cluster_t *r2, int n2, Cluster_t *result) { + Cluster_t all_bots[2*MAX_ROBOTS]; + int i, n_tot=0; + + // On copie les robots, n_tot va être égal à n1+n2 + for (i=0; i nb_robots_to_find) { + //Brute force ici, on aura max 4 robots de chaque hokuyo, 4x4 = 16 cas, ca reste peu + + //Calcul de la distance entre chaque combinaison de coords + int distR1R2[MAX_ROBOTS][MAX_ROBOTS], i, j; + struct corres dist_indexes[MAX_ROBOTS*MAX_ROBOTS]; //Tableau d'index de distR1R2, sert a trier les distances + //struct corres merged[MAX_ROBOTS]; //Correspondances finales et retenues + int changed = 0; + + //Calcul des distaces + for (i=0; iSTAIRS_Y_MIN) && (p1.x > STAIRS_X_MIN) && (p1.x < STAIRS_X_MAX); + int p2_zone = (p2.y>STAIRS_Y_MIN) && (p2.x > STAIRS_X_MIN) && (p2.x < STAIRS_X_MAX); + // s'ils sont dans la même zone + return p1_zone == p2_zone; +}; + + +/* + +int mergeRobots(Cluster_t *r1, int n1, Cluster_t *r2, int n2, Cluster_t *result, int nb_robots_to_find) { + struct corres { //permet d'établir des correspondances entre les index + int r1; + int r2; + }; + //Brute force ici, on aura max 4 robots de chaque hokuyo, 4x4 = 16 cas, ca reste peu + //Calcul de la distance entre chaque combinaison de coords + int distR1R2[MAX_ROBOTS][MAX_ROBOTS], i, j; + struct corres dist_indexes[MAX_ROBOTS*MAX_ROBOTS]; //Tableau d'index de distR1R2, sert a trier les distances + struct corres merged[MAX_ROBOTS]; //Correspondances finales et retenues + int changed = 0; + + //Calcul des distaces + for (i=0; i distR1R2[dist_indexes[i].r1][dist_indexes[i].r2]) { + struct corres temp = dist_indexes[i]; + dist_indexes[i] = dist_indexes[i-1]; + dist_indexes[i-1] = temp; + changed = 1; + } + } + } while (changed); + + //Choix des correspondances en prenant la premiere qui vientdu plus petit au plus grand + int used_R1_index[MAX_ROBOTS], used_R2_index[MAX_ROBOTS], nbr_found = 0; + for (i=0; i MAX_SIZE_TO_MERGE*MAX_SIZE_TO_MERGE) { + break; + } + + if (!isIn(c.r1, used_R1_index, n1) && //Aucun des deux robots n'est deja selectionné + !isIn(c.r2, used_R2_index, n2)) { //On ajout la correspondace + + merged[nbr_found] = c; + used_R1_index[nbr_found] = c.r1; + used_R2_index[nbr_found] = c.r2; + nbr_found++; + } + } + + for (i=0; i=0; i--) { + struct corres c = dist_indexes[i]; + + if (!isIn(c.r1, used_R1_index, nbr_found)) { + result[nbr_found] = r1[c.r1]; + used_R1_index[nbr_found] = c.r1; + nbr_found++; + } + + if (!isIn(c.r2, used_R2_index, nbr_found)) { + result[nbr_found] = r2[c.r2]; + used_R2_index[nbr_found] = c.r2; + nbr_found++; + } + } + + nbr_found = sortAndSelectRobots(nbr_found, result, nb_robots_to_find); + return nbr_found; +} + + +int isIn(int e, int *tab, int tab_size) { + int i, ret = 0; + for (i=0; i= n1 et >=n2) + OUT: retourne le nb de robot trouvés final + result (leur position) +*/ +// int mergeRobots(Cluster_t *r1, int n1, Cluster_t *r2, int n2, Cluster_t *result, int nb_robots_to_find); +int mergeRobots(Cluster_t *r1, int n1, Cluster_t *r2, int n2, Cluster_t *result); + + +/* Dit si 2 points sont dans la même zone (escalier / pas escalier) + IN: 2 points + OUT: boolean (true = 1, false = 0) +*/ +int sameZone(Pt_t p1, Pt_t p2); + +/* + IN: + OUT: +*/ +// int isIn(int e, int *tab, int tab_size); + +#endif diff --git a/hokuyo_bak/urg-0.8.18.zip b/hokuyo_bak/urg-0.8.18.zip new file mode 100644 index 0000000..51ea87a Binary files /dev/null and b/hokuyo_bak/urg-0.8.18.zip differ diff --git a/hokuyo_bak/urg_INSTALLATION b/hokuyo_bak/urg_INSTALLATION new file mode 100644 index 0000000..578b494 --- /dev/null +++ b/hokuyo_bak/urg_INSTALLATION @@ -0,0 +1,20 @@ +Ne PAS utiliser la librairie "urg-library-1.0.4", elle ne prend pas en charge plusieurs Hokuyo +(cf: https://github.com/utcoupe/coupe15/commit/5440d893299106c750ed76e205b14ad46b645b3d#commitcomment-7904503) + +Utiliser plutôt la librairie modifée dans le github. Elle nécessite au moins la librairie SDL. +(libsdl1.2 et libsdl1.2-dev sous Ubuntu) + +Se placer dans le répertoire urg-0.8.18, exécutez les commandes suivantes : +./configure && make +sudo make install +(EDIT : si le make plante au sample convert_2d.c, enlever "c_urg/" de la ligne 12 de ce fichier) + +Code de test pour savoir si la librairie est installée +Regarder dans le dossier /usr/local/c_urg/ pour les header +en langage C, dans /usr/local/urg/ pour les headers en C++ +(EDIT : tiens, chez moi c'est dans /usr/local/include/ ...) + +#include +void main() { } + +Il faut savoir que pour compiler des codes autres que samples de la librairies (qui sont bien pour tester si ça marche), il faut faire un fichier CMakeLists.txt (http://florian-goujeon.developpez.com/cours/cmake/initiation/) \ No newline at end of file diff --git a/ia/README.md b/ia/README.md new file mode 100644 index 0000000..b75f982 --- /dev/null +++ b/ia/README.md @@ -0,0 +1,5 @@ +IA des robots +======= + +Structure interne de l'IA : +![alt tag](https://raw.githubusercontent.com/utcoupe/coupe15/master/ia/architecture_ia_utcoupe_2015.jpg) \ No newline at end of file diff --git a/ia/actions.class.js b/ia/actions.class.js new file mode 100644 index 0000000..a7d91b8 --- /dev/null +++ b/ia/actions.class.js @@ -0,0 +1,289 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.actions'); + function Actions(ia) { + this.ia = ia; + this.color = ia.color; + + this.done = {}; + this.todo = {}; + this.inprogress = null; + this.killed = {}; + + this.valid_id_do_action = -1; + + this.todo = this.importActions(ia.data); + } + + var __dist_startpoints_plot = 120; + var __nb_startpoints_plot = 16; + function convertA(a) { return Math.atan2(Math.sin(a), Math.cos(a)); } + + Actions.prototype.collision = function() { + if(this.inprogress !== null) { + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // Décommenter la ligne, juste pour tester + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + this.todo[this.inprogress.name] = this.inprogress; + this.inprogress = null; + } + } + + Actions.prototype.importActions = function (data) { + var req; + + try { + req = require('./actions.json'); + } + catch(err) { + logger.fatal("Erreur lors de l'importation des actions dans l'IA: "+err); + } + var actions = req.actions; + + // Link "object" with exiting thing in the Data class + Object.keys(actions).forEach(function(i) { + actions[i].object = data.getObjectRef(actions[i].objectname); + actions[i].name = i; + + if ((actions[i].object !== null) && (actions[i].type == "plot") && (actions[i].startpoints.length === 0)) { + actions[i].startpoints.push({ + x: actions[i].object.pos.x, + y: actions[i].object.pos.y + }); + // var temp; + // for(var j = 0; j < __nb_startpoints_plot; j++) { + // temp = j*2*Math.PI/__nb_startpoints_plot; + // actions[i].startpoints.push({ + // x: actions[i].object.pos.x + __dist_startpoints_plot * Math.cos(temp), + // y: actions[i].object.pos.y + __dist_startpoints_plot * Math.sin(temp), + // a: convertA(temp+Math.PI) + // }); + // } + } + else if((actions[i].object !== null) && (actions[i].type == "clap")) { + if(this.color != "yellow") { + var a = actions[i].startpoints[0].a; + actions[i].startpoints[0].a = (a < 0) ? -Math.PI - a : Math.PI - a; + } + } + }.bind(this)); + + return actions; + }; + + Actions.prototype.parseOrder = function (from, name, params) { + switch(name) { + case 'actions.action_finished': + // logger.debug('received action_finished'); + this.actionFinished(); + break; + case 'actions.path_finished': + logger.debug('received path_finished'); + this.ia.pr.path = []; + break; + default: + logger.warn('Ordre inconnu dans ia.gr: '+name); + } + }; + + + Actions.prototype.kill = function (action_name){ + // If action doesn't exist + if (!!action_name && this.exists(action_name)){ + this.done[action_name] = this.todo[action_name]; + delete this.todo[action_name]; + } + }; + + Actions.prototype.exists = function (action_name){ + if (!this.todo[action_name]){ + if (!this.killed[action_name] && !this.done[action_name] && !this.done[action_name]) + logger.warn("Action named '"+"' doesn't exist"); + else + logger.warn("Action named '"+"' already killed in progress or done !"); + return false; + } else { + return true; + } + }; + + Actions.prototype.isDone = function (action_name){ + return !action_name || this.done.hasOwnProperty(action_name); + }; + + function norm2Points(A, B) { + return Math.sqrt(Math.pow(A.x-B.x, 2) + Math.pow(A.y-B.y, 2)); + } + Actions.prototype.getNormAction = function(pos, an) { + return norm2Points(pos, this.todo[an].object.pos); + }; + + Actions.prototype.getPriorityAction = function(an) { + return this.todo[an].object.status == "lost" ? -1000 : this.todo[an].priority; + }; + + Actions.prototype.isDoable = function(action) { + // Verifies some things about the action + + if (!!action.dependency && !this.isDone(action.dependency)){ + // Depends on an action, but it hasn't already been done + return false; + } + + if (action.dependencyRobotContent !== undefined){ + // Depends on the robot content + + if ((action.dependencyRobotContent.gobelet !== undefined) && + (this.ia.pr.content.gobelet !== action.dependencyRobotContent.gobelet)){ + // The cup holder position isn't consistent with needed state + return false; + } + + // If there's a constraint about the current number of cylinders + if ((action.dependencyRobotContent.invPlot !== undefined) && + (this.ia.pr.content.nb_plots < action.dependencyRobotContent.invPlot)){ + return false; + } + if ((action.dependencyRobotContent.subPlot !== undefined) && + (this.ia.pr.content.nb_plots > action.dependencyRobotContent.subPlot)){ + return false; + } + + } + + // if (action.object.status == "lost"){ + // return false; + // } + + return true; + }; + + Actions.prototype.doNextAction = function(callback) { + this.valid_id_do_action++; + var actions_todo = []; + + // Get les actions possibles + Object.getOwnPropertyNames(this.todo).forEach(function(an) { //an = action name + if(this.isDoable(this.todo[an])) { + actions_todo.push(an); + } + }, this); + + // Tri par priorité puis par norme + var pos = this.ia.pr.pos; + actions_todo.sort(function(a, b) { + return (this.getPriorityAction(b) - this.getPriorityAction(a)) || (this.getNormAction(pos, a) - this.getNormAction(pos, b)); + }.bind(this)); + + for(var i in actions_todo) { + logger.debug('[%d] %s (%d)', this.todo[actions_todo[i]].priority, actions_todo[i], this.getNormAction(pos, actions_todo[i])); + } + + // Va choisir l'action la plus proche, demander le path et faire doAction + this.pathDoAction(callback, actions_todo, this.valid_id_do_action); + }; + + Actions.prototype.getNearestStartpoint = function(pos, startpoints) { + var min_dist = Infinity; + var nearest = null; + + for (var i = 0; i < startpoints.length; i++) { + var dist = norm2Points(pos, startpoints[i]); + + if (dist < min_dist){ + min_dist = dist; + nearest = startpoints[i]; + } + } + + return nearest; + }; + + Actions.prototype.pathDoAction = function(callback, actions, id) { + if(id != this.valid_id_do_action) { + logger.Debug('id different'); + return; + } + // Va choisir l'action la plus proche, demander le path et faire doAction + if(actions.length > 0) { + var action = this.todo[actions.shift()]; + var startpoint = this.getNearestStartpoint(this.ia.pr.pos, action.startpoints); + this.ia.pathfinding.getPath(this.ia.pr.pos, startpoint, function(path) { + if(path !== null) { + this.ia.pr.path = path; + this.doAction(callback, action, startpoint, id); + } else { + logger.debug("path not found"); + // Si le pathfinding foire, on fait la deuxième action la plus importante + this.pathDoAction(callback, actions, id); + } + }.bind(this)); + } else { + logger.debug("all paths not found"); + setTimeout(function() { + this.doNextAction(callback); + }.bind(this), 500); + } + }; + + Actions.prototype.doAction = function (callback, action, startpoint, id) { + if(id != this.valid_id_do_action) + return; + this.callback = callback; + + // // Change action to state "in progress" + this.inprogress = action; + delete this.todo[action.name]; + + logger.debug('Action en cours %s (%d;%d;%d)', action.name, startpoint.x, startpoint.y, startpoint.a); + this.ia.pr.path.map(function(checkpoint) { + this.ia.client.send('pr', "goxy", { + x: checkpoint.x, + y: checkpoint.y, + sens: action.sens + }); + }, this); + if(!!startpoint.a) { + this.ia.client.send('pr', "goa", { + a: startpoint.a + }); + } + + this.ia.client.send('pr', "send_message", { + name: "actions.path_finished" + }); + // 1 order for 1 action + // action.orders.forEach(function (order, index, array){ + this.ia.client.send('pr', action.orders[0].name, action.orders[0].params); + // }.bind(this)); + this.ia.client.send('pr', "send_message", { + name: "actions.action_finished" + }); + + // // Set object to "done" ! XXX + + // // Change action and its "to be killed" actions to state done + + // console.log(this.todo); + // console.log(this.inprogress); + // console.log(this.done); + }; + + Actions.prototype.actionFinished = function () { + if(this.inprogress !== null) { + this.done[this.inprogress.name] = this.inprogress; + logger.info('Action %s est finie !', this.inprogress.name); + this.inprogress = null; + var temp = this.callback; + this.callback = function() {logger.warn('callback vide'); }; + temp(); + } + }; + + return Actions; +})(); diff --git a/ia/actions.json b/ia/actions.json new file mode 100644 index 0000000..e0fba8a --- /dev/null +++ b/ia/actions.json @@ -0,0 +1,213 @@ +{ + "__comment": "\nAdd durations, priority\nAdd actions without objects", + "actions":{ + "faire_les_claps_1":{ + "type": "clap", + "owner": "pr", + "objectname": "clap__1", + "startpoints": [{ + "x": 140, + "y": 140, + "a": 3.1416 + }], + "sens": "osef", + "priority": 18, + "dependency": "", + "duration": 1, + "orders":[{ + "name": "clap_1", + "params": {} + }], + "kill": null + }, + "faire_les_claps_3":{ + "type": "clap", + "owner": "pr", + "objectname": "clap__3", + "startpoints": [{ + "x": 730, + "y": 140, + "a": 3.1416 + }], + "sens": "osef", + "priority": 17, + "dependency": "", + "duration": 1, + "orders":[{ + "name": "clap_3", + "params": {} + }], + "kill": null + }, + "prendre_gobelet_et_2_plots_front_left":{ + "type": "gobelet", + "owner": "pr", + "objectname": "gobelet__front_left", + "startpoints": [ + { + "x": 450, + "y": 350, + "a": 0 + }], + "sens": "arriere", + "priority": 19, + "dependency": "", + "dependencyRobotContent": { + "gobelet": false, + "subPlot": 2, + "invPlot": 0 + }, + "duration": 1, + "orders":[{ + "name": "prendre_gobelet_et_2_plots_front", + "params": {} + }], + "kill": null + }, + "prendre_plot_middle_left1":{ + "type": "plot", + "owner": "pr", + "objectname": "plot__middle_left1", + "startpoints": [], + "sens": "avant", + "priority": 21, + "dependency": "", + "dependencyRobotContent": { + "subPlot": 3, + "invPlot": 0 + }, + "duration": 1, + "orders":[{ + "name": "prendre_plot", + "params": {} + }], + "kill": null + }, + "prendre_plot_middle_left2":{ + "type": "plot", + "owner": "pr", + "objectname": "plot__middle_left2", + "startpoints": [], + "sens": "avant", + "priority": 20, + "dependency": "", + "dependencyRobotContent": { + "subPlot": 3, + "invPlot": 0 + }, + "duration": 1, + "orders":[{ + "name": "prendre_plot", + "params": {} + }], + "kill": null + }, + "pousser_plot_middle_left3":{ + "type": "plot", + "owner": "pr", + "objectname": "plot__middle_left3", + "startpoints": [], + "sens": "avant", + "priority": 21, + "dependency": "", + "dependencyRobotContent": { + "subPlot": 3, + "invPlot": 0 + }, + "duration": 1, + "orders":[{ + "name": "pousser_plot", + "params": {} + }], + "kill": null + }, + "deposer_pile_gobelet_gauche":{ + "type": "depot_pile", + "owner": "pr", + "objectname": "pile__left", + "startpoints": [{ + "x": 700, + "y": 1000, + "a": 3.1416 + }], + "sens": "avant", + "priority": 14, + "dependency": "", + "dependencyRobotContent": { + "invPlot": 1 + }, + "duration": 1, + "orders":[{ + "name": "deposer_pile_et_gobelet", + "params": {} + }], + "kill": null + }, + "prendre_plot_rear_left":{ + "type": "plot", + "owner": "pr", + "objectname": "plot__rear_left", + "startpoints": [{ + "x": 650, + "y": 1450, + "a": 0 + }], + "sens": "avant", + "priority": 13, + "dependency": "deposer_pile_gobelet_gauche", + "dependencyRobotContent": { + "subPlot": 3 + }, + "duration": 1, + "orders":[{ + "name": "prendre_plot_rear_left_calage", + "params": {} + }], + "kill": null + }, + "prendre_2_plots_stairs_left":{ + "type": "plot", + "owner": "pr", + "objectname": "plot__stairs_left2", + "startpoints": [{ + "x": 705, + "y": 1640, + "a": 0 + }], + "sens": "avant", + "priority": 12, + "dependency": "prendre_plot_rear_left", + "dependencyRobotContent": { + "subPlot": 2 + }, + "duration": 2, + "orders":[{ + "name": "prendre_2_plots_stairs", + "params": {} + }], + "kill": null + }, + "deposer_pile_gauche_2":{ + "type": "depot_pile", + "owner": "pr", + "objectname": "pile__left2", + "startpoints": [{ + "x": 700, + "y": 1000, + "a": -1.5708 + }], + "sens": "avant", + "priority": 9, + "dependency": "deposer_pile_gobelet_gauche", + "dependencyRobotContent": { + "invPlot": 1 + }, + "duration": 1, + "orders":[{ + "name": "deposer_pile_left_2", + "params": {} + }], + "kill": null + } + } +} diff --git a/ia/architecture_ia_utcoupe_2015.jpg b/ia/architecture_ia_utcoupe_2015.jpg new file mode 100644 index 0000000..9d8c993 Binary files /dev/null and b/ia/architecture_ia_utcoupe_2015.jpg differ diff --git a/ia/data.class.js b/ia/data.class.js new file mode 100644 index 0000000..53a06dd --- /dev/null +++ b/ia/data.class.js @@ -0,0 +1,133 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.data'); + + function Data(ia, nb_erobots, EGR_d, EPR_d) { + this.balle = []; + this.chargeur = []; + this.clap = []; + this.plot = []; + this.erobot = []; + this.gobelet = []; + this.pile = {}; + this.dynamic = []; //used for laid stacks + this.depot = []; + this.nb_erobots = nb_erobots; + + this.dots = []; + + this.importObjects(); + + this.erobot = [{ // big robot on position 0 + name: "gr", + pos:{ + x:3200, + y:1000 + }, + speed:{ // in mm/sec + x:0, + y:0, + }, + lastUpdate: 0, // time in ms from the beining of the match + d: EGR_d || 320, + status: "lost" + },{ // small robot on position 1 + name: "pr", + pos:{ + x:3500, + y:1000 + }, + speed:{ + x:0, + y:0 + }, + lastUpdate: 0, + d: EPR_d || 200, + status: "lost" + }]; + } + + Data.prototype.importObjects = function () { + var ret = require('./objects.json'); + + this.balle = ret.balle; + this.chargeur = ret.chargeur; + this.clap = ret.clap; + this.plot = ret.plot; + this.gobelet = ret.gobelet; + this.pile = ret.pile; + this.depot = ret.depot; + return ret; + }; + + Data.prototype.getObjectRef = function(name) { + // Retourne une référence vers l'objet name + // name étant de la forme __ + // Permet d'avoir une référence vers l'objet dans une action + + var actName = name.split("__"); + if (actName.length != 2){ + logger.warn("Le nom '"+name+"' n'est pas un nom d'objet correct."); + return null; + } + + if (!this[actName[0]][actName[1]]){ + logger.warn("L'objet "+actName[0]+" de type "+actName[1]+" est inconnu."); + return null; + } + + return this[actName[0]][actName[1]]; + }; + + Data.prototype.isCloser = function (dist1, dist2){ // il y a la meme dans actions.class.js + // Returns true if dist1 is smaller than dist2 + // i.e. object 1 is closer than object 2 + + if(dist1 < dist2){ + return true; + } else { + return false; + } + }; + + Data.prototype.getDistance = function (pos1, pos2){ + return Math.sqrt(Math.pow(pos1.x - pos2.x, 2) + Math.pow(pos1.y - pos2.y, 2)); + }; + + Data.prototype.theEnnemyWentThere = function (pos, e_robot_id){ + // takes a position and the ennemy robot # to put everything in its surroundings (~ 1.1 * radius) as "lost" + + Object.keys(this.plot).forEach(function(c) { + if ((this.getDistance(pos, this.plot[c].pos) < 0.55*this.erobot[e_robot_id].d) && (this.plot[c].status != "lost")) { + logger.info("Le plot " + c + " est marqué lost"); + this.plot[c].status = "lost"; + } + }.bind(this)); + var min_dist = Infinity; + Object.keys(this.gobelet).forEach(function(g) { + if ((this.getDistance(pos, this.gobelet[g].pos) < 0.55*this.erobot[e_robot_id].d) && (this.gobelet[g].status != "lost")) { + logger.info("Le gobelet" + g + " est marqué lost"); + this.gobelet[g].status = "lost"; + } + }.bind(this)); + }; + + Data.prototype.parseOrder = function(from, name, param){ + switch(name){ + case "data.add_dynamic" : + if(!!param.pos && !!param.pos.x && !!param.pos.y && !!param.d){ + this.dynamic.push(param); + logger.info("added dynamic from :"+from+" params:"+JSON.stringify(param)); + }else{ + logger.error("invalid dynamic from :"+from+" params:"+JSON.stringify(param)); + } + break; + default: logger.error("Unknown order name:"+name+" from:"+from); + } + }; + + var data = new Data(); + + return Data; +})(); diff --git a/ia/export_simulator.class.js b/ia/export_simulator.class.js new file mode 100644 index 0000000..be35410 --- /dev/null +++ b/ia/export_simulator.class.js @@ -0,0 +1,77 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.export_simulator'); + + var __timeout = null; + var FPS = 30; + var color; + + function ExportSimulator(ia) { + this.ia = ia; + this.start(); + color = this.ia.color; + } + + function convertX(x) { + if(color == "yellow") { + return (x-1500)/1000; + } else { + return (1500-x)/1000; + } + } + function convertY(y) { + return (1000-y)/1000; + } + function convertA(a) { + if(color == "yellow") { + return a; + } else { + return (a < 0) ? -Math.PI - a : Math.PI - a; + } + } + + ExportSimulator.prototype.start = function() { + this.orderToSimu(); + } + ExportSimulator.prototype.stop = function() { + clearTimeout(__timeout); + } + + ExportSimulator.prototype.orderToSimu = function() { + var data = {}; + + data.robots = { + gr: { + x: convertX(this.ia.gr.pos.x), + y: convertY(this.ia.gr.pos.y), + a: convertA(this.ia.gr.pos.a) + }, + pr: { + x: convertX(this.ia.pr.pos.x), + y: convertY(this.ia.pr.pos.y), + a: convertA(this.ia.pr.pos.a), + path: [this.ia.pr.pos].concat(this.ia.pr.path).map(function(pos){ + return [convertX(pos.x), convertY(pos.y)]; + }) + }, + egr: { + x: convertX(this.ia.data.erobot[0].pos.x), + y: convertY(this.ia.data.erobot[0].pos.y), + }, + epr: { + x: convertX(this.ia.data.erobot[1].pos.x), + y: convertY(this.ia.data.erobot[1].pos.y) + } + }; + data.dynamic = this.ia.data.dynamic.map(function(o){ + return [convertX(o.pos.x), convertY(o.pos.y)]; + }); + // logger.debug(data.robots.egr); + this.ia.client.send("webclient", "simulateur", data); + + __timeout = setTimeout(function(){this.orderToSimu()}.bind(this), 1000/FPS); + } + + return ExportSimulator; +})(); \ No newline at end of file diff --git a/ia/gr.class.js b/ia/gr.class.js new file mode 100644 index 0000000..3d67fcf --- /dev/null +++ b/ia/gr.class.js @@ -0,0 +1,62 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.gr'); + + function Gr(ia, color) { + this.ia = ia; + this.pos = require('./gr.'+color+'.json')['pos']; + this.pos.color = color; + this.size = { + l: 290, + L: 290, + d: 420 + }; + this.orders = require('./gr.'+color+'.json')['script']; + // logger.debug(this.orders); + this.path = null; + } + + Gr.prototype.start = function () { + this.sendOrders(); + }; + + Gr.prototype.stop = function() { + logger.debug('stop GR'); + this.ia.client.send('gr', 'stop'); + } + + Gr.prototype.sendPos = function () { + this.ia.client.send("gr", "setpos", this.pos); + }; + + Gr.prototype.parseOrder = function (from, name, params) { + switch(name) { + case 'gr.pos': + this.pos.x = params.x; + this.pos.y = params.y; + this.pos.a = params.a; + break; + case 'gr.getpos': + this.sendPos(); + break; + default: + logger.warn('Ordre inconnu dans ia.gr: '+name); + } + }; + + Gr.prototype.sendOrders = function () { + for(var i in this.orders) { + this.ia.client.send("gr", this.orders[i].name, this.orders[i].data); + } + }; + + Gr.prototype.onCollision = function () { + logger.warn("Collision du gros robot"); + // TODO send order STOP + }; + + + + return Gr; +})(); diff --git a/ia/gr.green.json b/ia/gr.green.json new file mode 100644 index 0000000..3f4d36a --- /dev/null +++ b/ia/gr.green.json @@ -0,0 +1,69 @@ +{ + "pos": { + "x": 110, + "y": 1000, + "a": 0 + }, + "script": [ + { + "name": "setpid", + "data": { + "p": 1.2, + "i": 20, + "d": 5 + } + },{ + "name": "setpos", + "data": { + "x": 130, + "y": 1000, + "a": 0, + "color": "green" + } + },{ + "name": "acheter", + "data": {} + },{ + "name": "setvit", + "data": { + "v": 500, + "r": 0.3 + } + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 1000 + } + },{ + "name": "goa", + "data": { + "a": 1.5708 + } + },{ + "name": "setvit", + "data": { + "v": 150, + "r": 0.3 + } + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 1510 + } + },{ + "name": "vendre", + "data": {} + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 1800 + } + },{ + "name": "acheter", + "data": {} + } + ] +} diff --git a/ia/gr.yellow.json b/ia/gr.yellow.json new file mode 100644 index 0000000..4823ee6 --- /dev/null +++ b/ia/gr.yellow.json @@ -0,0 +1,69 @@ +{ + "pos": { + "x": 110, + "y": 1000, + "a": 0 + }, + "script": [ + { + "name": "setpid", + "data": { + "p": 1.2, + "i": 20, + "d": 5 + } + },{ + "name": "setpos", + "data": { + "x": 130, + "y": 1000, + "a": 0, + "color": "green" + } + },{ + "name": "acheter", + "data": {} + },{ + "name": "setvit", + "data": { + "v": 500, + "r": 0.3 + } + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 1000 + } + },{ + "name": "goa", + "data": { + "a": -1.5708 + } + },{ + "name": "setvit", + "data": { + "v": 150, + "r": 0.3 + } + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 490 + } + },{ + "name": "vendre", + "data": {} + },{ + "name": "goxy", + "data": { + "x": 1245, + "y": 200 + } + },{ + "name": "acheter", + "data": {} + } + ] +} diff --git a/ia/hokuyo.class.js b/ia/hokuyo.class.js new file mode 100644 index 0000000..1b472f8 --- /dev/null +++ b/ia/hokuyo.class.js @@ -0,0 +1,494 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + // var gaussian = require('gaussian'); + var logger = log4js.getLogger('ia.hokuyo'); + + var GR_OFFSET = 110; // (mm) distance between our robot origin and the robot's center + var PR_GR_COEF = 1; // security coeff to bind our robots with the dots + var SEGMENT_DELTA_D = 30; // (mm) between 2 iterations on a segment to detect colision + var DELTA_T = 500; // (ms) in the future : estimation of ennemy robots + var DEBUG = false; // mettre à true LE TEMPS DU DEBUG !!!!! + var LOST_TIMEOUT = 10; + var SECURITY_COEF = 1.1; + var MAX_SPD = 1; + var timeout; + + function Hokuyo(ia, params) { + this.params = params || {}; + this.nb_hokuyo = 0; + this.ia = ia; + this.lastNow = 0; // dernier timestamp où on a update (changé à la fin de l'update) + this.matchStarted = false; + this.nb_lost = 0; // nb d'itération depuis laquelle on a perdu un robot + } + + Hokuyo.prototype.start = function () { + this.matchStarted = true; + + logger.info("La classe hokuyo attend..."); + this.ia.client.send("hokuyo", "start", {color:this.params.color}); + timeout = setTimeout(function() {this.timedOut(); }.bind(this) , LOST_TIMEOUT*1000); + }; + + Hokuyo.prototype.stop = function () { + this.matchStarted = false; + this.ia.client.send("hokuyo", "stop", {}); + clearTimeout(timeout); + }; + + Hokuyo.prototype.timedOut = function() { + this.mayday("Hokuyo timed out"); + }; + + Hokuyo.prototype.getDistance = function (spot1, spot2) { + return Math.sqrt(Math.pow(spot1.x - spot2.x, 2) + Math.pow(spot1.y - spot2.y, 2)); + }; + + Hokuyo.prototype.isOnTheStairs = function (spot){ + return (spot.x>967) && (spot.x < 2033) && (spot.y > 2000-580); + }; + + Hokuyo.prototype.getMatchingCoef = function (spot, eRobot, dt, status){ + var estimatedPos = { + x: eRobot.pos.x + eRobot.speed.x*dt, + y: eRobot.pos.y + eRobot.speed.y*dt + }; + + // var distribution = gaussian(0,eRobot.d); + var distance = this.getDistance(spot, estimatedPos); + // logger.debug(distance); + // return 10*distribution.pdf(distance/10); + return distance; + }; + + Hokuyo.prototype.deleteOurRobots = function (dots){ + var pr_dist = this.getDistance({x: 0, y:0}, {x: 3000, y:2000}); + var pr_i = -1; + var gr_dist = this.getDistance({x: 0, y:0}, {x: 3000, y:2000}); + var gr_i = -1; + + var gr_pos_with_offset = { + x: this.ia.gr.pos.x + GR_OFFSET*Math.cos(this.ia.gr.pos.a), + y: this.ia.gr.pos.y + GR_OFFSET*Math.sin(this.ia.gr.pos.a) + }; + + // logger.debug("Pos PR"); + // logger.debug(this.ia.pr.pos); + // logger.debug("Pos GR"); + // logger.debug(gr_pos_with_offset); + + for (var i = 0; i < dots.length; i++) { + var pr_temp_dist = this.getDistance(dots[i], this.ia.pr.pos); + var gr_temp_dist = this.getDistance(dots[i], gr_pos_with_offset); + // logger.debug("Pr le pt :"); + // logger.debug(dots[i]); + // logger.debug(pr_temp_dist); + // logger.debug(gr_temp_dist); + + + if ((pr_dist > pr_temp_dist) && (pr_temp_dist < this.ia.pr.size.d * PR_GR_COEF)){ + pr_dist = pr_temp_dist; + pr_i = i; + } + + if ((gr_dist > gr_temp_dist) && (gr_temp_dist < this.ia.gr.size.d * PR_GR_COEF)){ + gr_dist = gr_temp_dist; + gr_i = i; + } + } + + if (pr_i != -1) { + // logger.debug("Deleting PR:"); + // logger.debug(dots[pr_i]); + // logger.debug(this.ia.pr.pos); + dots.splice(pr_i,1); + + if (pr_i < gr_i) { + gr_i = gr_i -1; + } + } else { + logger.warn("On a pas trouvé le PR parmis les points de l'Hokuyo"); + } + + if (gr_i != -1) { + // logger.debug("Deleting GR:"); + // logger.debug(dots[gr_i]); + // logger.debug(gr_pos_with_offset); + // logger.debug(this.getDistance(dots[gr_i], gr_pos_with_offset)); + + dots.splice(gr_i,1); + } else { + logger.warn("On a pas trouvé le GR parmis les points de l'Hokuyo"); + } + // logger.debug(dots); + }; + + Hokuyo.prototype.getBestMatchingCoef = function(dots, robots, now) { + // Return the couple of ennemy robot and dot that matches the best + + var matching_coef = []; + var best_coef = { + value: this.getDistance({x: 0, y:0}, {x: 3000, y:2000}), + dot: -1, + e_robot: -1 + }; + + for (var d = 0; d < dots.length; d++){ + // for each real point + matching_coef[d] = []; + for (var i = 0; i < robots.length; i++) { + // we make a matching coefficient + + matching_coef[d][i] = this.getMatchingCoef(dots[d], robots[i], robots[i].lastUpdate - now, robots[i].status); + + if (best_coef.value > matching_coef[d][i]) { + best_coef.value = matching_coef[d][i]; + best_coef.dot = d; + best_coef.e_robot = i; + } + } + } + + //logger.debug("Coefficients de matching"); + //logger.debug(matching_coef); + + return best_coef; + }; + + Hokuyo.prototype.updatePos = function (dots) { + // Invert dots if we are green + if (this.ia.color == "green"){ + for (var i = 0; i < dots.length; i++) { + dots[i].x = 3000 - dots[i].x; + } + } + + if (!this.matchStarted){ +// logger.debug("Le match n'a pas commencé"); + return; + } + + if (dots.length === 0) + logger.warn("On a reçu un message vide (pas de spots dedans)"); + else { + + clearTimeout(timeout); + + if (dots.length != ((this.params.we_have_hats?2:0) + this.params.nb_erobots)) { + // logger.info("On a pas le meme nombre de spots ("+dots.length+") que de robots à détecter ("+ +// ((this.params.we_have_hats?2:0) + this.params.nb_erobots) + ")."); + } + + // takes a timestamp to be able to compute speeds + var now = this.ia.timer.get(); + // var now = this.lastNow = this.lastNow+1; + + // if we have hats, kill ourselves (virtualy, of course) + if (this.params.we_have_hats) + this.deleteOurRobots(dots); + + // // until there are dots left to be matched with ennmies + // while (dots.length > 0){ + // // if some robots aren't already matched + // var e_r2Bmatched = []; + + // for (var i = 0; i < this.ia.data.nb_erobots; i++) { + // if(this.ia.data.erobot[i].lastUpdate < now){ + // // logger.debug(this.ia.data.erobot[i].name + " last update :"); + // // logger.debug(this.ia.data.erobot[i].lastUpdate); + // e_r2Bmatched.push(this.ia.data.erobot[i]); + // } + // } + + // if (e_r2Bmatched.length > 0) { + // // logger.debug("On s'occupe des robots :"); + // // logger.debug(e_r2Bmatched); + // // logger.debug("Avec les points :"); + // // logger.debug(dots); + + + // // for each eatimated position of the robots + // var best_coef = this.getBestMatchingCoef(dots, e_r2Bmatched, now); + // // logger.debug(best_coef); + + // // if the bigger coefficient is under the arbitrary threshold + // // XXX + + // // we take the best/bigger coefficient (well named best_coef :P ) + + // // if it isn't, set the new position, speed, status, call the "ennemy went there" function + // // logger.debug("On a matché le point suivant avec le "+e_r2Bmatched[best_coef.e_robot].name+" ennemi"); + // // logger.debug(dots[best_coef.dot]); + + // if (best_coef.e_robot == -1){ + // logger.warn('Erreur de matching (il reste des robots à matcher avec des points mais ils collent pas) :'); + // //logger.warn(e_r2Bmatched); + // //logger.warn(dots); + // break; + // } + + // //logger.debug("Le "+e_r2Bmatched[best_coef.e_robot].name+" est passé de "+e_r2Bmatched[best_coef.e_robot].pos.x+", "+e_r2Bmatched[best_coef.e_robot].pos.y+ " à "+dots[best_coef.dot].x+", "+dots[best_coef.dot].y); + + // e_r2Bmatched[best_coef.e_robot].lastUpdate = now; + // var deltaT = now - this.lastNow; + // if (deltaT !== 0){ + // var x_spd = (dots[best_coef.dot].x - e_r2Bmatched[best_coef.e_robot].pos.x)/deltaT; + // var y_spd = (dots[best_coef.dot].y - e_r2Bmatched[best_coef.e_robot].pos.y)/deltaT; + // if (x_spd > MAX_SPD) { + // x_spd = MAX_SPD; + // } else if (x_spd < -MAX_SPD) { + // x_spd = -MAX_SPD; + // } + // if (y_spd > MAX_SPD) { + // y_spd = MAX_SPD; + // } else if (y_spd < -MAX_SPD) { + // y_spd = -MAX_SPD; + // } + + // e_r2Bmatched[best_coef.e_robot].speed = { + // x: x_spd, + // y: y_spd + // }; + // } else { + // logger.warn("Tiens, deltaT = 0, c'est bizarre..."); + // e_r2Bmatched[best_coef.e_robot].speed = { + // x:0, + // y:0 + // }; + // } + + // e_r2Bmatched[best_coef.e_robot].pos = { + // x: dots[best_coef.dot].x, + // y: dots[best_coef.dot].y, + // }; + + // // logger.debug("Position et vitesse du robot :"); + // // logger.debug(e_r2Bmatched[best_coef.e_robot].pos); + // // logger.debug(e_r2Bmatched[best_coef.e_robot].speed); + + + // // if it's climbing the stairs, set the correct status + // if (this.isOnTheStairs(dots[best_coef.dot])){ + // e_r2Bmatched[best_coef.e_robot].status = "on_the_stairs"; + // logger.info(e_r2Bmatched[best_coef.e_robot].status); + // } else { + // e_r2Bmatched[best_coef.e_robot].status = "moving"; + // this.ia.data.theEnnemyWentThere(e_r2Bmatched[best_coef.e_robot].pos, best_coef.e_robot); + // } + + // // and delete the dot + // dots.splice(best_coef.dot,1); + // } else { + // // if all the robots are tidied up, ouw, that's strange ^^ + // //logger.warn("Un ou plusieurs spots de plus que de robots sur la table :"); + // //logger.warn(dots); + // //logger.warn("e_r2Bmatched"); + // //logger.warn(e_r2Bmatched); + // break; + // } + + // } + + // // XXX /!\ robots on the stairs ! + + + // // if there's some robots to be matched (but no real point left :/), they're lost... + // // we estimate their position and tag them with "lost" + // for (var i = 0; i < this.ia.data.nb_erobots; i++) { + // if ((this.ia.data.erobot[i].lastUpdate < now) && (this.ia.data.erobot[i].status == "moving")){ + // this.ia.data.erobot[i].pos = { + // x: this.ia.data.erobot[i].pos.x + this.ia.data.erobot[i].speed.x*Math.abs(this.ia.data.erobot[i].lastUpdate - now), + // y: this.ia.data.erobot[i].pos.y + this.ia.data.erobot[i].speed.y*Math.abs(this.ia.data.erobot[i].lastUpdate - now) + // }; + // this.ia.data.erobot[i].speed = { + // x:0, + // y:0, + // }; + // this.ia.data.erobot[i].status = "lost"; + // this.ia.data.erobot[i].lastUpdate = now; + // this.nb_lost = 0; + // } else if ((this.ia.data.erobot[i].lastUpdate < now) && (this.ia.data.erobot[i].status == "lost") && (this.nb_lost 300 && d.x < 2700 && d.y > 300 && d.y < 1700) { + this.ia.pr.stop(); + } + } + + timeout = setTimeout(function() {this.timedOut();}.bind(this) , 1000); + } + + }; + + Hokuyo.prototype.detectCollision = function(dots) { + var collision = false; + var pf = this.ia.pr.path; + var minDist; + // for each path segment + var complete_path = [this.ia.pr.pos].concat(this.ia.pr.path); + for (var i = 0; i < complete_path.length-1 && !collision; (i++) ) { + var segment = [complete_path[i], complete_path[i+1]]; // so segment[0][0] is the x value of the beginning of the segment + var segLength = this.getDistance({x:segment[0].x , y:segment[0].y }, {x:segment[1].x , y:segment[1].y }); + var nthX = (segment[1].x-segment[0].x)*SEGMENT_DELTA_D/segLength; // segment X axis length nth + var nthY = (segment[1].y-segment[0].y)*SEGMENT_DELTA_D/segLength; + + // for each X cm of the segment + for (var j = 0; (j*SEGMENT_DELTA_D) < segLength && !collision; (j++)) { + + var segPoint = { + x: segment[0].x + nthX*j, + y: segment[0].y + nthY*j + }; + + // distance to each estimated position of the ennemi robots + minDist = 10000;//this.getDistance(dots[0], segPoint); + + for(var k = 0; k < dots.length; k++) { + var tmp = this.getDistance(dots[k], segPoint); + if (tmp < minDist) { + minDist = tmp; + } + } + // if (ebots.length == 2) { + // var tmp = this.getDistance(ebots[1], segPoint); + // if (tmp < minDist) { + // minDist = tmp; + // } + // } + + // if one of the dist < security diameter, there will be a collision + if (minDist < 450) { + collision = true; + } + + } + } + + if (collision) { + this.ia.pr.collision(); + } + }; + + // Hokuyo.prototype.detectCollision = function() { + // var collision = false; + // var pf = this.ia.pr.path; + // var ebots = [{ // estimated positions + // x:this.ia.data.erobot[0].pos.x + this.ia.data.erobot[0].speed.x*DELTA_T, + // y:this.ia.data.erobot[0].pos.y + this.ia.data.erobot[0].speed.y*DELTA_T, + // d:this.ia.data.erobot[0].d + // }]; + + // if (this.ia.data.nb_erobots == 2) { + // ebots.push = { + // x:this.ia.data.erobot[1].pos.x + this.ia.data.erobot[1].speed.x*DELTA_T, + // y:this.ia.data.erobot[1].pos.y + this.ia.data.erobot[1].speed.y*DELTA_T, + // d:this.ia.data.erobot[1].d + // }; + // } + + // // for each path segment + // var complete_path = [this.ia.pr.pos].concat(this.ia.pr.path); + // for (var i = 0; i < complete_path.length-1 && !collision; (i++) ) { + // var segment = [complete_path[i], complete_path[i+1]]; // so segment[0][0] is the x value of the beginning of the segment + // var segLength = this.getDistance({x:segment[0].x , y:segment[0].y }, {x:segment[1].x , y:segment[1].y }); + // var nthX = (segment[1].x-segment[0].x)*SEGMENT_DELTA_D/segLength; // segment X axis length nth + // var nthY = (segment[1].y-segment[0].y)*SEGMENT_DELTA_D/segLength; + + // // for each X cm of the segment + // for (var j = 0; (j*SEGMENT_DELTA_D) < segLength && !collision; (j++)) { + + // var segPoint = { + // x: segment[0].x + nthX*j, + // y: segment[0].y + nthY*j + // }; + + // // distance to each estimated position of the ennemi robots + // var minDist = this.getDistance(ebots[0], segPoint); + + // /* + // if (ebots.length == 2) { + // var tmp = this.getDistance(ebots[1], segPoint); + // if (tmp < minDist) { + // minDist = tmp; + // } + // }*/ + + // // if one of the dist < security diameter, there will be a collision + // if (minDist < SECURITY_COEF*ebots[0].d/2.0) { + // collision = true; + // } + + // } + // } + + // if (collision) { + // this.ia.pr.collision(); + // } + // }; + + Hokuyo.prototype.updateNumberOfRobots = function (nb) { + switch (nb){ + case 0: + this.nb_hokuyo = 0; + // Fatal error + this.mayday("On plus d'hokuyo"); + break; + case 1: + this.nb_hokuyo = 1; + // bigger security zone ? throw startAgain ? + break; + case 2: + this.nb_hokuyo = 2; + break; + default: + logger.info("Invalid number of robots received :" + nb); + } + }; + + Hokuyo.prototype.mayday = function(reason) { + logger.error("Mayday called, the given reason is :"); + logger.error(reason); + + // XXX ? -> en fait on bloque peut-être pas ! : si le robot s'est vautré, c'est son soucis + }; + + Hokuyo.prototype.parseOrder = function (from, name, params) { + var orderName = name.split('.')[1]; + switch (orderName){ + case "position_tous_robots": + this.updatePos(params.dots); + break; + case "nb_hokuyo": + this.updateNumberOfRobots(params.nb); + break; + default: + logger.warn("Message name " + name + " not understood"); + } + }; + + Hokuyo.prototype.isOk = function () { + if (this.nb_hokuyo === 0) + return false; + else + return true; + }; + + return Hokuyo; +})(); diff --git a/ia/ia.class.js b/ia/ia.class.js new file mode 100644 index 0000000..a600a0d --- /dev/null +++ b/ia/ia.class.js @@ -0,0 +1,113 @@ +module.exports = (function () { + "use strict"; + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.ia'); + var log_counter = 0; + + function norm(Ax, Ay, Bx, By) { + return Math.sqrt(Math.pow(Ax-Bx, 2) + Math.pow(Ay-By, 2)); + } + + function Ia(color, nb_erobots, EGR_d, EPR_d) { + var we_have_hats = true; + if(!color) { + logger.error('Please give a color to ia'); + } + if(!nb_erobots) { + logger.error('Please give the number of ennemis robots'); + } + if(!we_have_hats) { + logger.error('Please say true if we have something on our robots detectable by the Hokuyos'); + } + if(!EGR_d) { + logger.error('Please give the EGR diameter'); + } + if(!EPR_d) { + logger.error('Please give the EPR diameter'); + } + this.color = color || "yellow"; + this.nb_erobots = nb_erobots || 2; + logger.info("Launching a "+this.color+" AI with "+this.nb_erobots+" ennemies."); + + this.client = new (require('../server/socket_client.class.js'))({type: 'ia', server_ip: require('../config.js').server }); + this.timer = new (require('./timer.class.js'))(this); + this.pathfinding = new (require('./pathfinding.class.js'))(this); + this.data = new (require('./data.class.js'))(this, this.nb_erobots, EGR_d, EPR_d); + this.actions = new (require('./actions.class.js'))(this); + this.gr = new (require('./gr.class.js'))(this, this.color); + this.pr = new (require('./pr.class.js'))(this, this.color); + // this.hokuyo = new (require('./hokuyo.class.js'))(this, { + // color: this.color, + // nb_erobots: parseInt(this.nb_erobots), + // we_have_hats: (we_have_hats === "true") + // }); + this.export_simulator = new (require('./export_simulator.class.js'))(this); + + this.client.send("server", "server.iaColor", {color: this.color}); + + this.client.order(function(from, name, params) { + var classe = name.split('.')[0]; + // logger.debug(this[classe]); + if(classe == 'ia') { + switch(name) { + case 'ia.jack': + this.jack(); + break; + case 'ia.stop': + this.stop(); + break; + case 'ia.hok': + if ((log_counter++ % 15) == 0) { + logger.debug(params); + } + this.pr.updatePos(params); + break; + case 'ia.hokfailed': + logger.fatal("HOKUYO NOT WORKING, UNPLUG AND REPLUG USB"); + this.pr.updatePos(params); + break; + default: + logger.warn("Ordre pour l'ia inconnu : "+name); + } + } else if(!!this[classe]) { + // logger.debug("Order to class: "+classe); + if(!this[classe].parseOrder) { + logger.warn("Attention, pas de fonction parseOrder dans ia."+classe); + } else { + this[classe].parseOrder(from, name, params); + } + } else { + logger.warn("Sous client inconnu: "+classe); + } + }.bind(this)); + + // temp // + // this.gr.start(); + // this.jack(); + ////////// + } + + Ia.prototype.jack = function() { + if(!this.timer.match_started) { + logger.info("Démarrage du match"); + this.timer.start(); + setTimeout(function() { + this.gr.start(); + }.bind(this), 10000); + this.pr.start(); + // this.hokuyo.start(); + } else { + logger.warn("Match déjà lancé"); + } + }; + + Ia.prototype.stop = function() { + logger.fatal('Stop IA'); + this.gr.stop(); + this.pr.stop(); + // this.hokuyo.stop(); + setTimeout(process.exit, 1000); + }; + + return Ia; +})(); diff --git a/ia/main.js b/ia/main.js new file mode 100644 index 0000000..7805f09 --- /dev/null +++ b/ia/main.js @@ -0,0 +1,18 @@ +(function () { + "use strict"; + + // Modules nodejs + var log4js = require('log4js'); + var logger = log4js.getLogger('ia.ia'); + + var color = process.argv[2]; + if(!color) color = null; + var nb_erobots = process.argv[3]; + if(!nb_erobots) nb_erobots = null; + var EGR_d = process.argv[4]; + if(!EGR_d) EGR_d = null; + var EPR_d = process.argv[5]; + if(!EPR_d) EPR_d = null; + + var ia = new (require('./ia.class.js'))(color, nb_erobots, EGR_d, EPR_d); +})(); \ No newline at end of file diff --git a/ia/objects.json b/ia/objects.json new file mode 100644 index 0000000..1565f1f --- /dev/null +++ b/ia/objects.json @@ -0,0 +1,258 @@ +{ + "balle":{ + "gauche":{ + "diametre": 100, + "color": "none", + "status": "initial", + "pos": { + "x":1250, + "y":50 + } + }, + "droite":{ + "diametre": 100, + "color": "none", + "status": "initial", + "pos": { + "x":1750, + "y":50 + } + } + }, + "chargeur":{ + "1":{ + "diametre": 70, + "color": "none", + "status": "initial", + "pos": { + "x":300, + "y":1965 + } + }, + "2":{ + "diametre": 70, + "color": "none", + "status": "initial", + "pos": { + "x":600, + "y":1965 + } + }, + "3":{ + "diametre": 70, + "color": "none", + "status": "initial", + "pos": { + "x":2400, + "y":1965 + } + }, + "4":{ + "diametre": 70, + "color": "none", + "status": "initial", + "pos": { + "x":2700, + "y":1965 + } + } + }, + "clap":{ + "1":{ + "diametre": 0, + "color": "yellow", + "status": "initial", + "pos": { + "x":320, + "y":0 + } + }, + "3":{ + "diametre": 0, + "color": "yellow", + "status": "initial", + "pos": { + "x":920, + "y":0 + } + }, + "5":{ + "diametre": 0, + "color": "yellow", + "status": "initial", + "pos": { + "x":2380, + "y":0 + } + } + + }, + "plot":{ + "front_left1":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":90, + "y":250 + } + }, + "front_left2":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":90, + "y":150 + } + }, + "middle_left1":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":870, + "y":645 + } + }, + "middle_left2":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":1300, + "y":600 + } + }, + "middle_left3":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":1100, + "y":230 + } + }, + "rear_left":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":90, + "y":1800 + } + }, + "stairs_left1":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":850, + "y":1900 + } + }, + "stairs_left2":{ + "diametre": 60, + "color": "yellow", + "status": "initial", + "pos": { + "x":850, + "y":1800 + } + } + }, + "gobelet":{ + "front_left":{ + "diametre": 95, + "color": "none", + "status": "initial", + "pos": { + "x":250, + "y":250 + } + }, + "middle_left":{ + "diametre": 95, + "color": "none", + "status": "initial", + "pos": { + "x":910, + "y":1170 + } + }, + "middle":{ + "diametre": 95, + "color": "none", + "status": "initial", + "pos": { + "x":1500, + "y":350 + } + }, + "middle_right":{ + "diametre": 95, + "color": "none", + "status": "initial", + "pos": { + "x":2090, + "y":1170 + } + }, + "front_right":{ + "diametre": 95, + "color": "none", + "status": "initial", + "pos": { + "x":2750, + "y":250 + } + } + }, + "pile": { + "left": { + "color": "none", + "status": "initial", + "pos": { + "x":-200, + "y":1000 + }, + "d" : 6 + }, + "left2": { + "color": "none", + "status": "initial", + "pos": { + "x":550, + "y":1000 + }, + "d" : 6 + }, + "middle": { + "color": "none", + "status": "initial", + "pos": { + "x":1500, + "y":150 + }, + "d" : 6 + } + }, + "depot": { + "gobelet_rear_right": { + "color": "none", + "status": "initial", + "pos": { + "x": 2750, + "y": 1400 + } + }, + "gobelet_front_right": { + "color": "none", + "status": "initial", + "pos": { + "x": 2750, + "y": 600 + } + } + } +} diff --git a/ia/objects.pdf b/ia/objects.pdf new file mode 100644 index 0000000..a1d96f8 Binary files /dev/null and b/ia/objects.pdf differ diff --git a/ia/objects.svg b/ia/objects.svg new file mode 100644 index 0000000..1301394 --- /dev/null +++ b/ia/objects.svg @@ -0,0 +1,812 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + + + front_left middle middle_left middle_right front_right + + + + + + + + + + + + + + + + + + + + + + + + + front_left1 front_left2 middle_left3 middle_left2 middle_left1 middle_right1 middle_right2 middle_right3 front_right1 front_right1 rear_right stairs_right2 stairs_right1 stairs_left1 stairs_left2 rear_left Chargeur 1 2 3 4 Clap 1 Balle gauche droite + + + + + + 2 3 4 5 6 + diff --git a/ia/pathfinding.class.js b/ia/pathfinding.class.js new file mode 100644 index 0000000..c3192c7 --- /dev/null +++ b/ia/pathfinding.class.js @@ -0,0 +1,165 @@ +module.exports = (function () { + "use strict"; + + var Path = require('path'); + + var programm = Path.normalize("./pathfinding/bin/pathfinding"); + var image = Path.normalize("./pathfinding/img/map-20mm-yellow.bmp"); + var RATIO = 20; + var SEPARATOR = ";"; + + var logger = require('log4js').getLogger('ia.pathfinding'); + var Child_process = require('child_process'); + var Byline = require('byline'); + + function Pathfinding(ia) { + this.ia = ia; + var fifo = []; + + /*var instance_pkill = Child_process.spawn("pkill", ["pathfinding"]); + instance_pkill.on('error', function(err){ + logger.error("error pkilling pathfinding code:"+err.code); + }); + instance_pkill.on('exit', function(code){ + logger.info("successfully pkilled all old instances of pathfinding"); + }); + + + var instance = Child_process.spawn(programm, [ image ]);*/ + var instance = Child_process.spawn("bash", ["-c", "pkill pathfinding;"+programm+" "+image]); + + instance.on('error', function(err) { + if(err.code === 'ENOENT'){ + logger.fatal("pathfinding programm executable not found! Is it compiled ? :) Was looking in \""+Path.resolve(programm)+"\""); + process.exit(); + } + logger.error("c++ subprocess terminated with error:", err); + console.log(err); + }); + instance.on('exit', function(code) { + logger.fatal("c++ subprocess terminated with code:"+code); + }); + + + + process.on('exit', function(){ //ensure child process is killed + if(instance.connected){ //and was still connected (dont kill another process) + instance.kill(); + } + }); + + var stdout = Byline.createStream(instance.stdout); + stdout.setEncoding('utf8') + stdout.on('data', function(data) { + logger.debug("Received: "+data); + parse(data); + }); + + instance.stderr.on('data', function(data) { + logger.error("stderr :"+data.toString()); + }); + + + function vecMultiply(arr, ratio){ + return arr.map(function(val){ + return Math.round(val*ratio); + }); + } + + this.sendQuery = function(start, end, cb){ + fifo.push(cb || true); + + + var str = ["C"].concat( vecMultiply(start, 1/RATIO) ).concat( vecMultiply(end, 1/RATIO) ).join(SEPARATOR) + "\n"; + instance.stdin.write( str ); + logger.info("Sending:"+str); + }; + + this.sendDynamic = function(objects){ + //X0, Y0, R0, ... + var str = ["D"].concat(objects.reduce(function(acc, obj){ + return acc.concat( vecMultiply(obj, 1/RATIO) ); + }, [])).join(SEPARATOR) + "\n"; + // logger.debug(str); + instance.stdin.write(str); + } + + function parse (data) { + // X0; Y0; ... Xn; Yn + var ret = null; + if(data != "FAIL") { + var path = []; + var splitted = data.split(SEPARATOR); + while(splitted.length > 1){ + path.push( vecMultiply([splitted.shift(), splitted.shift()], RATIO) ); + } + + + if(path.length > 0) ret = path; + } + + var callback = fifo.shift(); + callback(ret); // if(typeof callback === "function") + } + + } + + Pathfinding.prototype.getPath = function (start, end, callback) { + this.ia.pathfinding.updateMap(); + this.timeout_getpath = setTimeout(function() { + callback(null); + callback = function() {}; + }, 1000); + this.sendQuery([start.x, start.y], [end.x, end.y], function(path){ + clearTimeout(this.timeout_getpath); + if(path !== null) { + path.shift(); + path = path.map(function(val) { + return { + x: val[0], + y: val[1] + }; + }); + } + callback(path); + }.bind(this)); + }; + + function borne(x, min, max) { + return x > max ? max : x < min ? min : x; + } + + //[ [x, y, r], ... ] + Pathfinding.prototype.updateMap = function () { + // var objects = []; + // objects.push(); + var objects = [{ + pos: this.ia.gr.pos, + d: this.ia.gr.size.d + }].concat(this.ia.data.dots).concat(this.ia.data.dynamic); + + + // logger.debug(objects); + + this.sendDynamic( objects.map(function(val){ + // logger.debug(val); + return [borne(val.pos.x, 0, 2980), borne(val.pos.y, 0, 1980), 1*((val.d/2)+(this.ia.pr.size.d/2))]; + }.bind(this)) ); + }; + + return Pathfinding; +})(); + +/* +(function(){ + var pathfinding = new module.exports(); + function log(result){ + console.log("RESULT", result); + process.exit(); + } + + setTimeout(function(){ + pathfinding.getPath([500,1000], [1500, 200], log); + }, 10); +})(); +//*/ diff --git a/ia/pr.class.js b/ia/pr.class.js new file mode 100644 index 0000000..7a0ea09 --- /dev/null +++ b/ia/pr.class.js @@ -0,0 +1,224 @@ +module.exports = (function () { + "use strict"; + var logger = require('log4js').getLogger('ia.pr'); + var GR_OFFSET = 110; + var PR_GR_COEF = 1; + + function Pr(ia, color) { + this.ia = ia; + this.pos = { // if we are yellow, default, left side of the table + x: 0, + y: 0, + a: 0 + }; + this.size = { + l: 170, + L: 220, + d: 280 + }; + this.current_action = null; + //this.path = null; + this.path = []; + this.content = { + nb_plots: 0, + gobelet:false + }; + this.we_have_hats = false; + this.color = color; + } + + Pr.prototype.loop = function () { + logger.debug('loop'); + this.ia.actions.doNextAction(function() { + this.loop(); + }.bind(this)); + }; + + Pr.prototype.collision = function() { + if(this.path.length === 0) { // Utile quand on clique nous même sur le bouton dans le simu + logger.warn("Normalement impossible, collision sur un path vide ?"); + //return; + } + + logger.info('collision'); + this.path = []; + this.ia.client.send('pr', "collision"); + this.ia.actions.collision(); + this.loop(); + } + Pr.prototype.stop = function() { + this.ia.client.send('pr', 'stop'); + } + + Pr.prototype.place = function () { + // logger.debug('place'); + this.sendInitialPos(); + this.ia.client.send('pr', 'placer'); + }; + + Pr.prototype.start = function () { + this.ia.client.send("pr", "ouvrir_ax12"); + this.loop(); + }; + + Pr.prototype.sendInitialPos = function () { + this.ia.client.send("pr", "setpos", { + x: 142, + y: 1000, + a: 0, + color: this.color + }); + }; + + function borne(x, min, max) { + return x > max ? max : x < min ? min : x; + } + + Pr.prototype.parseOrder = function (from, name, params) { + switch(name) { + case 'pr.collision': + this.collision(); + break; + // Asserv + case 'pr.pos': + params.x = borne(params.x, 0, 3000); + params.y = borne(params.y, 0, 2000); + this.pos = params; + break; + case 'pr.getpos': + this.sendInitialPos(); + break; + case 'pr.placer': + this.place(); + break; + case 'pr.plot++': + this.content.nb_plots += 1; + break; + case 'pr.plot0': + this.content.nb_plots = 0; + break; + case 'pr.gobelet1': + this.content.gobelet = true; + break; + case 'pr.gobelet0': + this.content.gobelet = false; + break; + case 'pr.hok': + this.updatePos(params); + break; + default: + logger.warn('Ordre inconnu dans ia.pr: '+name); + } + }; + + var SEGMENT_DELTA_D = 30; // (mm) between 2 iterations on a segment to detect colision + + Pr.prototype.getDistance = function (spot1, spot2) { + return Math.sqrt(Math.pow(spot1.x - spot2.x, 2) + Math.pow(spot1.y - spot2.y, 2)); + }; + + Pr.prototype.detectCollision = function(dots) { + var collision = false; + var pf = this.path; + var minDist; + // for each path segment + var complete_path = [this.pos].concat(this.path); + for (var i = 0; i < complete_path.length-1 && !collision; (i++) ) { + var segment = [complete_path[i], complete_path[i+1]]; // so segment[0][0] is the x value of the beginning of the segment + var segLength = this.getDistance({x:segment[0].x , y:segment[0].y }, {x:segment[1].x , y:segment[1].y }); + var nthX = (segment[1].x-segment[0].x)*SEGMENT_DELTA_D/segLength; // segment X axis length nth + var nthY = (segment[1].y-segment[0].y)*SEGMENT_DELTA_D/segLength; + + // for each X cm of the segment + for (var j = 0; (j*SEGMENT_DELTA_D) < segLength && !collision; (j++)) { + + var segPoint = { + x: segment[0].x + nthX*j, + y: segment[0].y + nthY*j + }; + + // distance to each estimated position of the ennemi robots + minDist = 10000;//this.getDistance(dots[0], segPoint); + + for(var k = 0; k < dots.length; k++) { + var tmp = this.getDistance(dots[k], segPoint); + if (tmp < minDist) { + minDist = tmp; + } + } + // if (ebots.length == 2) { + // var tmp = this.getDistance(ebots[1], segPoint); + // if (tmp < minDist) { + // minDist = tmp; + // } + // } + + // if one of the dist < security diameter, there will be a collision + if (minDist < 450) { + collision = true; + } + + } + } + + if (collision) { + this.collision(); + } + } + + Pr.prototype.updatePos = function(dots) { + if(this.ia.timer.match_started) { + + // Invert if green + if(this.color == "green") { + dots = dots.map(function(val) { + return [val[0], 2000-val[1]]; + }); + } + + // Delete our robots + for(var i in dots) { + if(this.norm(dots[i][0], dots[i][1], this.pos.x, this.pos.y) < 150 || + this.norm(dots[i][0], dots[i][1], this.ia.gr.pos.x, this.ia.gr.pos.y) < 150) + dots.splice(i, 1); + } + + // Check path + this.detectCollision(dots); + + // Update data + this.ia.data.dots = dots.map(function(val) { + return { + pos: { + x: val[0], + y: val[1], + }, + d: 320 + } + }); + if (dots.length > 0) { + this.ia.data.erobot[0].pos= { + x: dots[0][0], + y: dots[0][1] + }; + + if (dots.length > 1) { + this.ia.data.erobot[1].pos= { + x: dots[1][0], + y: dots[1][1] + }; + }; + }; + + // logger.fatal(dots); + // logger.fatal(this.pos); + // logger.fatal(this.ia.data.erobot); + } + }; + + Pr.prototype.norm = function(Ax, Ay, Bx, By) { + return Math.sqrt(Math.pow(Ax-Bx, 2) + Math.pow(Ay-By, 2)); + } + + return Pr; +})(); diff --git a/ia/timer.class.js b/ia/timer.class.js new file mode 100644 index 0000000..91225c5 --- /dev/null +++ b/ia/timer.class.js @@ -0,0 +1,36 @@ +module.exports = (function () { + "use strict"; + + function Timer(ia) { + this.t0 = null; + this.match_started = false; + this.ia = ia; + } + Timer.prototype.start = function() { + this.t0 = Date.now(); + this.match_started = true; // le match commence + setTimeout(function() { + logger.fatal("TIME OVER"); + this.ia.stop(); + }.bind(this), 89000); + }; + + + Timer.prototype.get = function () { // permet d'obtenir le temps écoulé en ms + if (this.match_started) + return Date.now() - this.t0; + else + return 0; + }; + Timer.prototype.status = function () { + if (this.match_started) { + return "Match started"; + } + else { + return "Wainting for jack"; + } + }; + + return Timer; +})(); + diff --git a/ia/utils/actionMap.html b/ia/utils/actionMap.html new file mode 100644 index 0000000..e76fb49 --- /dev/null +++ b/ia/utils/actionMap.html @@ -0,0 +1,15 @@ + + + + UTCoupe 15 | Action map + + + + + + + + + + + \ No newline at end of file diff --git a/ia/utils/actionMap.js b/ia/utils/actionMap.js new file mode 100644 index 0000000..fe12094 --- /dev/null +++ b/ia/utils/actionMap.js @@ -0,0 +1,79 @@ +(function () { + "use strict"; + /* + Shows a map of the differents Startpoints + + */ + + var canvas = $('#map'); + var ctx = canvas[0].getContext("2d"); + + function getColor(){ + var randR = Math.round(Math.random()*256).toString(16); + randR = (randR.length == 1)?("a"+ randR):randR; + var randG = Math.round(Math.random()*256).toString(16); + randG = (randG.length == 1)?("a"+ randG):randG; + var randB = Math.round(Math.random()*256).toString(16); + randB = (randB.length == 1)?("a"+ randB):randB; + return "#" + randR + randG + randB; + } + + function drawArrow(x, y, angle, color){ + /* + + --------------> 2 + | + | + | + \/ 1 + + */ + // var lside = [y-10, x-5]; + // var rside = [y-10, x+5]; + // var center = [y-4, x]; + x = 3*x; + y = 3*y; + var lside = [y - 12*Math.cos((angle + 30)*Math.PI/180), x + 12*Math.sin((angle + 30)*Math.PI/180)]; + var rside = [y - 12*Math.cos((angle - 30)*Math.PI/180), x + 12*Math.sin((angle - 30)*Math.PI/180)]; + var center = [y - 5*Math.cos(angle*Math.PI/180), x + 5*Math.sin(angle*Math.PI/180)]; + + ctx.beginPath(); + ctx.moveTo(y,x); + ctx.lineTo(lside[0], lside[1]); + ctx.lineTo(center[0], center[1]); + ctx.lineTo(rside[0], rside[1]); + ctx.lineTo(y,x); + ctx.fillStyle = color; + ctx.fill(); + ctx.closePath(); + } + + function drawMap(actions) { + // draw(50,50,"red"); + // drawArrow(120, 340, 0, getColor()); + var pt, color; + + Object.keys(actions).forEach(function(action_name) { + color = getColor(); + // console.log("Printing "+actions[action_name].startpoints.length+" sp"); + for(var i=0; i + + +Redirection + + + + + + + diff --git a/libs/arduino-1.0/arduinopc.jar b/libs/arduino-1.0/arduinopc.jar new file mode 100644 index 0000000..58a5b5c Binary files /dev/null and b/libs/arduino-1.0/arduinopc.jar differ diff --git a/libs/arduino-1.0/examples b/libs/arduino-1.0/examples new file mode 100644 index 0000000..7ff3f7d --- /dev/null +++ b/libs/arduino-1.0/examples @@ -0,0 +1 @@ +../doc/arduino-core/examples \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/boards.txt b/libs/arduino-1.0/hardware/arduino/boards.txt new file mode 100644 index 0000000..145d551 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/boards.txt @@ -0,0 +1,524 @@ +# See: http://code.google.com/p/arduino/wiki/Platforms + +############################################################## + +uno.name=Arduino Uno +uno.upload.protocol=arduino +uno.upload.maximum_size=32256 +uno.upload.speed=115200 +uno.bootloader.low_fuses=0xff +uno.bootloader.high_fuses=0xde +uno.bootloader.extended_fuses=0x05 +uno.bootloader.path=optiboot +uno.bootloader.file=optiboot_atmega328.hex +uno.bootloader.unlock_bits=0x3F +uno.bootloader.lock_bits=0x0F +uno.build.mcu=atmega328p +uno.build.f_cpu=16000000L +uno.build.core=arduino +uno.build.variant=standard + +############################################################## + +atmega328.name=Arduino Duemilanove w/ ATmega328 + +atmega328.upload.protocol=arduino +atmega328.upload.maximum_size=30720 +atmega328.upload.speed=57600 + +atmega328.bootloader.low_fuses=0xFF +atmega328.bootloader.high_fuses=0xDA +atmega328.bootloader.extended_fuses=0x05 +atmega328.bootloader.path=atmega +atmega328.bootloader.file=ATmegaBOOT_168_atmega328.hex +atmega328.bootloader.unlock_bits=0x3F +atmega328.bootloader.lock_bits=0x0F + +atmega328.build.mcu=atmega328p +atmega328.build.f_cpu=16000000L +atmega328.build.core=arduino +atmega328.build.variant=standard + +############################################################## + +diecimila.name=Arduino Diecimila or Duemilanove w/ ATmega168 + +diecimila.upload.protocol=arduino +diecimila.upload.maximum_size=14336 +diecimila.upload.speed=19200 + +diecimila.bootloader.low_fuses=0xff +diecimila.bootloader.high_fuses=0xdd +diecimila.bootloader.extended_fuses=0x00 +diecimila.bootloader.path=atmega +diecimila.bootloader.file=ATmegaBOOT_168_diecimila.hex +diecimila.bootloader.unlock_bits=0x3F +diecimila.bootloader.lock_bits=0x0F + +diecimila.build.mcu=atmega168 +diecimila.build.f_cpu=16000000L +diecimila.build.core=arduino +diecimila.build.variant=standard + +############################################################## + +nano328.name=Arduino Nano w/ ATmega328 + +nano328.upload.protocol=arduino +nano328.upload.maximum_size=30720 +nano328.upload.speed=57600 + +nano328.bootloader.low_fuses=0xFF +nano328.bootloader.high_fuses=0xDA +nano328.bootloader.extended_fuses=0x05 +nano328.bootloader.path=atmega +nano328.bootloader.file=ATmegaBOOT_168_atmega328.hex +nano328.bootloader.unlock_bits=0x3F +nano328.bootloader.lock_bits=0x0F + +nano328.build.mcu=atmega328p +nano328.build.f_cpu=16000000L +nano328.build.core=arduino +nano328.build.variant=eightanaloginputs + +############################################################## + +nano.name=Arduino Nano w/ ATmega168 + +nano.upload.protocol=arduino +nano.upload.maximum_size=14336 +nano.upload.speed=19200 + +nano.bootloader.low_fuses=0xff +nano.bootloader.high_fuses=0xdd +nano.bootloader.extended_fuses=0x00 +nano.bootloader.path=atmega +nano.bootloader.file=ATmegaBOOT_168_diecimila.hex +nano.bootloader.unlock_bits=0x3F +nano.bootloader.lock_bits=0x0F + +nano.build.mcu=atmega168 +nano.build.f_cpu=16000000L +nano.build.core=arduino +nano.build.variant=eightanaloginputs + +############################################################## + +mega2560.name=Arduino Mega 2560 or Mega ADK + +mega2560.upload.protocol=wiring +mega2560.upload.maximum_size=258048 +mega2560.upload.speed=115200 + +mega2560.bootloader.low_fuses=0xFF +mega2560.bootloader.high_fuses=0xD8 +mega2560.bootloader.extended_fuses=0xFD +mega2560.bootloader.path=stk500v2 +mega2560.bootloader.file=stk500boot_v2_mega2560.hex +mega2560.bootloader.unlock_bits=0x3F +mega2560.bootloader.lock_bits=0x0F + +mega2560.build.mcu=atmega2560 +mega2560.build.f_cpu=16000000L +mega2560.build.core=arduino +mega2560.build.variant=mega + +############################################################## + +mega.name=Arduino Mega (ATmega1280) + +mega.upload.protocol=arduino +mega.upload.maximum_size=126976 +mega.upload.speed=57600 + +mega.bootloader.low_fuses=0xFF +mega.bootloader.high_fuses=0xDA +mega.bootloader.extended_fuses=0xF5 +mega.bootloader.path=atmega +mega.bootloader.file=ATmegaBOOT_168_atmega1280.hex +mega.bootloader.unlock_bits=0x3F +mega.bootloader.lock_bits=0x0F + +mega.build.mcu=atmega1280 +mega.build.f_cpu=16000000L +mega.build.core=arduino +mega.build.variant=mega + +############################################################## + +leonardo.name=Arduino Leonardo +leonardo.upload.protocol=avr109 +leonardo.upload.maximum_size=28672 +leonardo.upload.speed=57600 +leonardo.upload.disable_flushing=true +leonardo.bootloader.low_fuses=0xff +leonardo.bootloader.high_fuses=0xd8 +leonardo.bootloader.extended_fuses=0xcb +leonardo.bootloader.path=caterina +leonardo.bootloader.file=Caterina-Leonardo.hex +leonardo.bootloader.unlock_bits=0x3F +leonardo.bootloader.lock_bits=0x2F +leonardo.build.mcu=atmega32u4 +leonardo.build.f_cpu=16000000L +leonardo.build.vid=0x2341 +leonardo.build.pid=0x8036 +leonardo.build.core=arduino +leonardo.build.variant=leonardo + +############################################################## + +esplora.name=Arduino Esplora +esplora.upload.protocol=avr109 +esplora.upload.maximum_size=28672 +esplora.upload.speed=57600 +esplora.upload.disable_flushing=true +esplora.bootloader.low_fuses=0xff +esplora.bootloader.high_fuses=0xd8 +esplora.bootloader.extended_fuses=0xcb +esplora.bootloader.path=caterina +esplora.bootloader.file=Caterina-Esplora.hex +esplora.bootloader.unlock_bits=0x3F +esplora.bootloader.lock_bits=0x2F +esplora.build.mcu=atmega32u4 +esplora.build.f_cpu=16000000L +esplora.build.vid=0x2341 +esplora.build.pid=0x803C +esplora.build.core=arduino +esplora.build.variant=leonardo + +############################################################## + +micro.name=Arduino Micro +micro.upload.protocol=avr109 +micro.upload.maximum_size=28672 +micro.upload.speed=57600 +micro.upload.disable_flushing=true +micro.bootloader.low_fuses=0xff +micro.bootloader.high_fuses=0xd8 +micro.bootloader.extended_fuses=0xcb +micro.bootloader.path=caterina +micro.bootloader.file=Caterina-Micro.hex +micro.bootloader.unlock_bits=0x3F +micro.bootloader.lock_bits=0x2F +micro.build.mcu=atmega32u4 +micro.build.f_cpu=16000000L +micro.build.vid=0x2341 +micro.build.pid=0x8037 +micro.build.core=arduino +micro.build.variant=micro + +############################################################## + +mini328.name=Arduino Mini w/ ATmega328 + +mini328.upload.protocol=arduino +mini328.upload.maximum_size=28672 +mini328.upload.speed=115200 + +mini328.bootloader.low_fuses=0xff +mini328.bootloader.high_fuses=0xd8 +mini328.bootloader.extended_fuses=0x05 +mini328.bootloader.path=optiboot +mini328.bootloader.file=optiboot_atmega328-Mini.hex +mini328.bootloader.unlock_bits=0x3F +mini328.bootloader.lock_bits=0x0F + +mini328.build.mcu=atmega328p +mini328.build.f_cpu=16000000L +mini328.build.core=arduino +mini328.build.variant=eightanaloginputs + +############################################################## + +mini.name=Arduino Mini w/ ATmega168 + +mini.upload.protocol=arduino +mini.upload.maximum_size=14336 +mini.upload.speed=19200 + +mini.bootloader.low_fuses=0xff +mini.bootloader.high_fuses=0xdd +mini.bootloader.extended_fuses=0x00 +mini.bootloader.path=atmega +mini.bootloader.file=ATmegaBOOT_168_ng.hex +mini.bootloader.unlock_bits=0x3F +mini.bootloader.lock_bits=0x0F + +mini.build.mcu=atmega168 +mini.build.f_cpu=16000000L +mini.build.core=arduino +mini.build.variant=eightanaloginputs + +############################################################## + +ethernet.name=Arduino Ethernet + +ethernet.upload.protocol=arduino +ethernet.upload.maximum_size=32256 +ethernet.upload.speed=115200 + +ethernet.bootloader.low_fuses=0xff +ethernet.bootloader.high_fuses=0xde +ethernet.bootloader.extended_fuses=0x05 +ethernet.bootloader.path=optiboot +ethernet.bootloader.file=optiboot_atmega328.hex +ethernet.bootloader.unlock_bits=0x3F +ethernet.bootloader.lock_bits=0x0F + +ethernet.build.variant=standard +ethernet.build.mcu=atmega328p +ethernet.build.f_cpu=16000000L +ethernet.build.core=arduino + +############################################################## + +fio.name=Arduino Fio + +fio.upload.protocol=arduino +fio.upload.maximum_size=30720 +fio.upload.speed=57600 + +fio.bootloader.low_fuses=0xFF +fio.bootloader.high_fuses=0xDA +fio.bootloader.extended_fuses=0x05 +fio.bootloader.path=arduino:atmega +fio.bootloader.file=ATmegaBOOT_168_atmega328_pro_8MHz.hex +fio.bootloader.unlock_bits=0x3F +fio.bootloader.lock_bits=0x0F + +fio.build.mcu=atmega328p +fio.build.f_cpu=8000000L +fio.build.core=arduino +fio.build.variant=eightanaloginputs + +############################################################## + +bt328.name=Arduino BT w/ ATmega328 + +bt328.upload.protocol=arduino +bt328.upload.maximum_size=28672 +bt328.upload.speed=19200 +bt328.upload.disable_flushing=true + +bt328.bootloader.low_fuses=0xff +bt328.bootloader.high_fuses=0xd8 +bt328.bootloader.extended_fuses=0x05 +bt328.bootloader.path=bt +bt328.bootloader.file=ATmegaBOOT_168_atmega328_bt.hex +bt328.bootloader.unlock_bits=0x3F +bt328.bootloader.lock_bits=0x0F + +bt328.build.mcu=atmega328p +bt328.build.f_cpu=16000000L +bt328.build.core=arduino +bt328.build.variant=eightanaloginputs + +############################################################## + +bt.name=Arduino BT w/ ATmega168 + +bt.upload.protocol=arduino +bt.upload.maximum_size=14336 +bt.upload.speed=19200 +bt.upload.disable_flushing=true + +bt.bootloader.low_fuses=0xff +bt.bootloader.high_fuses=0xdd +bt.bootloader.extended_fuses=0x00 +bt.bootloader.path=bt +bt.bootloader.file=ATmegaBOOT_168.hex +bt.bootloader.unlock_bits=0x3F +bt.bootloader.lock_bits=0x0F + +bt.build.mcu=atmega168 +bt.build.f_cpu=16000000L +bt.build.core=arduino +bt.build.variant=eightanaloginputs + +############################################################## + +LilyPadUSB.name=LilyPad Arduino USB +LilyPadUSB.upload.protocol=avr109 +LilyPadUSB.upload.maximum_size=28672 +LilyPadUSB.upload.speed=57600 +LilyPadUSB.upload.disable_flushing=true +LilyPadUSB.bootloader.low_fuses=0xff +LilyPadUSB.bootloader.high_fuses=0xd8 +LilyPadUSB.bootloader.extended_fuses=0xce +LilyPadUSB.bootloader.path=caterina-LilyPadUSB +LilyPadUSB.bootloader.file=Caterina-LilyPadUSB.hex +LilyPadUSB.bootloader.unlock_bits=0x3F +LilyPadUSB.bootloader.lock_bits=0x2F +LilyPadUSB.build.mcu=atmega32u4 +LilyPadUSB.build.f_cpu=8000000L +LilyPadUSB.build.vid=0x1B4F +LilyPadUSB.build.pid=0x9208 +LilyPadUSB.build.core=arduino +LilyPadUSB.build.variant=leonardo + +############################################################## + +lilypad328.name=LilyPad Arduino w/ ATmega328 + +lilypad328.upload.protocol=arduino +lilypad328.upload.maximum_size=30720 +lilypad328.upload.speed=57600 + +lilypad328.bootloader.low_fuses=0xFF +lilypad328.bootloader.high_fuses=0xDA +lilypad328.bootloader.extended_fuses=0x05 +lilypad328.bootloader.path=atmega +lilypad328.bootloader.file=ATmegaBOOT_168_atmega328_pro_8MHz.hex +lilypad328.bootloader.unlock_bits=0x3F +lilypad328.bootloader.lock_bits=0x0F + +lilypad328.build.mcu=atmega328p +lilypad328.build.f_cpu=8000000L +lilypad328.build.core=arduino +lilypad328.build.variant=standard + +############################################################## + +lilypad.name=LilyPad Arduino w/ ATmega168 + +lilypad.upload.protocol=arduino +lilypad.upload.maximum_size=14336 +lilypad.upload.speed=19200 + +lilypad.bootloader.low_fuses=0xe2 +lilypad.bootloader.high_fuses=0xdd +lilypad.bootloader.extended_fuses=0x00 +lilypad.bootloader.path=lilypad +lilypad.bootloader.file=LilyPadBOOT_168.hex +lilypad.bootloader.unlock_bits=0x3F +lilypad.bootloader.lock_bits=0x0F + +lilypad.build.mcu=atmega168 +lilypad.build.f_cpu=8000000L +lilypad.build.core=arduino +lilypad.build.variant=standard + +############################################################## + +pro5v328.name=Arduino Pro or Pro Mini (5V, 16 MHz) w/ ATmega328 + +pro5v328.upload.protocol=arduino +pro5v328.upload.maximum_size=30720 +pro5v328.upload.speed=57600 + +pro5v328.bootloader.low_fuses=0xFF +pro5v328.bootloader.high_fuses=0xDA +pro5v328.bootloader.extended_fuses=0x05 +pro5v328.bootloader.path=atmega +pro5v328.bootloader.file=ATmegaBOOT_168_atmega328.hex +pro5v328.bootloader.unlock_bits=0x3F +pro5v328.bootloader.lock_bits=0x0F + +pro5v328.build.mcu=atmega328p +pro5v328.build.f_cpu=16000000L +pro5v328.build.core=arduino +pro5v328.build.variant=standard + +############################################################## + +pro5v.name=Arduino Pro or Pro Mini (5V, 16 MHz) w/ ATmega168 + +pro5v.upload.protocol=arduino +pro5v.upload.maximum_size=14336 +pro5v.upload.speed=19200 + +pro5v.bootloader.low_fuses=0xff +pro5v.bootloader.high_fuses=0xdd +pro5v.bootloader.extended_fuses=0x00 +pro5v.bootloader.path=atmega +pro5v.bootloader.file=ATmegaBOOT_168_diecimila.hex +pro5v.bootloader.unlock_bits=0x3F +pro5v.bootloader.lock_bits=0x0F + +pro5v.build.mcu=atmega168 +pro5v.build.f_cpu=16000000L +pro5v.build.core=arduino +pro5v.build.variant=standard + +############################################################## + +pro328.name=Arduino Pro or Pro Mini (3.3V, 8 MHz) w/ ATmega328 + +pro328.upload.protocol=arduino +pro328.upload.maximum_size=30720 +pro328.upload.speed=57600 + +pro328.bootloader.low_fuses=0xFF +pro328.bootloader.high_fuses=0xDA +pro328.bootloader.extended_fuses=0x05 +pro328.bootloader.path=atmega +pro328.bootloader.file=ATmegaBOOT_168_atmega328_pro_8MHz.hex +pro328.bootloader.unlock_bits=0x3F +pro328.bootloader.lock_bits=0x0F + +pro328.build.mcu=atmega328p +pro328.build.f_cpu=8000000L +pro328.build.core=arduino +pro328.build.variant=standard + +############################################################## + +pro.name=Arduino Pro or Pro Mini (3.3V, 8 MHz) w/ ATmega168 + +pro.upload.protocol=arduino +pro.upload.maximum_size=14336 +pro.upload.speed=19200 + +pro.bootloader.low_fuses=0xc6 +pro.bootloader.high_fuses=0xdd +pro.bootloader.extended_fuses=0x00 +pro.bootloader.path=atmega +pro.bootloader.file=ATmegaBOOT_168_pro_8MHz.hex +pro.bootloader.unlock_bits=0x3F +pro.bootloader.lock_bits=0x0F + +pro.build.mcu=atmega168 +pro.build.f_cpu=8000000L +pro.build.core=arduino +pro.build.variant=standard + +############################################################## + +atmega168.name=Arduino NG or older w/ ATmega168 + +atmega168.upload.protocol=arduino +atmega168.upload.maximum_size=14336 +atmega168.upload.speed=19200 + +atmega168.bootloader.low_fuses=0xff +atmega168.bootloader.high_fuses=0xdd +atmega168.bootloader.extended_fuses=0x00 +atmega168.bootloader.path=atmega +atmega168.bootloader.file=ATmegaBOOT_168_ng.hex +atmega168.bootloader.unlock_bits=0x3F +atmega168.bootloader.lock_bits=0x0F + +atmega168.build.mcu=atmega168 +atmega168.build.f_cpu=16000000L +atmega168.build.core=arduino +atmega168.build.variant=standard + +############################################################## + +atmega8.name=Arduino NG or older w/ ATmega8 + +atmega8.upload.protocol=arduino +atmega8.upload.maximum_size=7168 +atmega8.upload.speed=19200 + +atmega8.bootloader.low_fuses=0xdf +atmega8.bootloader.high_fuses=0xca +atmega8.bootloader.path=atmega8 +atmega8.bootloader.file=ATmegaBOOT-prod-firmware-2009-11-07.hex +atmega8.bootloader.unlock_bits=0x3F +atmega8.bootloader.lock_bits=0x0F + +atmega8.build.mcu=atmega8 +atmega8.build.f_cpu=16000000L +atmega8.build.core=arduino +atmega8.build.variant=standard diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/ATmegaBOOT_168.c b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/ATmegaBOOT_168.c new file mode 100644 index 0000000..e8c3fb8 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/ATmegaBOOT_168.c @@ -0,0 +1,1054 @@ +/**********************************************************/ +/* Serial Bootloader for Atmel megaAVR Controllers */ +/* */ +/* tested with ATmega8, ATmega128 and ATmega168 */ +/* should work with other mega's, see code for details */ +/* */ +/* ATmegaBOOT.c */ +/* */ +/* */ +/* 20090308: integrated Mega changes into main bootloader */ +/* source by D. Mellis */ +/* 20080930: hacked for Arduino Mega (with the 1280 */ +/* processor, backwards compatible) */ +/* by D. Cuartielles */ +/* 20070626: hacked for Arduino Diecimila (which auto- */ +/* resets when a USB connection is made to it) */ +/* by D. Mellis */ +/* 20060802: hacked for Arduino by D. Cuartielles */ +/* based on a previous hack by D. Mellis */ +/* and D. Cuartielles */ +/* */ +/* Monitor and debug functions were added to the original */ +/* code by Dr. Erik Lins, chip45.com. (See below) */ +/* */ +/* Thanks to Karl Pitrich for fixing a bootloader pin */ +/* problem and more informative LED blinking! */ +/* */ +/* For the latest version see: */ +/* http://www.chip45.com/ */ +/* */ +/* ------------------------------------------------------ */ +/* */ +/* based on stk500boot.c */ +/* Copyright (c) 2003, Jason P. Kyle */ +/* All rights reserved. */ +/* see avr1.org for original file and information */ +/* */ +/* This program is free software; you can redistribute it */ +/* and/or modify it under the terms of the GNU General */ +/* Public License as published by the Free Software */ +/* Foundation; either version 2 of the License, or */ +/* (at your option) any later version. */ +/* */ +/* This program is distributed in the hope that it will */ +/* be useful, but WITHOUT ANY WARRANTY; without even the */ +/* implied warranty of MERCHANTABILITY or FITNESS FOR A */ +/* PARTICULAR PURPOSE. See the GNU General Public */ +/* License for more details. */ +/* */ +/* You should have received a copy of the GNU General */ +/* Public License along with this program; if not, write */ +/* to the Free Software Foundation, Inc., */ +/* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* */ +/* Licence can be viewed at */ +/* http://www.fsf.org/licenses/gpl.txt */ +/* */ +/* Target = Atmel AVR m128,m64,m32,m16,m8,m162,m163,m169, */ +/* m8515,m8535. ATmega161 has a very small boot block so */ +/* isn't supported. */ +/* */ +/* Tested with m168 */ +/**********************************************************/ + +/* $Id$ */ + + +/* some includes */ +#include +#include +#include +#include +#include +#include + +/* the current avr-libc eeprom functions do not support the ATmega168 */ +/* own eeprom write/read functions are used instead */ +#if !defined(__AVR_ATmega168__) || !defined(__AVR_ATmega328P__) +#include +#endif + +/* Use the F_CPU defined in Makefile */ + +/* 20060803: hacked by DojoCorp */ +/* 20070626: hacked by David A. Mellis to decrease waiting time for auto-reset */ +/* set the waiting time for the bootloader */ +/* get this from the Makefile instead */ +/* #define MAX_TIME_COUNT (F_CPU>>4) */ + +/* 20070707: hacked by David A. Mellis - after this many errors give up and launch application */ +#define MAX_ERROR_COUNT 5 + +/* set the UART baud rate */ +/* 20060803: hacked by DojoCorp */ +//#define BAUD_RATE 115200 +#ifndef BAUD_RATE +#define BAUD_RATE 19200 +#endif + + +/* SW_MAJOR and MINOR needs to be updated from time to time to avoid warning message from AVR Studio */ +/* never allow AVR Studio to do an update !!!! */ +#define HW_VER 0x02 +#define SW_MAJOR 0x01 +#define SW_MINOR 0x10 + + +/* Adjust to suit whatever pin your hardware uses to enter the bootloader */ +/* ATmega128 has two UARTS so two pins are used to enter bootloader and select UART */ +/* ATmega1280 has four UARTS, but for Arduino Mega, we will only use RXD0 to get code */ +/* BL0... means UART0, BL1... means UART1 */ +#ifdef __AVR_ATmega128__ +#define BL_DDR DDRF +#define BL_PORT PORTF +#define BL_PIN PINF +#define BL0 PINF7 +#define BL1 PINF6 +#elif defined __AVR_ATmega1280__ +/* we just don't do anything for the MEGA and enter bootloader on reset anyway*/ +#else +/* other ATmegas have only one UART, so only one pin is defined to enter bootloader */ +#define BL_DDR DDRD +#define BL_PORT PORTD +#define BL_PIN PIND +#define BL PIND6 +#endif + + +/* onboard LED is used to indicate, that the bootloader was entered (3x flashing) */ +/* if monitor functions are included, LED goes on after monitor was entered */ +#if defined __AVR_ATmega128__ || defined __AVR_ATmega1280__ +/* Onboard LED is connected to pin PB7 (e.g. Crumb128, PROBOmega128, Savvy128, Arduino Mega) */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB7 +#else +/* Onboard LED is connected to pin PB5 in Arduino NG, Diecimila, and Duomilanuove */ +/* other boards like e.g. Crumb8, Crumb168 are using PB2 */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB5 +#endif + + +/* monitor functions will only be compiled when using ATmega128, due to bootblock size constraints */ +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) +#define MONITOR 1 +#endif + + +/* define various device id's */ +/* manufacturer byte is always the same */ +#define SIG1 0x1E // Yep, Atmel is the only manufacturer of AVR micros. Single source :( + +#if defined __AVR_ATmega1280__ +#define SIG2 0x97 +#define SIG3 0x03 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega1281__ +#define SIG2 0x97 +#define SIG3 0x04 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega128__ +#define SIG2 0x97 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega64__ +#define SIG2 0x96 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega32__ +#define SIG2 0x95 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega16__ +#define SIG2 0x94 +#define SIG3 0x03 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8__ +#define SIG2 0x93 +#define SIG3 0x07 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega88__ +#define SIG2 0x93 +#define SIG3 0x0a +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega168__ +#define SIG2 0x94 +#define SIG3 0x06 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega328P__ +#define SIG2 0x95 +#define SIG3 0x0F +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega162__ +#define SIG2 0x94 +#define SIG3 0x04 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega163__ +#define SIG2 0x94 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega169__ +#define SIG2 0x94 +#define SIG3 0x05 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8515__ +#define SIG2 0x93 +#define SIG3 0x06 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega8535__ +#define SIG2 0x93 +#define SIG3 0x08 +#define PAGE_SIZE 0x20U //32 words +#endif + + +/* function prototypes */ +void putch(char); +char getch(void); +void getNch(uint8_t); +void byte_response(uint8_t); +void nothing_response(void); +char gethex(void); +void puthex(char); +void flash_led(uint8_t); + +/* some variables */ +union address_union { + uint16_t word; + uint8_t byte[2]; +} address; + +union length_union { + uint16_t word; + uint8_t byte[2]; +} length; + +struct flags_struct { + unsigned eeprom : 1; + unsigned rampz : 1; +} flags; + +uint8_t buff[256]; +uint8_t address_high; + +uint8_t pagesz=0x80; + +uint8_t i; +uint8_t bootuart = 0; + +uint8_t error_count = 0; + +void (*app_start)(void) = 0x0000; + + +/* main program starts here */ +int main(void) +{ + uint8_t ch,ch2; + uint16_t w; + +#ifdef WATCHDOG_MODS + ch = MCUSR; + MCUSR = 0; + + WDTCSR |= _BV(WDCE) | _BV(WDE); + WDTCSR = 0; + + // Check if the WDT was used to reset, in which case we dont bootload and skip straight to the code. woot. + if (! (ch & _BV(EXTRF))) // if its a not an external reset... + app_start(); // skip bootloader +#else + asm volatile("nop\n\t"); +#endif + + /* set pin direction for bootloader pin and enable pullup */ + /* for ATmega128, two pins need to be initialized */ +#ifdef __AVR_ATmega128__ + BL_DDR &= ~_BV(BL0); + BL_DDR &= ~_BV(BL1); + BL_PORT |= _BV(BL0); + BL_PORT |= _BV(BL1); +#else + /* We run the bootloader regardless of the state of this pin. Thus, don't + put it in a different state than the other pins. --DAM, 070709 + This also applies to Arduino Mega -- DC, 080930 + BL_DDR &= ~_BV(BL); + BL_PORT |= _BV(BL); + */ +#endif + + +#ifdef __AVR_ATmega128__ + /* check which UART should be used for booting */ + if(bit_is_clear(BL_PIN, BL0)) { + bootuart = 1; + } + else if(bit_is_clear(BL_PIN, BL1)) { + bootuart = 2; + } +#endif + +#if defined __AVR_ATmega1280__ + /* the mega1280 chip has four serial ports ... we could eventually use any of them, or not? */ + /* however, we don't wanna confuse people, to avoid making a mess, we will stick to RXD0, TXD0 */ + bootuart = 1; +#endif + + /* check if flash is programmed already, if not start bootloader anyway */ + if(pgm_read_byte_near(0x0000) != 0xFF) { + +#ifdef __AVR_ATmega128__ + /* no UART was selected, start application */ + if(!bootuart) { + app_start(); + } +#else + /* check if bootloader pin is set low */ + /* we don't start this part neither for the m8, nor m168 */ + //if(bit_is_set(BL_PIN, BL)) { + // app_start(); + // } +#endif + } + +#ifdef __AVR_ATmega128__ + /* no bootuart was selected, default to uart 0 */ + if(!bootuart) { + bootuart = 1; + } +#endif + + + /* initialize UART(s) depending on CPU defined */ +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + if(bootuart == 1) { + UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR0A = 0x00; + UCSR0C = 0x06; + UCSR0B = _BV(TXEN0)|_BV(RXEN0); + } + if(bootuart == 2) { + UBRR1L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR1H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR1A = 0x00; + UCSR1C = 0x06; + UCSR1B = _BV(TXEN1)|_BV(RXEN1); + } +#elif defined __AVR_ATmega163__ + UBRR = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRRHI = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSRA = 0x00; + UCSRB = _BV(TXEN)|_BV(RXEN); +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + +#ifdef DOUBLE_SPEED + UCSR0A = (1<> 8; +#else + UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; +#endif + + UCSR0B = (1<>8; // set baud rate + UBRRL = (((F_CPU/BAUD_RATE)/16)-1); + UCSRB = (1<> 8; + UCSRA = 0x00; + UCSRC = 0x06; + UCSRB = _BV(TXEN)|_BV(RXEN); +#endif + +#if defined __AVR_ATmega1280__ + /* Enable internal pull-up resistor on pin D0 (RX), in order + to supress line noise that prevents the bootloader from + timing out (DAM: 20070509) */ + /* feature added to the Arduino Mega --DC: 080930 */ + DDRE &= ~_BV(PINE0); + PORTE |= _BV(PINE0); +#endif + + + /* set LED pin as output */ + LED_DDR |= _BV(LED); + + + /* flash onboard LED to signal entering of bootloader */ +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + // 4x for UART0, 5x for UART1 + flash_led(NUM_LED_FLASHES + bootuart); +#else + flash_led(NUM_LED_FLASHES); +#endif + + /* 20050803: by DojoCorp, this is one of the parts provoking the + system to stop listening, cancelled from the original */ + //putch('\0'); + + /* forever loop */ + for (;;) { + + /* get character from UART */ + ch = getch(); + + /* A bunch of if...else if... gives smaller code than switch...case ! */ + + /* Hello is anyone home ? */ + if(ch=='0') { + nothing_response(); + } + + + /* Request programmer ID */ + /* Not using PROGMEM string due to boot block in m128 being beyond 64kB boundry */ + /* Would need to selectively manipulate RAMPZ, and it's only 9 characters anyway so who cares. */ + else if(ch=='1') { + if (getch() == ' ') { + putch(0x14); + putch('A'); + putch('V'); + putch('R'); + putch(' '); + putch('I'); + putch('S'); + putch('P'); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } + } + + + /* AVR ISP/STK500 board commands DON'T CARE so default nothing_response */ + else if(ch=='@') { + ch2 = getch(); + if (ch2>0x85) getch(); + nothing_response(); + } + + + /* AVR ISP/STK500 board requests */ + else if(ch=='A') { + ch2 = getch(); + if(ch2==0x80) byte_response(HW_VER); // Hardware version + else if(ch2==0x81) byte_response(SW_MAJOR); // Software major version + else if(ch2==0x82) byte_response(SW_MINOR); // Software minor version + else if(ch2==0x98) byte_response(0x03); // Unknown but seems to be required by avr studio 3.56 + else byte_response(0x00); // Covers various unnecessary responses we don't care about + } + + + /* Device Parameters DON'T CARE, DEVICE IS FIXED */ + else if(ch=='B') { + getNch(20); + nothing_response(); + } + + + /* Parallel programming stuff DON'T CARE */ + else if(ch=='E') { + getNch(5); + nothing_response(); + } + + + /* P: Enter programming mode */ + /* R: Erase device, don't care as we will erase one page at a time anyway. */ + else if(ch=='P' || ch=='R') { + nothing_response(); + } + + + /* Leave programming mode */ + else if(ch=='Q') { + nothing_response(); +#ifdef WATCHDOG_MODS + // autoreset via watchdog (sneaky!) + WDTCSR = _BV(WDE); + while (1); // 16 ms +#endif + } + + + /* Set address, little endian. EEPROM in bytes, FLASH in words */ + /* Perhaps extra address bytes may be added in future to support > 128kB FLASH. */ + /* This might explain why little endian was used here, big endian used everywhere else. */ + else if(ch=='U') { + address.byte[0] = getch(); + address.byte[1] = getch(); + nothing_response(); + } + + + /* Universal SPI programming command, disabled. Would be used for fuses and lock bits. */ + else if(ch=='V') { + if (getch() == 0x30) { + getch(); + ch = getch(); + getch(); + if (ch == 0) { + byte_response(SIG1); + } else if (ch == 1) { + byte_response(SIG2); + } else { + byte_response(SIG3); + } + } else { + getNch(3); + byte_response(0x00); + } + } + + + /* Write memory, length is big endian and is in bytes */ + else if(ch=='d') { + length.byte[1] = getch(); + length.byte[0] = getch(); + flags.eeprom = 0; + if (getch() == 'E') flags.eeprom = 1; + for (w=0;w127) address_high = 0x01; //Only possible with m128, m256 will need 3rd address byte. FIXME + else address_high = 0x00; +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega1281__) + RAMPZ = address_high; +#endif + address.word = address.word << 1; //address * 2 -> byte location + /* if ((length.byte[0] & 0x01) == 0x01) length.word++; //Even up an odd number of bytes */ + if ((length.byte[0] & 0x01)) length.word++; //Even up an odd number of bytes + cli(); //Disable interrupts, just to be sure +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega1281__) + while(bit_is_set(EECR,EEPE)); //Wait for previous EEPROM writes to complete +#else + while(bit_is_set(EECR,EEWE)); //Wait for previous EEPROM writes to complete +#endif + asm volatile( + "clr r17 \n\t" //page_word_count + "lds r30,address \n\t" //Address of FLASH location (in bytes) + "lds r31,address+1 \n\t" + "ldi r28,lo8(buff) \n\t" //Start of buffer array in RAM + "ldi r29,hi8(buff) \n\t" + "lds r24,length \n\t" //Length of data to be written (in bytes) + "lds r25,length+1 \n\t" + "length_loop: \n\t" //Main loop, repeat for number of words in block + "cpi r17,0x00 \n\t" //If page_word_count=0 then erase page + "brne no_page_erase \n\t" + "wait_spm1: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm1 \n\t" + "ldi r16,0x03 \n\t" //Erase page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "wait_spm2: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm2 \n\t" + + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "no_page_erase: \n\t" + "ld r0,Y+ \n\t" //Write 2 bytes into page buffer + "ld r1,Y+ \n\t" + + "wait_spm3: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm3 \n\t" + "ldi r16,0x01 \n\t" //Load r0,r1 into FLASH page buffer + "sts %0,r16 \n\t" + "spm \n\t" + + "inc r17 \n\t" //page_word_count++ + "cpi r17,%1 \n\t" + "brlo same_page \n\t" //Still same page in FLASH + "write_page: \n\t" + "clr r17 \n\t" //New page, write current one first + "wait_spm4: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm4 \n\t" +#ifdef __AVR_ATmega163__ + "andi r30,0x80 \n\t" // m163 requires Z6:Z1 to be zero during page write +#endif + "ldi r16,0x05 \n\t" //Write page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" + "ori r30,0x7E \n\t" // recover Z6:Z1 state after page write (had to be zero during write) +#endif + "wait_spm5: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm5 \n\t" + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "same_page: \n\t" + "adiw r30,2 \n\t" //Next word in FLASH + "sbiw r24,2 \n\t" //length-2 + "breq final_write \n\t" //Finished + "rjmp length_loop \n\t" + "final_write: \n\t" + "cpi r17,0 \n\t" + "breq block_done \n\t" + "adiw r24,2 \n\t" //length+2, fool above check on length after short page write + "rjmp write_page \n\t" + "block_done: \n\t" + "clr __zero_reg__ \n\t" //restore zero register +#if defined __AVR_ATmega168__ || __AVR_ATmega328P__ || __AVR_ATmega128__ || __AVR_ATmega1280__ || __AVR_ATmega1281__ + : "=m" (SPMCSR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#else + : "=m" (SPMCR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#endif + ); + /* Should really add a wait for RWW section to be enabled, don't actually need it since we never */ + /* exit the bootloader without a power cycle anyhow */ + } + putch(0x14); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } + } + + + /* Read memory block mode, length is big endian. */ + else if(ch=='t') { + length.byte[1] = getch(); + length.byte[0] = getch(); +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + if (address.word>0x7FFF) flags.rampz = 1; // No go with m256, FIXME + else flags.rampz = 0; +#endif + address.word = address.word << 1; // address * 2 -> byte location + if (getch() == 'E') flags.eeprom = 1; + else flags.eeprom = 0; + if (getch() == ' ') { // Command terminator + putch(0x14); + for (w=0;w < length.word;w++) { // Can handle odd and even lengths okay + if (flags.eeprom) { // Byte access EEPROM read +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + while(EECR & (1<= 'a') { + return (a - 'a' + 0x0a); + } else if(a >= '0') { + return(a - '0'); + } + return a; +} + + +char gethex(void) { + return (gethexnib() << 4) + gethexnib(); +} + + +void puthex(char ch) { + char ah; + + ah = ch >> 4; + if(ah >= 0x0a) { + ah = ah - 0x0a + 'a'; + } else { + ah += '0'; + } + + ch &= 0x0f; + if(ch >= 0x0a) { + ch = ch - 0x0a + 'a'; + } else { + ch += '0'; + } + + putch(ah); + putch(ch); +} + + +void putch(char ch) +{ +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + if(bootuart == 1) { + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; + } + else if (bootuart == 2) { + while (!(UCSR1A & _BV(UDRE1))); + UDR1 = ch; + } +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; +#else + /* m8,16,32,169,8515,8535,163 */ + while (!(UCSRA & _BV(UDRE))); + UDR = ch; +#endif +} + + +char getch(void) +{ +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + uint32_t count = 0; + if(bootuart == 1) { + while(!(UCSR0A & _BV(RXC0))) { + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + + return UDR0; + } + else if(bootuart == 2) { + while(!(UCSR1A & _BV(RXC1))) { + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + + return UDR1; + } + return 0; +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + uint32_t count = 0; + while(!(UCSR0A & _BV(RXC0))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR0; +#else + /* m8,16,32,169,8515,8535,163 */ + uint32_t count = 0; + while(!(UCSRA & _BV(RXC))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR; +#endif +} + + +void getNch(uint8_t count) +{ + while(count--) { +#if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) + if(bootuart == 1) { + while(!(UCSR0A & _BV(RXC0))); + UDR0; + } + else if(bootuart == 2) { + while(!(UCSR1A & _BV(RXC1))); + UDR1; + } +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + getch(); +#else + /* m8,16,32,169,8515,8535,163 */ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + //while(!(UCSRA & _BV(RXC))); + //UDR; + getch(); // need to handle time out +#endif + } +} + + +void byte_response(uint8_t val) +{ + if (getch() == ' ') { + putch(0x14); + putch(val); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } +} + + +void nothing_response(void) +{ + if (getch() == ' ') { + putch(0x14); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } +} + +void flash_led(uint8_t count) +{ + while (count--) { + LED_PORT |= _BV(LED); + _delay_ms(100); + LED_PORT &= ~_BV(LED); + _delay_ms(100); + } +} + + +/* end of file ATmegaBOOT.c */ diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/Makefile b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/Makefile new file mode 100644 index 0000000..0fd54db --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega/Makefile @@ -0,0 +1,224 @@ +# Makefile for ATmegaBOOT +# E.Lins, 18.7.2005 +# $Id$ +# +# Instructions +# +# To make bootloader .hex file: +# make diecimila +# make lilypad +# make ng +# etc... +# +# To burn bootloader .hex file: +# make diecimila_isp +# make lilypad_isp +# make ng_isp +# etc... + +# program name should not be changed... +PROGRAM = ATmegaBOOT_168 + +# enter the parameters for the avrdude isp tool +ISPTOOL = stk500v2 +ISPPORT = usb +ISPSPEED = -b 115200 + +MCU_TARGET = atmega168 +LDSECTION = --section-start=.text=0x3800 + +# the efuse should really be 0xf8; since, however, only the lower +# three bits of that byte are used on the atmega168, avrdude gets +# confused if you specify 1's for the higher bits, see: +# http://tinker.it/now/2007/02/24/the-tale-of-avrdude-atmega168-and-extended-bits-fuses/ +# +# similarly, the lock bits should be 0xff instead of 0x3f (to +# unlock the bootloader section) and 0xcf instead of 0x0f (to +# lock it), but since the high two bits of the lock byte are +# unused, avrdude would get confused. + +ISPFUSES = avrdude -c $(ISPTOOL) -p $(MCU_TARGET) -P $(ISPPORT) $(ISPSPEED) \ +-e -u -U lock:w:0x3f:m -U efuse:w:0x$(EFUSE):m -U hfuse:w:0x$(HFUSE):m -U lfuse:w:0x$(LFUSE):m +ISPFLASH = avrdude -c $(ISPTOOL) -p $(MCU_TARGET) -P $(ISPPORT) $(ISPSPEED) \ +-U flash:w:$(PROGRAM)_$(TARGET).hex -U lock:w:0x0f:m + +STK500 = "C:\Program Files\Atmel\AVR Tools\STK500\Stk500.exe" +STK500-1 = $(STK500) -e -d$(MCU_TARGET) -pf -vf -if$(PROGRAM)_$(TARGET).hex \ +-lFF -LFF -f$(HFUSE)$(LFUSE) -EF8 -ms -q -cUSB -I200kHz -s -wt +STK500-2 = $(STK500) -d$(MCU_TARGET) -ms -q -lCF -LCF -cUSB -I200kHz -s -wt + + +OBJ = $(PROGRAM).o +OPTIMIZE = -O2 + +DEFS = +LIBS = + +CC = avr-gcc + +# Override is only needed by avr-lib build system. + +override CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) -DF_CPU=$(AVR_FREQ) $(DEFS) +override LDFLAGS = -Wl,$(LDSECTION) +#override LDFLAGS = -Wl,-Map,$(PROGRAM).map,$(LDSECTION) + +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump + +all: + +lilypad: TARGET = lilypad +lilypad: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>1' '-DNUM_LED_FLASHES=3' +lilypad: AVR_FREQ = 8000000L +lilypad: $(PROGRAM)_lilypad.hex + +lilypad_isp: lilypad +lilypad_isp: TARGET = lilypad +lilypad_isp: HFUSE = DD +lilypad_isp: LFUSE = E2 +lilypad_isp: EFUSE = 00 +lilypad_isp: isp + +lilypad_resonator: TARGET = lilypad_resonator +lilypad_resonator: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=3' +lilypad_resonator: AVR_FREQ = 8000000L +lilypad_resonator: $(PROGRAM)_lilypad_resonator.hex + +lilypad_resonator_isp: lilypad_resonator +lilypad_resonator_isp: TARGET = lilypad_resonator +lilypad_resonator_isp: HFUSE = DD +lilypad_resonator_isp: LFUSE = C6 +lilypad_resonator_isp: EFUSE = 00 +lilypad_resonator_isp: isp + +pro8: TARGET = pro_8MHz +pro8: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' '-DWATCHDOG_MODS' +pro8: AVR_FREQ = 8000000L +pro8: $(PROGRAM)_pro_8MHz.hex + +pro8_isp: pro8 +pro8_isp: TARGET = pro_8MHz +pro8_isp: HFUSE = DD +pro8_isp: LFUSE = C6 +pro8_isp: EFUSE = 00 +pro8_isp: isp + +pro16: TARGET = pro_16MHz +pro16: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' '-DWATCHDOG_MODS' +pro16: AVR_FREQ = 16000000L +pro16: $(PROGRAM)_pro_16MHz.hex + +pro16_isp: pro16 +pro16_isp: TARGET = pro_16MHz +pro16_isp: HFUSE = DD +pro16_isp: LFUSE = C6 +pro16_isp: EFUSE = 00 +pro16_isp: isp + +pro20: TARGET = pro_20mhz +pro20: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' '-DWATCHDOG_MODS' +pro20: AVR_FREQ = 20000000L +pro20: $(PROGRAM)_pro_20mhz.hex + +pro20_isp: pro20 +pro20_isp: TARGET = pro_20mhz +pro20_isp: HFUSE = DD +pro20_isp: LFUSE = C6 +pro20_isp: EFUSE = 00 +pro20_isp: isp + +diecimila: TARGET = diecimila +diecimila: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' +diecimila: AVR_FREQ = 16000000L +diecimila: $(PROGRAM)_diecimila.hex + +diecimila_isp: diecimila +diecimila_isp: TARGET = diecimila +diecimila_isp: HFUSE = DD +diecimila_isp: LFUSE = FF +diecimila_isp: EFUSE = 00 +diecimila_isp: isp + +ng: TARGET = ng +ng: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>1' '-DNUM_LED_FLASHES=3' +ng: AVR_FREQ = 16000000L +ng: $(PROGRAM)_ng.hex + +ng_isp: ng +ng_isp: TARGET = ng +ng_isp: HFUSE = DD +ng_isp: LFUSE = FF +ng_isp: EFUSE = 00 +ng_isp: isp + +atmega328: TARGET = atmega328 +atmega328: MCU_TARGET = atmega328p +atmega328: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' -DBAUD_RATE=57600 +atmega328: AVR_FREQ = 16000000L +atmega328: LDSECTION = --section-start=.text=0x7800 +atmega328: $(PROGRAM)_atmega328.hex + +atmega328_isp: atmega328 +atmega328_isp: TARGET = atmega328 +atmega328_isp: MCU_TARGET = atmega328p +atmega328_isp: HFUSE = DA +atmega328_isp: LFUSE = FF +atmega328_isp: EFUSE = 05 +atmega328_isp: isp + +atmega328_pro8: TARGET = atmega328_pro_8MHz +atmega328_pro8: MCU_TARGET = atmega328p +atmega328_pro8: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=1' -DBAUD_RATE=57600 -DDOUBLE_SPEED +atmega328_pro8: AVR_FREQ = 8000000L +atmega328_pro8: LDSECTION = --section-start=.text=0x7800 +atmega328_pro8: $(PROGRAM)_atmega328_pro_8MHz.hex + +atmega328_pro8_isp: atmega328_pro8 +atmega328_pro8_isp: TARGET = atmega328_pro_8MHz +atmega328_pro8_isp: MCU_TARGET = atmega328p +atmega328_pro8_isp: HFUSE = DA +atmega328_pro8_isp: LFUSE = FF +atmega328_pro8_isp: EFUSE = 05 +atmega328_pro8_isp: isp + +mega: TARGET = atmega1280 +mega: MCU_TARGET = atmega1280 +mega: CFLAGS += '-DMAX_TIME_COUNT=F_CPU>>4' '-DNUM_LED_FLASHES=0' -DBAUD_RATE=57600 +mega: AVR_FREQ = 16000000L +mega: LDSECTION = --section-start=.text=0x1F000 +mega: $(PROGRAM)_atmega1280.hex + +mega_isp: mega +mega_isp: TARGET = atmega1280 +mega_isp: MCU_TARGET = atmega1280 +mega_isp: HFUSE = DA +mega_isp: LFUSE = FF +mega_isp: EFUSE = F5 +mega_isp: isp + +isp: $(TARGET) + $(ISPFUSES) + $(ISPFLASH) + +isp-stk500: $(PROGRAM)_$(TARGET).hex + $(STK500-1) + $(STK500-2) + +%.elf: $(OBJ) + $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) + +clean: + rm -rf *.o *.elf *.lst *.map *.sym *.lss *.eep *.srec *.bin *.hex + +%.lst: %.elf + $(OBJDUMP) -h -S $< > $@ + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/atmega8/ATmegaBOOT.c b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega8/ATmegaBOOT.c new file mode 100644 index 0000000..8c8d22a --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/atmega8/ATmegaBOOT.c @@ -0,0 +1,507 @@ +/**********************************************************/ +/* Serial Bootloader for Atmel mega8 AVR Controller */ +/* */ +/* ATmegaBOOT.c */ +/* */ +/* Copyright (c) 2003, Jason P. Kyle */ +/* */ +/* Hacked by DojoCorp - ZGZ - MMX - IVR */ +/* Hacked by David A. Mellis */ +/* */ +/* This program is free software; you can redistribute it */ +/* and/or modify it under the terms of the GNU General */ +/* Public License as published by the Free Software */ +/* Foundation; either version 2 of the License, or */ +/* (at your option) any later version. */ +/* */ +/* This program is distributed in the hope that it will */ +/* be useful, but WITHOUT ANY WARRANTY; without even the */ +/* implied warranty of MERCHANTABILITY or FITNESS FOR A */ +/* PARTICULAR PURPOSE. See the GNU General Public */ +/* License for more details. */ +/* */ +/* You should have received a copy of the GNU General */ +/* Public License along with this program; if not, write */ +/* to the Free Software Foundation, Inc., */ +/* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* */ +/* Licence can be viewed at */ +/* http://www.fsf.org/licenses/gpl.txt */ +/* */ +/* Target = Atmel AVR m8 */ +/**********************************************************/ + +#include +#include +#include +#include +#include +#include + +//#define F_CPU 16000000 + +/* We, Malmoitians, like slow interaction + * therefore the slow baud rate ;-) + */ +//#define BAUD_RATE 9600 + +/* 6.000.000 is more or less 8 seconds at the + * speed configured here + */ +//#define MAX_TIME_COUNT 6000000 +#define MAX_TIME_COUNT (F_CPU>>1) +///#define MAX_TIME_COUNT_MORATORY 1600000 + +/* SW_MAJOR and MINOR needs to be updated from time to time to avoid warning message from AVR Studio */ +#define HW_VER 0x02 +#define SW_MAJOR 0x01 +#define SW_MINOR 0x12 + +// AVR-GCC compiler compatibility +// avr-gcc compiler v3.1.x and older doesn't support outb() and inb() +// if necessary, convert outb and inb to outp and inp +#ifndef outb + #define outb(sfr,val) (_SFR_BYTE(sfr) = (val)) +#endif +#ifndef inb + #define inb(sfr) _SFR_BYTE(sfr) +#endif + +/* defines for future compatibility */ +#ifndef cbi + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +/* Adjust to suit whatever pin your hardware uses to enter the bootloader */ +#define eeprom_rb(addr) eeprom_read_byte ((uint8_t *)(addr)) +#define eeprom_rw(addr) eeprom_read_word ((uint16_t *)(addr)) +#define eeprom_wb(addr, val) eeprom_write_byte ((uint8_t *)(addr), (uint8_t)(val)) + +/* Onboard LED is connected to pin PB5 */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB5 + + +#define SIG1 0x1E // Yep, Atmel is the only manufacturer of AVR micros. Single source :( +#define SIG2 0x93 +#define SIG3 0x07 +#define PAGE_SIZE 0x20U //32 words + + +void putch(char); +char getch(void); +void getNch(uint8_t); +void byte_response(uint8_t); +void nothing_response(void); + +union address_union { + uint16_t word; + uint8_t byte[2]; +} address; + +union length_union { + uint16_t word; + uint8_t byte[2]; +} length; + +struct flags_struct { + unsigned eeprom : 1; + unsigned rampz : 1; +} flags; + +uint8_t buff[256]; +//uint8_t address_high; + +uint8_t pagesz=0x80; + +uint8_t i; +//uint8_t bootuart0=0,bootuart1=0; + + +void (*app_start)(void) = 0x0000; + +int main(void) +{ + uint8_t ch,ch2; + uint16_t w; + + //cbi(BL_DDR,BL); + //sbi(BL_PORT,BL); + + asm volatile("nop\n\t"); + + /* check if flash is programmed already, if not start bootloader anyway */ + //if(pgm_read_byte_near(0x0000) != 0xFF) { + + /* check if bootloader pin is set low */ + //if(bit_is_set(BL_PIN,BL)) app_start(); + //} + + /* initialize UART(s) depending on CPU defined */ + /* m8 */ + UBRRH = (((F_CPU/BAUD_RATE)/16)-1)>>8; // set baud rate + UBRRL = (((F_CPU/BAUD_RATE)/16)-1); + UCSRB = (1<> 8; + //UCSRA = 0x00; + //UCSRC = 0x86; + //UCSRB = _BV(TXEN)|_BV(RXEN); + + + /* this was giving uisp problems, so I removed it; without it, the boot + works on with uisp and avrdude on the mac (at least). */ + //putch('\0'); + + //uint32_t l; + //uint32_t time_count; + //time_count=0; + + /* set LED pin as output */ + sbi(LED_DDR,LED); + for (i = 0; i < 16; i++) { + outb(LED_PORT, inb(LED_PORT) ^ _BV(LED)); + _delay_loop_2(0); + } + + //for (l=0; l<40000000; l++) + //outb(LED_PORT, inb(LED_PORT) ^= _BV(LED)); + + /* flash onboard LED three times to signal entering of bootloader */ + //for(i=0; i<3; ++i) { + //for(l=0; l<40000000; ++l); + //sbi(LED_PORT,LED); + //for(l=0; l<40000000; ++l); + //cbi(LED_PORT,LED); + //} + + /* see comment at previous call to putch() */ + //putch('\0'); // this line is needed for the synchronization of the programmer + + /* forever */ + for (;;) { + //if((inb(UCSRA) & _BV(RXC))){ + /* get character from UART */ + ch = getch(); + + /* A bunch of if...else if... gives smaller code than switch...case ! */ + + /* Hello is anyone home ? */ + if(ch=='0') { + nothing_response(); + } + + /* Request programmer ID */ + /* Not using PROGMEM string due to boot block in m128 being beyond 64kB boundry */ + /* Would need to selectively manipulate RAMPZ, and it's only 9 characters anyway so who cares. */ + else if(ch=='1') { + if (getch() == ' ') { + putch(0x14); + putch('A'); + putch('V'); + putch('R'); + putch(' '); + putch('I'); + putch('S'); + putch('P'); + putch(0x10); + } + } + + /* AVR ISP/STK500 board commands DON'T CARE so default nothing_response */ + else if(ch=='@') { + ch2 = getch(); + if (ch2>0x85) getch(); + nothing_response(); + } + + /* AVR ISP/STK500 board requests */ + else if(ch=='A') { + ch2 = getch(); + if(ch2==0x80) byte_response(HW_VER); // Hardware version + else if(ch2==0x81) byte_response(SW_MAJOR); // Software major version + else if(ch2==0x82) byte_response(SW_MINOR); // Software minor version + //else if(ch2==0x98) byte_response(0x03); // Unknown but seems to be required by avr studio 3.56 + else byte_response(0x00); // Covers various unnecessary responses we don't care about + } + + /* Device Parameters DON'T CARE, DEVICE IS FIXED */ + else if(ch=='B') { + getNch(20); + nothing_response(); + } + + /* Parallel programming stuff DON'T CARE */ + else if(ch=='E') { + getNch(5); + nothing_response(); + } + + /* Enter programming mode */ + else if(ch=='P') { + nothing_response(); + // FIXME: modified only here by DojoCorp, Mumbai, India, 20050626 + //time_count=0; // exted the delay once entered prog.mode + } + + /* Leave programming mode */ + else if(ch=='Q') { + nothing_response(); + //time_count=MAX_TIME_COUNT_MORATORY; // once the programming is done, + // we should start the application + // but uisp has problems with this, + // therefore we just change the times + // and give the programmer 1 sec to react + } + + /* Erase device, don't care as we will erase one page at a time anyway. */ + else if(ch=='R') { + nothing_response(); + } + + /* Set address, little endian. EEPROM in bytes, FLASH in words */ + /* Perhaps extra address bytes may be added in future to support > 128kB FLASH. */ + /* This might explain why little endian was used here, big endian used everywhere else. */ + else if(ch=='U') { + address.byte[0] = getch(); + address.byte[1] = getch(); + nothing_response(); + } + + /* Universal SPI programming command, disabled. Would be used for fuses and lock bits. */ + else if(ch=='V') { + getNch(4); + byte_response(0x00); + } + + /* Write memory, length is big endian and is in bytes */ + else if(ch=='d') { + length.byte[1] = getch(); + length.byte[0] = getch(); + flags.eeprom = 0; + if (getch() == 'E') flags.eeprom = 1; + for (w=0;w127) address_high = 0x01; //Only possible with m128, m256 will need 3rd address byte. FIXME + //else address_high = 0x00; + + //address.word = address.word << 1; //address * 2 -> byte location + //if ((length.byte[0] & 0x01)) length.word++; //Even up an odd number of bytes + cli(); //Disable interrupts, just to be sure + while(bit_is_set(EECR,EEWE)); //Wait for previous EEPROM writes to complete + asm volatile( + "clr r17 \n\t" //page_word_count + "lds r30,address \n\t" //Address of FLASH location (in words) + "lds r31,address+1 \n\t" + "lsl r30 \n\t" //address * 2 -> byte location + "rol r31 \n\t" + "ldi r28,lo8(buff) \n\t" //Start of buffer array in RAM + "ldi r29,hi8(buff) \n\t" + "lds r24,length \n\t" //Length of data to be written (in bytes) + "lds r25,length+1 \n\t" + "sbrs r24,0 \n\t" //Even up an odd number of bytes + "rjmp length_loop \n\t" + "adiw r24,1 \n\t" + "length_loop: \n\t" //Main loop, repeat for number of words in block + "cpi r17,0x00 \n\t" //If page_word_count=0 then erase page + "brne no_page_erase \n\t" + "rcall wait_spm \n\t" +// "wait_spm1: \n\t" +// "lds r16,%0 \n\t" //Wait for previous spm to complete +// "andi r16,1 \n\t" +// "cpi r16,1 \n\t" +// "breq wait_spm1 \n\t" + "ldi r16,0x03 \n\t" //Erase page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" + "rcall wait_spm \n\t" +// "wait_spm2: \n\t" +// "lds r16,%0 \n\t" //Wait for previous spm to complete +// "andi r16,1 \n\t" +// "cpi r16,1 \n\t" +// "breq wait_spm2 \n\t" + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" + "no_page_erase: \n\t" + "ld r0,Y+ \n\t" //Write 2 bytes into page buffer + "ld r1,Y+ \n\t" + + "rcall wait_spm \n\t" +// "wait_spm3: \n\t" +// "lds r16,%0 \n\t" //Wait for previous spm to complete +// "andi r16,1 \n\t" +// "cpi r16,1 \n\t" +// "breq wait_spm3 \n\t" + "ldi r16,0x01 \n\t" //Load r0,r1 into FLASH page buffer + "sts %0,r16 \n\t" + "spm \n\t" + + "inc r17 \n\t" //page_word_count++ + "cpi r17,%1 \n\t" + "brlo same_page \n\t" //Still same page in FLASH + "write_page: \n\t" + "clr r17 \n\t" //New page, write current one first + "rcall wait_spm \n\t" +// "wait_spm4: \n\t" +// "lds r16,%0 \n\t" //Wait for previous spm to complete +// "andi r16,1 \n\t" +// "cpi r16,1 \n\t" +// "breq wait_spm4 \n\t" + "ldi r16,0x05 \n\t" //Write page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" + "rcall wait_spm \n\t" +// "wait_spm5: \n\t" +// "lds r16,%0 \n\t" //Wait for previous spm to complete +// "andi r16,1 \n\t" +// "cpi r16,1 \n\t" +// "breq wait_spm5 \n\t" + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" + "same_page: \n\t" + "adiw r30,2 \n\t" //Next word in FLASH + "sbiw r24,2 \n\t" //length-2 + "breq final_write \n\t" //Finished + "rjmp length_loop \n\t" + + "wait_spm: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm \n\t" + "ret \n\t" + + "final_write: \n\t" + "cpi r17,0 \n\t" + "breq block_done \n\t" + "adiw r24,2 \n\t" //length+2, fool above check on length after short page write + "rjmp write_page \n\t" + "block_done: \n\t" + "clr __zero_reg__ \n\t" //restore zero register + : "=m" (SPMCR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31"); + + /* Should really add a wait for RWW section to be enabled, don't actually need it since we never */ + /* exit the bootloader without a power cycle anyhow */ + } + putch(0x14); + putch(0x10); + } + } + + /* Read memory block mode, length is big endian. */ + else if(ch=='t') { + length.byte[1] = getch(); + length.byte[0] = getch(); + if (getch() == 'E') flags.eeprom = 1; + else { + flags.eeprom = 0; + address.word = address.word << 1; // address * 2 -> byte location + } + if (getch() == ' ') { // Command terminator + putch(0x14); + for (w=0;w < length.word;w++) { // Can handle odd and even lengths okay + if (flags.eeprom) { // Byte access EEPROM read + putch(eeprom_rb(address.word)); + address.word++; + } else { + if (!flags.rampz) putch(pgm_read_byte_near(address.word)); + address.word++; + } + } + putch(0x10); + } + } + + /* Get device signature bytes */ + else if(ch=='u') { + if (getch() == ' ') { + putch(0x14); + putch(SIG1); + putch(SIG2); + putch(SIG3); + putch(0x10); + } + } + + /* Read oscillator calibration byte */ + else if(ch=='v') { + byte_response(0x00); + } +// } else { +// time_count++; +// if (time_count>=MAX_TIME_COUNT) { +// app_start(); +// } +// } + } /* end of forever loop */ +} + +void putch(char ch) +{ + /* m8 */ + while (!(inb(UCSRA) & _BV(UDRE))); + outb(UDR,ch); +} + +char getch(void) +{ + /* m8 */ + uint32_t count = 0; + while(!(inb(UCSRA) & _BV(RXC))) { + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return (inb(UDR)); +} + +void getNch(uint8_t count) +{ + uint8_t i; + for(i=0;i $@ + +size: $(PROGRAM).hex + $(SIZE) $^ + +# Rules for building the .text rom images + +text: hex bin srec + +hex: $(PROGRAM).hex +bin: $(PROGRAM).bin +srec: $(PROGRAM).srec + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/bt/ATmegaBOOT_168.c b/libs/arduino-1.0/hardware/arduino/bootloaders/bt/ATmegaBOOT_168.c new file mode 100644 index 0000000..c73eefa --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/bt/ATmegaBOOT_168.c @@ -0,0 +1,1038 @@ +/**********************************************************/ +/* Serial Bootloader for Atmel megaAVR Controllers */ +/* */ +/* tested with ATmega8, ATmega128 and ATmega168 */ +/* should work with other mega's, see code for details */ +/* */ +/* ATmegaBOOT.c */ +/* */ +/* build: 050815 */ +/* date : 15.08.2005 */ +/* */ +/* 20060802: hacked for Arduino by D. Cuartielles */ +/* based on a previous hack by D. Mellis */ +/* and D. Cuartielles */ +/* */ +/* Monitor and debug functions were added to the original */ +/* code by Dr. Erik Lins, chip45.com. (See below) */ +/* */ +/* Thanks to Karl Pitrich for fixing a bootloader pin */ +/* problem and more informative LED blinking! */ +/* */ +/* For the latest version see: */ +/* http://www.chip45.com/ */ +/* */ +/* ------------------------------------------------------ */ +/* */ +/* based on stk500boot.c */ +/* Copyright (c) 2003, Jason P. Kyle */ +/* All rights reserved. */ +/* see avr1.org for original file and information */ +/* */ +/* This program is free software; you can redistribute it */ +/* and/or modify it under the terms of the GNU General */ +/* Public License as published by the Free Software */ +/* Foundation; either version 2 of the License, or */ +/* (at your option) any later version. */ +/* */ +/* This program is distributed in the hope that it will */ +/* be useful, but WITHOUT ANY WARRANTY; without even the */ +/* implied warranty of MERCHANTABILITY or FITNESS FOR A */ +/* PARTICULAR PURPOSE. See the GNU General Public */ +/* License for more details. */ +/* */ +/* You should have received a copy of the GNU General */ +/* Public License along with this program; if not, write */ +/* to the Free Software Foundation, Inc., */ +/* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* */ +/* Licence can be viewed at */ +/* http://www.fsf.org/licenses/gpl.txt */ +/* */ +/* Target = Atmel AVR m128,m64,m32,m16,m8,m162,m163,m169, */ +/* m8515,m8535. ATmega161 has a very small boot block so */ +/* isn't supported. */ +/* */ +/* Tested with m128,m8,m163 - feel free to let me know */ +/* how/if it works for you. */ +/* */ +/**********************************************************/ + + +/* some includes */ +#include +#include +#include +#include +#include + + +#define set_output(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#define set_input(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + + +#define high(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#define low(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + + + + +/* the current avr-libc eeprom functions do not support the ATmega168 */ +/* own eeprom write/read functions are used instead */ +#if !defined(__AVR_ATmega168__) || !defined(__AVR_ATmega328P__) +#include +#endif + +/* define F_CPU according to AVR_FREQ set in Makefile */ +/* Is there a better way to pass such a parameter from Makefile to source code ? */ + +#define F_CPU 16000000L + +#include + + +/* 20060803: hacked by DojoCorp */ +/* set the waiting time for the bootloader */ +#define MAX_TIME_COUNT (F_CPU>>1) + +/* set the UART baud rate */ +/* 20060803: hacked by DojoCorp */ +#define BAUD_RATE 115200 + + +/* SW_MAJOR and MINOR needs to be updated from time to time to avoid warning message from AVR Studio */ +/* never allow AVR Studio to do an update !!!! */ +#define HW_VER 0x02 +#define SW_MAJOR 0x01 +#define SW_MINOR 0x0f + + +/* Adjust to suit whatever pin your hardware uses to enter the bootloader */ +/* ATmega128 has two UARTS so two pins are used to enter bootloader and select UART */ +/* BL0... means UART0, BL1... means UART1 */ +#ifdef __AVR_ATmega128__ +#define BL_DDR DDRF +#define BL_PORT PORTF +#define BL_PIN PINF +#define BL0 PINF7 +#define BL1 PINF6 +#else +/* other ATmegas have only one UART, so only one pin is defined to enter bootloader */ +#define BL_DDR DDRD +#define BL_PORT PORTD +#define BL_PIN PIND +#define BL PIND6 +#endif + + +/* onboard LED is used to indicate, that the bootloader was entered (3x flashing) */ +/* if monitor functions are included, LED goes on after monitor was entered */ +#ifdef __AVR_ATmega128__ +/* Onboard LED is connected to pin PB7 (e.g. Crumb128, PROBOmega128, Savvy128) */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB7 +#else +/* Onboard LED is connected to pin PB2 (e.g. Crumb8, Crumb168) */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +/* 20060803: hacked by DojoCorp, LED pin is B5 in Arduino */ +/* #define LED PINB2 */ +#define LED PINB5 +#endif + + +/* monitor functions will only be compiled when using ATmega128, due to bootblock size constraints */ +#ifdef __AVR_ATmega128__ +#define MONITOR +#endif + + +/* define various device id's */ +/* manufacturer byte is always the same */ +#define SIG1 0x1E // Yep, Atmel is the only manufacturer of AVR micros. Single source :( + +#if defined __AVR_ATmega128__ +#define SIG2 0x97 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega64__ +#define SIG2 0x96 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega32__ +#define SIG2 0x95 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega16__ +#define SIG2 0x94 +#define SIG3 0x03 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8__ +#define SIG2 0x93 +#define SIG3 0x07 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega88__ +#define SIG2 0x93 +#define SIG3 0x0a +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega168__ +#define SIG2 0x94 +#define SIG3 0x06 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega328P__ +#define SIG2 0x95 +#define SIG3 0x0F +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega162__ +#define SIG2 0x94 +#define SIG3 0x04 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega163__ +#define SIG2 0x94 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega169__ +#define SIG2 0x94 +#define SIG3 0x05 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8515__ +#define SIG2 0x93 +#define SIG3 0x06 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega8535__ +#define SIG2 0x93 +#define SIG3 0x08 +#define PAGE_SIZE 0x20U //32 words +#endif + + +/* function prototypes */ +void putch(char); +char getch(void); +void getNch(uint8_t); +void byte_response(uint8_t); +void nothing_response(void); +char gethex(void); +void puthex(char); +void flash_led(uint8_t); + +/* some variables */ +union address_union { + uint16_t word; + uint8_t byte[2]; +} address; + +union length_union { + uint16_t word; + uint8_t byte[2]; +} length; + +struct flags_struct { + unsigned eeprom : 1; + unsigned rampz : 1; +} flags; + +uint8_t buff[256]; +uint8_t address_high; + +uint8_t pagesz=0x80; + +uint8_t i; +uint8_t bootuart = 0; + +void (*app_start)(void) = 0x0000; + + +/* main program starts here */ +int main(void) +{ + uint8_t ch,ch2; + uint16_t w; + + asm volatile("nop\n\t"); + + /* set pin direction for bootloader pin and enable pullup */ + /* for ATmega128, two pins need to be initialized */ +#ifdef __AVR_ATmega128__ + BL_DDR &= ~_BV(BL0); + BL_DDR &= ~_BV(BL1); + BL_PORT |= _BV(BL0); + BL_PORT |= _BV(BL1); +#else + BL_DDR &= ~_BV(BL); + BL_PORT |= _BV(BL); +#endif + + +#ifdef __AVR_ATmega128__ + /* check which UART should be used for booting */ + if(bit_is_clear(BL_PIN, BL0)) { + bootuart = 1; + } + else if(bit_is_clear(BL_PIN, BL1)) { + bootuart = 2; + } +#endif + + /* check if flash is programmed already, if not start bootloader anyway */ + if(pgm_read_byte_near(0x0000) != 0xFF) { + +#ifdef __AVR_ATmega128__ + /* no UART was selected, start application */ + if(!bootuart) { + app_start(); + } +#else + /* check if bootloader pin is set low */ + /* we don't start this part neither for the m8, nor m168 */ + //if(bit_is_set(BL_PIN, BL)) { + // app_start(); + // } +#endif + } + +#ifdef __AVR_ATmega128__ + /* no bootuart was selected, default to uart 0 */ + if(!bootuart) { + bootuart = 1; + } +#endif + + + /* initialize UART(s) depending on CPU defined */ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR0A = 0x00; + UCSR0C = 0x06; + UCSR0B = _BV(TXEN0)|_BV(RXEN0); + } + if(bootuart == 2) { + UBRR1L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR1H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR1A = 0x00; + UCSR1C = 0x06; + UCSR1B = _BV(TXEN1)|_BV(RXEN1); + } +#elif defined __AVR_ATmega163__ + UBRR = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRRHI = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSRA = 0x00; + UCSRB = _BV(TXEN)|_BV(RXEN); +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + + UBRR0H = ((F_CPU / 16 + BAUD_RATE / 2) / BAUD_RATE - 1) >> 8; + UBRR0L = ((F_CPU / 16 + BAUD_RATE / 2) / BAUD_RATE - 1); + + + //UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + //UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR0B = (1<>8; // set baud rate + UBRRL = (((F_CPU/BAUD_RATE)/16)-1); + UCSRB = (1<> 8; + UCSRA = 0x00; + UCSRC = 0x06; + UCSRB = _BV(TXEN)|_BV(RXEN); +#endif + + /* set LED pin as output */ + LED_DDR |= _BV(LED); + + + + set_output(DDRD,PIND7); + high(PORTD,PD7); + for (i = 0; i < 16; i++) { + + _delay_loop_2(0); + } + + + low(PORTD,PD7); + + + /* flash onboard LED to signal entering of bootloader */ +#ifdef __AVR_ATmega128__ + // 4x for UART0, 5x for UART1 + flash_led(3 + bootuart); +#else + flash_led(3); +#endif + + /* 20050803: by DojoCorp, this is one of the parts provoking the + system to stop listening, cancelled from the original */ + //putch('\0'); + + + //message("SET BT PAGEMODE 3 2000 1"); +putch('S'); +putch('E'); +putch('T'); +putch(' '); +putch('B'); +putch('T'); +putch(' '); +putch('P'); +putch('A'); +putch('G'); +putch('E'); +putch('M'); +putch('O'); +putch('D'); +putch('E'); +putch(' '); +putch('3'); +putch(' '); +putch('2'); +putch('0'); +putch('0'); +putch('0'); +putch(' '); +putch('1'); +putch(0x0D); + + + //put_s("SET BT ROLE 0 f 7d00"); + putch('S'); + putch('E'); + putch('T'); + putch(' '); + putch('B'); + putch('T'); + putch(' '); + putch('R'); + putch('O'); + putch('L'); + putch('E'); + putch(' '); + putch('0'); + putch(' '); + putch('f'); + putch(' '); + putch('7'); + putch('d'); + putch('0'); + putch('0'); + putch(0x0D); + + + + + + + /* forever loop */ + for (;;) { + + /* get character from UART */ + ch = getch(); + + /* A bunch of if...else if... gives smaller code than switch...case ! */ + + /* Hello is anyone home ? */ + if(ch=='0') { + nothing_response(); + } + + + /* Request programmer ID */ + /* Not using PROGMEM string due to boot block in m128 being beyond 64kB boundry */ + /* Would need to selectively manipulate RAMPZ, and it's only 9 characters anyway so who cares. */ + else if(ch=='1') { + if (getch() == ' ') { + putch(0x14); + putch('A'); + putch('V'); + putch('R'); + putch(' '); + putch('I'); + putch('S'); + putch('P'); + putch(0x10); + } + } + + + /* AVR ISP/STK500 board commands DON'T CARE so default nothing_response */ + else if(ch=='@') { + ch2 = getch(); + if (ch2>0x85) getch(); + nothing_response(); + } + + + /* AVR ISP/STK500 board requests */ + else if(ch=='A') { + ch2 = getch(); + if(ch2==0x80) byte_response(HW_VER); // Hardware version + else if(ch2==0x81) byte_response(SW_MAJOR); // Software major version + else if(ch2==0x82) byte_response(SW_MINOR); // Software minor version + else if(ch2==0x98) byte_response(0x03); // Unknown but seems to be required by avr studio 3.56 + else byte_response(0x00); // Covers various unnecessary responses we don't care about + } + + + /* Device Parameters DON'T CARE, DEVICE IS FIXED */ + else if(ch=='B') { + getNch(20); + nothing_response(); + } + + + /* Parallel programming stuff DON'T CARE */ + else if(ch=='E') { + getNch(5); + nothing_response(); + } + + + /* Enter programming mode */ + else if(ch=='P') { + nothing_response(); + } + + + /* Leave programming mode */ + else if(ch=='Q') { + nothing_response(); + } + + + /* Erase device, don't care as we will erase one page at a time anyway. */ + else if(ch=='R') { + nothing_response(); + } + + + /* Set address, little endian. EEPROM in bytes, FLASH in words */ + /* Perhaps extra address bytes may be added in future to support > 128kB FLASH. */ + /* This might explain why little endian was used here, big endian used everywhere else. */ + else if(ch=='U') { + address.byte[0] = getch(); + address.byte[1] = getch(); + nothing_response(); + } + + + /* Universal SPI programming command, disabled. Would be used for fuses and lock bits. */ + else if(ch=='V') { + getNch(4); + byte_response(0x00); + } + + + /* Write memory, length is big endian and is in bytes */ + else if(ch=='d') { + length.byte[1] = getch(); + length.byte[0] = getch(); + flags.eeprom = 0; + if (getch() == 'E') flags.eeprom = 1; + for (w=0;w127) address_high = 0x01; //Only possible with m128, m256 will need 3rd address byte. FIXME + else address_high = 0x00; +#ifdef __AVR_ATmega128__ + RAMPZ = address_high; +#endif + address.word = address.word << 1; //address * 2 -> byte location + /* if ((length.byte[0] & 0x01) == 0x01) length.word++; //Even up an odd number of bytes */ + if ((length.byte[0] & 0x01)) length.word++; //Even up an odd number of bytes + cli(); //Disable interrupts, just to be sure + // HACKME: EEPE used to be EEWE + while(bit_is_set(EECR,EEPE)); //Wait for previous EEPROM writes to complete + asm volatile( + "clr r17 \n\t" //page_word_count + "lds r30,address \n\t" //Address of FLASH location (in bytes) + "lds r31,address+1 \n\t" + "ldi r28,lo8(buff) \n\t" //Start of buffer array in RAM + "ldi r29,hi8(buff) \n\t" + "lds r24,length \n\t" //Length of data to be written (in bytes) + "lds r25,length+1 \n\t" + "length_loop: \n\t" //Main loop, repeat for number of words in block + "cpi r17,0x00 \n\t" //If page_word_count=0 then erase page + "brne no_page_erase \n\t" + "wait_spm1: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm1 \n\t" + "ldi r16,0x03 \n\t" //Erase page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "wait_spm2: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm2 \n\t" + + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "no_page_erase: \n\t" + "ld r0,Y+ \n\t" //Write 2 bytes into page buffer + "ld r1,Y+ \n\t" + + "wait_spm3: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm3 \n\t" + "ldi r16,0x01 \n\t" //Load r0,r1 into FLASH page buffer + "sts %0,r16 \n\t" + "spm \n\t" + + "inc r17 \n\t" //page_word_count++ + "cpi r17,%1 \n\t" + "brlo same_page \n\t" //Still same page in FLASH + "write_page: \n\t" + "clr r17 \n\t" //New page, write current one first + "wait_spm4: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm4 \n\t" +#ifdef __AVR_ATmega163__ + "andi r30,0x80 \n\t" // m163 requires Z6:Z1 to be zero during page write +#endif + "ldi r16,0x05 \n\t" //Write page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" + "ori r30,0x7E \n\t" // recover Z6:Z1 state after page write (had to be zero during write) +#endif + "wait_spm5: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm5 \n\t" + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "same_page: \n\t" + "adiw r30,2 \n\t" //Next word in FLASH + "sbiw r24,2 \n\t" //length-2 + "breq final_write \n\t" //Finished + "rjmp length_loop \n\t" + "final_write: \n\t" + "cpi r17,0 \n\t" + "breq block_done \n\t" + "adiw r24,2 \n\t" //length+2, fool above check on length after short page write + "rjmp write_page \n\t" + "block_done: \n\t" + "clr __zero_reg__ \n\t" //restore zero register +#if defined __AVR_ATmega168__ || __AVR_ATmega328P__ + : "=m" (SPMCSR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#else + : "=m" (SPMCR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#endif + ); + /* Should really add a wait for RWW section to be enabled, don't actually need it since we never */ + /* exit the bootloader without a power cycle anyhow */ + } + putch(0x14); + putch(0x10); + } + } + + + /* Read memory block mode, length is big endian. */ + else if(ch=='t') { + length.byte[1] = getch(); + length.byte[0] = getch(); +#if defined __AVR_ATmega128__ + if (address.word>0x7FFF) flags.rampz = 1; // No go with m256, FIXME + else flags.rampz = 0; +#endif + if (getch() == 'E') flags.eeprom = 1; + else { + flags.eeprom = 0; + address.word = address.word << 1; // address * 2 -> byte location + } + if (getch() == ' ') { // Command terminator + putch(0x14); + for (w=0;w < length.word;w++) { // Can handle odd and even lengths okay + if (flags.eeprom) { // Byte access EEPROM read +#if defined __AVR_ATmega168__ || __AVR_ATmega328P__ + while(EECR & (1<= 'a') { + ah = ah - 'a' + 0x0a; + } else if(ah >= '0') { + ah -= '0'; + } + if(al >= 'a') { + al = al - 'a' + 0x0a; + } else if(al >= '0') { + al -= '0'; + } + return (ah << 4) + al; +} + + +void puthex(char ch) { + char ah,al; + + ah = (ch & 0xf0) >> 4; + if(ah >= 0x0a) { + ah = ah - 0x0a + 'a'; + } else { + ah += '0'; + } + al = (ch & 0x0f); + if(al >= 0x0a) { + al = al - 0x0a + 'a'; + } else { + al += '0'; + } + putch(ah); + putch(al); +} + + +void putch(char ch) +{ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; + } + else if (bootuart == 2) { + while (!(UCSR1A & _BV(UDRE1))); + UDR1 = ch; + } +#elif defined (__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; +#else + /* m8,16,32,169,8515,8535,163 */ + while (!(UCSRA & _BV(UDRE))); + UDR = ch; +#endif +} + + +char getch(void) +{ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + while(!(UCSR0A & _BV(RXC0))); + return UDR0; + } + else if(bootuart == 2) { + while(!(UCSR1A & _BV(RXC1))); + return UDR1; + } + return 0; +#elif defined (__AVR_ATmega168__) || defined(__AVR_ATmega328P__) + uint32_t count = 0; + while(!(UCSR0A & _BV(RXC0))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR0; +#else + /* m8,16,32,169,8515,8535,163 */ + uint32_t count = 0; + while(!(UCSRA & _BV(RXC))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR; +#endif +} + + +void getNch(uint8_t count) +{ + uint8_t i; + for(i=0;i $@ + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.c b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.c new file mode 100644 index 0000000..296c62b --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.c @@ -0,0 +1,780 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Main source file for the CDC class bootloader. This file contains the complete bootloader logic. + */ + +#define INCLUDE_FROM_CATERINA_C +#include "Caterina.h" + +/** Contains the current baud rate and other settings of the first virtual serial port. This must be retained as some + * operating systems will not open the port unless the settings can be set successfully. + */ +static CDC_LineEncoding_t LineEncoding = { .BaudRateBPS = 0, + .CharFormat = CDC_LINEENCODING_OneStopBit, + .ParityType = CDC_PARITY_None, + .DataBits = 8 }; + +/** Current address counter. This stores the current address of the FLASH or EEPROM as set by the host, + * and is used when reading or writing to the AVRs memory (either FLASH or EEPROM depending on the issued + * command.) + */ +static uint32_t CurrAddress; + +/** Flag to indicate if the bootloader should be running, or should exit and allow the application code to run + * via a watchdog reset. When cleared the bootloader will exit, starting the watchdog and entering an infinite + * loop until the AVR restarts and the application runs. + */ +static bool RunBootloader = true; + +/* Pulse generation counters to keep track of the time remaining for each pulse type */ +#define TX_RX_LED_PULSE_PERIOD 100 +uint16_t TxLEDPulse = 0; // time remaining for Tx LED pulse +uint16_t RxLEDPulse = 0; // time remaining for Rx LED pulse + +/* Bootloader timeout timer */ +// MAH 8/15/12- change so timeouts work properly when the chip is running at 8MHz instead of 16. +#define TIMEOUT_PERIOD 4000 +#define EXT_RESET_TIMEOUT_PERIOD 375 + + +/********************************************************************************************************* +LilyPadUSB bootloader code +The LilyPadUSB bootloader has been changed to remove the 8-second delay after external reset which is in +the Leonardo. To enter the bootloader, the user should execute TWO external resets within 750 ms; that is, +press the reset button twice, quickly.\ + +Some other changes were made to allow this code to compile tightly enough to fit in the alloted 4k of +bootloader space. +*/ +// MAH 8/15/12- added this flag to replace the bulky program memory reads to check for the presence of a sketch +// at the top of the memory space. +static bool sketchPresent = false; + +// MAH 8/15/12- make this volatile, since we modify it in one place and read it in another, we want to make +// sure we're always working on the copy in memory and not an erroneous value stored in a cache somewhere. +// This variable stores the length of time we've been in the bootloader when waiting for the 8 second delay. +volatile uint16_t Timeout = 0; +// MAH 8/15/12- added this for delay during startup. Did not use existing Timeout value b/c it only increments +// when there's a sketch at the top of the memory. +volatile uint16_t resetTimeout = 0; + +// MAH 8/15/12- let's make this an 8-bit value instead of 16- that saves on memory because 16-bit addition and +// comparison compiles to bulkier code. Note that this does *not* require a change to the Arduino core- we're +// just sort of ignoring the extra byte that the Arduino core puts at the next location. +uint8_t bootKey = 0x77; +volatile uint8_t *const bootKeyPtr = (volatile uint8_t *)0x0800; + +// StartSketch() is called to clean up our mess before passing execution to the sketch. +void StartSketch(void) +{ + cli(); + + /* Undo TIMER1 setup and clear the count before running the sketch */ + TIMSK1 = 0; + TCCR1B = 0; + + /* Relocate the interrupt vector table to the application section */ + MCUCR = (1 << IVCE); + MCUCR = 0; + + L_LED_OFF(); + TX_LED_OFF(); + RX_LED_OFF(); + + /* jump to beginning of application space */ + __asm__ volatile("jmp 0x0000"); + +} + +uint16_t LLEDPulse; + +/** Main program entry point. This routine configures the hardware required by the bootloader, then continuously + * runs the bootloader processing routine until it times out or is instructed to exit. + */ +int main(void) +{ + /* Save the value of the boot key memory before it is overwritten */ + uint8_t bootKeyPtrVal = *bootKeyPtr; + *bootKeyPtr = 0; + + /* Check the reason for the reset so we can act accordingly */ + uint8_t mcusr_state = MCUSR; // store the initial state of the Status register + MCUSR = 0; // clear all reset flags + + /* Watchdog may be configured with a 15 ms period so must disable it before going any further */ + // MAH 8/15/12- I removed this because wdt_disable() is the first thing SetupHardware() does- why + // do it twice right in a row? + //wdt_disable(); + + /* Setup hardware required for the bootloader */ + // MAH 8/15/12- Moved this up to before the bootloader go/no-go decision tree so I could use the + // timer in that decision tree. Removed the USBInit() call from it; if I'm not going to stay in + // the bootloader, there's no point spending the time initializing the USB. + // SetupHardware(); + wdt_disable(); + + // Disable clock division + clock_prescale_set(clock_div_1); + + // Relocate the interrupt vector table to the bootloader section + MCUCR = (1 << IVCE); + MCUCR = (1 << IVSEL); + + LED_SETUP(); + CPU_PRESCALE(0); + L_LED_OFF(); + TX_LED_OFF(); + RX_LED_OFF(); + + // Initialize TIMER1 to handle bootloader timeout and LED tasks. + // With 16 MHz clock and 1/64 prescaler, timer 1 is clocked at 250 kHz + // Our chosen compare match generates an interrupt every 1 ms. + // This interrupt is disabled selectively when doing memory reading, erasing, + // or writing since SPM has tight timing requirements. + + OCR1AH = 0; + OCR1AL = 250; + TIMSK1 = (1 << OCIE1A); // enable timer 1 output compare A match interrupt + TCCR1B = ((1 << CS11) | (1 << CS10)); // 1/64 prescaler on timer 1 input + + + // MAH 8/15/12- this replaces bulky pgm_read_word(0) calls later on, to save memory. + if (pgm_read_word(0) != 0xFFFF) sketchPresent = true; + +// MAH 26 Oct 2012- The "bootload or not?" section has been modified since the code released +// with Arduino 1.0.1. The simplest modification is the replacement of equivalence checks on +// the reset bits with masked checks, so if more than one reset occurs before the register is +// checked, the check doesn't fail and fall through to the bootloader unnecessarily. + +// The second, more in depth modification addresses behavior after an external reset (i.e., +// user pushes the reset button). The Leonardo treats all external resets as requests to +// re-enter the bootloader and wait for code to be loaded. It remains in bootloader mode for +// 8 seconds before continuing on to the sketch (if one is present). By defining RESET_DELAY +// equal to 1, this behavior will persist. + +// However, if RESET_DELAY is defined to 0, the reset timeout before loading the sketch drops +// to 750ms. If, during that 750ms, another external reset occurs, THEN an 8-second delay +// in the bootloader will occur. + + // This is the "no-8-second-delay" code. If this is the first time through the loop, we + // don't expect to see the bootKey in memory. + if ( (mcusr_state & (1< EXT_RESET_TIMEOUT_PERIOD) // resetTimeout is getting incremeted + RunBootloader = false; // in the timer1 ISR. + } + // If we make it past that while loop, it's sketch loading time! + *bootKeyPtr = 0; // clear out the bootKey; from now on, we want to treat a reset like + // a normal reset. + cli(); // Disable interrupts, in case no sketch is present. + RunBootloader = true; // We want to hang out in the bootloader if no sketch is present. + if (sketchPresent) StartSketch(); // If a sketch is present, go! Otherwise, wait around + // in the bootloader until one is uploaded. + } + // On a power-on reset, we ALWAYS want to go to the sketch. If there is one. + // This is a place where the old code had an equivalence and now there is a mask. + else if ( (mcusr_state & (1< TIMEOUT_PERIOD) + RunBootloader = false; + + // MAH 8/15/12- This used to be a function call- inlining it saves a few bytes. + LLEDPulse++; + uint8_t p = LLEDPulse >> 8; + if (p > 127) + p = 254-p; + p += p; + if (((uint8_t)LLEDPulse) > p) + L_LED_OFF(); + else + L_LED_ON(); + } + + /* Disconnect from the host - USB interface will be reset later along with the AVR */ + USB_Detach(); + + /* Jump to beginning of application space to run the sketch - do not reset */ + StartSketch(); +} + +// Timer1 is set up to provide periodic interrupts. This is used to flicker the LEDs during +// programming as well as to generate the clock counts which determine how long the board should +// remain in bootloading mode. + +ISR(TIMER1_COMPA_vect, ISR_BLOCK) +{ + /* Reset counter */ + TCNT1H = 0; + TCNT1L = 0; + + /* Check whether the TX or RX LED one-shot period has elapsed. if so, turn off the LED */ + if (TxLEDPulse && !(--TxLEDPulse)) + TX_LED_OFF(); + if (RxLEDPulse && !(--RxLEDPulse)) + RX_LED_OFF(); + resetTimeout++; // Needed for the "short reset delay" mode- governs the time the board waits + // for a second reset before loading the sketch. + if (pgm_read_word(0) != 0xFFFF) + Timeout++; +} + +// MAH 29 Oct 2012 Nothing below this point has to change for the LilyPadUSB support + +/** Event handler for the USB_ConfigurationChanged event. This configures the device's endpoints ready + * to relay data to and from the attached USB host. + */ +void EVENT_USB_Device_ConfigurationChanged(void) +{ + /* Setup CDC Notification, Rx and Tx Endpoints */ + Endpoint_ConfigureEndpoint(CDC_NOTIFICATION_EPNUM, EP_TYPE_INTERRUPT, + ENDPOINT_DIR_IN, CDC_NOTIFICATION_EPSIZE, + ENDPOINT_BANK_SINGLE); + + Endpoint_ConfigureEndpoint(CDC_TX_EPNUM, EP_TYPE_BULK, + ENDPOINT_DIR_IN, CDC_TXRX_EPSIZE, + ENDPOINT_BANK_SINGLE); + + Endpoint_ConfigureEndpoint(CDC_RX_EPNUM, EP_TYPE_BULK, + ENDPOINT_DIR_OUT, CDC_TXRX_EPSIZE, + ENDPOINT_BANK_SINGLE); +} + +/** Event handler for the USB_ControlRequest event. This is used to catch and process control requests sent to + * the device from the USB host before passing along unhandled control requests to the library for processing + * internally. + */ +void EVENT_USB_Device_ControlRequest(void) +{ + /* Ignore any requests that aren't directed to the CDC interface */ + if ((USB_ControlRequest.bmRequestType & (CONTROL_REQTYPE_TYPE | CONTROL_REQTYPE_RECIPIENT)) != + (REQTYPE_CLASS | REQREC_INTERFACE)) + { + return; + } + + /* Process CDC specific control requests */ + switch (USB_ControlRequest.bRequest) + { + case CDC_REQ_GetLineEncoding: + if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE)) + { + Endpoint_ClearSETUP(); + + /* Write the line coding data to the control endpoint */ + Endpoint_Write_Control_Stream_LE(&LineEncoding, sizeof(CDC_LineEncoding_t)); + Endpoint_ClearOUT(); + } + + break; + case CDC_REQ_SetLineEncoding: + if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE)) + { + Endpoint_ClearSETUP(); + + /* Read the line coding data in from the host into the global struct */ + Endpoint_Read_Control_Stream_LE(&LineEncoding, sizeof(CDC_LineEncoding_t)); + Endpoint_ClearIN(); + } + + break; + } +} + +#if !defined(NO_BLOCK_SUPPORT) +/** Reads or writes a block of EEPROM or FLASH memory to or from the appropriate CDC data endpoint, depending + * on the AVR910 protocol command issued. + * + * \param[in] Command Single character AVR910 protocol command indicating what memory operation to perform + */ +static void ReadWriteMemoryBlock(const uint8_t Command) +{ + uint16_t BlockSize; + char MemoryType; + + bool HighByte = false; + uint8_t LowByte = 0; + + BlockSize = (FetchNextCommandByte() << 8); + BlockSize |= FetchNextCommandByte(); + + MemoryType = FetchNextCommandByte(); + + if ((MemoryType != 'E') && (MemoryType != 'F')) + { + /* Send error byte back to the host */ + WriteNextResponseByte('?'); + + return; + } + + /* Disable timer 1 interrupt - can't afford to process nonessential interrupts + * while doing SPM tasks */ + TIMSK1 = 0; + + /* Check if command is to read memory */ + if (Command == 'g') + { + /* Re-enable RWW section */ + boot_rww_enable(); + + while (BlockSize--) + { + if (MemoryType == 'F') + { + /* Read the next FLASH byte from the current FLASH page */ + #if (FLASHEND > 0xFFFF) + WriteNextResponseByte(pgm_read_byte_far(CurrAddress | HighByte)); + #else + WriteNextResponseByte(pgm_read_byte(CurrAddress | HighByte)); + #endif + + /* If both bytes in current word have been read, increment the address counter */ + if (HighByte) + CurrAddress += 2; + + HighByte = !HighByte; + } + else + { + /* Read the next EEPROM byte into the endpoint */ + WriteNextResponseByte(eeprom_read_byte((uint8_t*)(intptr_t)(CurrAddress >> 1))); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + } + } + else + { + uint32_t PageStartAddress = CurrAddress; + + if (MemoryType == 'F') + { + boot_page_erase(PageStartAddress); + boot_spm_busy_wait(); + } + + while (BlockSize--) + { + if (MemoryType == 'F') + { + /* If both bytes in current word have been written, increment the address counter */ + if (HighByte) + { + /* Write the next FLASH word to the current FLASH page */ + boot_page_fill(CurrAddress, ((FetchNextCommandByte() << 8) | LowByte)); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + else + { + LowByte = FetchNextCommandByte(); + } + + HighByte = !HighByte; + } + else + { + /* Write the next EEPROM byte from the endpoint */ + eeprom_write_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)), FetchNextCommandByte()); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + } + + /* If in FLASH programming mode, commit the page after writing */ + if (MemoryType == 'F') + { + /* Commit the flash page to memory */ + boot_page_write(PageStartAddress); + + /* Wait until write operation has completed */ + boot_spm_busy_wait(); + } + + /* Send response byte back to the host */ + WriteNextResponseByte('\r'); + } + + /* Re-enable timer 1 interrupt disabled earlier in this routine */ + TIMSK1 = (1 << OCIE1A); +} +#endif + +/** Retrieves the next byte from the host in the CDC data OUT endpoint, and clears the endpoint bank if needed + * to allow reception of the next data packet from the host. + * + * \return Next received byte from the host in the CDC data OUT endpoint + */ +static uint8_t FetchNextCommandByte(void) +{ + /* Select the OUT endpoint so that the next data byte can be read */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* If OUT endpoint empty, clear it and wait for the next packet from the host */ + while (!(Endpoint_IsReadWriteAllowed())) + { + Endpoint_ClearOUT(); + + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return 0; + } + } + + /* Fetch the next byte from the OUT endpoint */ + return Endpoint_Read_8(); +} + +/** Writes the next response byte to the CDC data IN endpoint, and sends the endpoint back if needed to free up the + * bank when full ready for the next byte in the packet to the host. + * + * \param[in] Response Next response byte to send to the host + */ +static void WriteNextResponseByte(const uint8_t Response) +{ + /* Select the IN endpoint so that the next data byte can be written */ + Endpoint_SelectEndpoint(CDC_TX_EPNUM); + + /* If IN endpoint full, clear it and wait until ready for the next packet to the host */ + if (!(Endpoint_IsReadWriteAllowed())) + { + Endpoint_ClearIN(); + + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Write the next byte to the IN endpoint */ + Endpoint_Write_8(Response); + + TX_LED_ON(); + TxLEDPulse = TX_RX_LED_PULSE_PERIOD; +} + +#define STK_OK 0x10 +#define STK_INSYNC 0x14 // ' ' +#define CRC_EOP 0x20 // 'SPACE' +#define STK_GET_SYNC 0x30 // '0' + +#define STK_GET_PARAMETER 0x41 // 'A' +#define STK_SET_DEVICE 0x42 // 'B' +#define STK_SET_DEVICE_EXT 0x45 // 'E' +#define STK_LOAD_ADDRESS 0x55 // 'U' +#define STK_UNIVERSAL 0x56 // 'V' +#define STK_PROG_PAGE 0x64 // 'd' +#define STK_READ_PAGE 0x74 // 't' +#define STK_READ_SIGN 0x75 // 'u' + +/** Task to read in AVR910 commands from the CDC data OUT endpoint, process them, perform the required actions + * and send the appropriate response back to the host. + */ +void CDC_Task(void) +{ + /* Select the OUT endpoint */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* Check if endpoint has a command in it sent from the host */ + if (!(Endpoint_IsOUTReceived())) + return; + + RX_LED_ON(); + RxLEDPulse = TX_RX_LED_PULSE_PERIOD; + + /* Read in the bootloader command (first byte sent from host) */ + uint8_t Command = FetchNextCommandByte(); + + if (Command == 'E') + { + /* We nearly run out the bootloader timeout clock, + * leaving just a few hundred milliseconds so the + * bootloder has time to respond and service any + * subsequent requests */ + Timeout = TIMEOUT_PERIOD - 500; + + /* Re-enable RWW section - must be done here in case + * user has disabled verification on upload. */ + boot_rww_enable_safe(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'T') + { + FetchNextCommandByte(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if ((Command == 'L') || (Command == 'P')) + { + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 't') + { + // Return ATMEGA128 part code - this is only to allow AVRProg to use the bootloader + WriteNextResponseByte(0x44); + WriteNextResponseByte(0x00); + } + else if (Command == 'a') + { + // Indicate auto-address increment is supported + WriteNextResponseByte('Y'); + } + else if (Command == 'A') + { + // Set the current address to that given by the host + CurrAddress = (FetchNextCommandByte() << 9); + CurrAddress |= (FetchNextCommandByte() << 1); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'p') + { + // Indicate serial programmer back to the host + WriteNextResponseByte('S'); + } + else if (Command == 'S') + { + // Write the 7-byte software identifier to the endpoint + for (uint8_t CurrByte = 0; CurrByte < 7; CurrByte++) + WriteNextResponseByte(SOFTWARE_IDENTIFIER[CurrByte]); + } + else if (Command == 'V') + { + WriteNextResponseByte('0' + BOOTLOADER_VERSION_MAJOR); + WriteNextResponseByte('0' + BOOTLOADER_VERSION_MINOR); + } + else if (Command == 's') + { + WriteNextResponseByte(AVR_SIGNATURE_3); + WriteNextResponseByte(AVR_SIGNATURE_2); + WriteNextResponseByte(AVR_SIGNATURE_1); + } + else if (Command == 'e') + { + // Clear the application section of flash + for (uint32_t CurrFlashAddress = 0; CurrFlashAddress < BOOT_START_ADDR; CurrFlashAddress += SPM_PAGESIZE) + { + boot_page_erase(CurrFlashAddress); + boot_spm_busy_wait(); + boot_page_write(CurrFlashAddress); + boot_spm_busy_wait(); + } + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + #if !defined(NO_LOCK_BYTE_WRITE_SUPPORT) + else if (Command == 'l') + { + // Set the lock bits to those given by the host + boot_lock_bits_set(FetchNextCommandByte()); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + #endif + else if (Command == 'r') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_LOCK_BITS)); + } + else if (Command == 'F') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS)); + } + else if (Command == 'N') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS)); + } + else if (Command == 'Q') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS)); + } + #if !defined(NO_BLOCK_SUPPORT) + else if (Command == 'b') + { + WriteNextResponseByte('Y'); + + // Send block size to the host + WriteNextResponseByte(SPM_PAGESIZE >> 8); + WriteNextResponseByte(SPM_PAGESIZE & 0xFF); + } + else if ((Command == 'B') || (Command == 'g')) + { + // Keep resetting the timeout counter if we're receiving self-programming instructions + Timeout = 0; + // Delegate the block write/read to a separate function for clarity + ReadWriteMemoryBlock(Command); + } + #endif + #if !defined(NO_FLASH_BYTE_SUPPORT) + else if (Command == 'C') + { + // Write the high byte to the current flash page + boot_page_fill(CurrAddress, FetchNextCommandByte()); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'c') + { + // Write the low byte to the current flash page + boot_page_fill(CurrAddress | 0x01, FetchNextCommandByte()); + + // Increment the address + CurrAddress += 2; + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'm') + { + // Commit the flash page to memory + boot_page_write(CurrAddress); + + // Wait until write operation has completed + boot_spm_busy_wait(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'R') + { + #if (FLASHEND > 0xFFFF) + uint16_t ProgramWord = pgm_read_word_far(CurrAddress); + #else + uint16_t ProgramWord = pgm_read_word(CurrAddress); + #endif + + WriteNextResponseByte(ProgramWord >> 8); + WriteNextResponseByte(ProgramWord & 0xFF); + } + #endif + #if !defined(NO_EEPROM_BYTE_SUPPORT) + else if (Command == 'D') + { + // Read the byte from the endpoint and write it to the EEPROM + eeprom_write_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)), FetchNextCommandByte()); + + // Increment the address after use + CurrAddress += 2; + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'd') + { + // Read the EEPROM byte and write it to the endpoint + WriteNextResponseByte(eeprom_read_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)))); + + // Increment the address after use + CurrAddress += 2; + } + #endif + else if (Command != 27) + { + // Unknown (non-sync) command, return fail code + WriteNextResponseByte('?'); + } + + + /* Select the IN endpoint */ + Endpoint_SelectEndpoint(CDC_TX_EPNUM); + + /* Remember if the endpoint is completely full before clearing it */ + bool IsEndpointFull = !(Endpoint_IsReadWriteAllowed()); + + /* Send the endpoint data to the host */ + Endpoint_ClearIN(); + + /* If a full endpoint's worth of data was sent, we need to send an empty packet afterwards to signal end of transfer */ + if (IsEndpointFull) + { + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + Endpoint_ClearIN(); + } + + /* Wait until the data has been sent to the host */ + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + /* Select the OUT endpoint */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* Acknowledge the command from the host */ + Endpoint_ClearOUT(); +} + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.h b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.h new file mode 100644 index 0000000..7836ed7 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Caterina.h @@ -0,0 +1,99 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for BootloaderCDC.c. + */ + +#ifndef _CDC_H_ +#define _CDC_H_ + + /* Includes: */ + #include + #include + #include + #include + #include + #include + #include + + #include "Descriptors.h" + + #include + /* Macros: */ + /** Version major of the CDC bootloader. */ + #define BOOTLOADER_VERSION_MAJOR 0x01 + + /** Version minor of the CDC bootloader. */ + #define BOOTLOADER_VERSION_MINOR 0x00 + + /** Hardware version major of the CDC bootloader. */ + #define BOOTLOADER_HWVERSION_MAJOR 0x01 + + /** Hardware version minor of the CDC bootloader. */ + #define BOOTLOADER_HWVERSION_MINOR 0x00 + + /** Eight character bootloader firmware identifier reported to the host when requested */ + #define SOFTWARE_IDENTIFIER "CATERINA" + + #define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n)) + #define LED_SETUP() DDRC |= (1<<7); DDRB |= (1<<0); DDRD |= (1<<5); + #define L_LED_OFF() PORTC &= ~(1<<7) + #define L_LED_ON() PORTC |= (1<<7) + #define L_LED_TOGGLE() PORTC ^= (1<<7) + #define TX_LED_OFF() PORTD |= (1<<5) + #define TX_LED_ON() PORTD &= ~(1<<5) + #define RX_LED_OFF() PORTB |= (1<<0) + #define RX_LED_ON() PORTB &= ~(1<<0) + + /* Type Defines: */ + /** Type define for a non-returning pointer to the start of the loaded application in flash memory. */ + typedef void (*AppPtr_t)(void) ATTR_NO_RETURN; + + /* Function Prototypes: */ + void StartSketch(void); + void LEDPulse(void); + + void CDC_Task(void); + void SetupHardware(void); + + void EVENT_USB_Device_ConfigurationChanged(void); + + #if defined(INCLUDE_FROM_CATERINA_C) || defined(__DOXYGEN__) + #if !defined(NO_BLOCK_SUPPORT) + static void ReadWriteMemoryBlock(const uint8_t Command); + #endif + static uint8_t FetchNextCommandByte(void); + static void WriteNextResponseByte(const uint8_t Response); + #endif + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.c b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.c new file mode 100644 index 0000000..a19707f --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.c @@ -0,0 +1,260 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * USB Device Descriptors, for library use when in USB device mode. Descriptors are special + * computer-readable structures which the host requests upon device enumeration, to determine + * the device's capabilities and functions. + */ + +#include "Descriptors.h" + +/** Device descriptor structure. This descriptor, located in SRAM memory, describes the overall + * device characteristics, including the supported USB version, control endpoint size and the + * number of device configurations. The descriptor is read out by the USB host when the enumeration + * process begins. + */ +const USB_Descriptor_Device_t DeviceDescriptor = +{ + .Header = {.Size = sizeof(USB_Descriptor_Device_t), .Type = DTYPE_Device}, + + .USBSpecification = VERSION_BCD(01.10), + .Class = CDC_CSCP_CDCClass, + .SubClass = CDC_CSCP_NoSpecificSubclass, + .Protocol = CDC_CSCP_NoSpecificProtocol, + + .Endpoint0Size = FIXED_CONTROL_ENDPOINT_SIZE, + + .VendorID = DEVICE_VID, + .ProductID = DEVICE_PID, + .ReleaseNumber = VERSION_BCD(00.01), + + .ManufacturerStrIndex = 0x02, + .ProductStrIndex = 0x01, + .SerialNumStrIndex = NO_DESCRIPTOR, + + .NumberOfConfigurations = FIXED_NUM_CONFIGURATIONS +}; + +/** Configuration descriptor structure. This descriptor, located in SRAM memory, describes the usage + * of the device in one of its supported configurations, including information about any device interfaces + * and endpoints. The descriptor is read out by the USB host during the enumeration process when selecting + * a configuration so that the host may correctly communicate with the USB device. + */ +const USB_Descriptor_Configuration_t ConfigurationDescriptor = +{ + .Config = + { + .Header = {.Size = sizeof(USB_Descriptor_Configuration_Header_t), .Type = DTYPE_Configuration}, + + .TotalConfigurationSize = sizeof(USB_Descriptor_Configuration_t), + .TotalInterfaces = 2, + + .ConfigurationNumber = 1, + .ConfigurationStrIndex = NO_DESCRIPTOR, + + .ConfigAttributes = USB_CONFIG_ATTR_BUSPOWERED, + + .MaxPowerConsumption = USB_CONFIG_POWER_MA(100) + }, + + .CDC_CCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 0, + .AlternateSetting = 0, + + .TotalEndpoints = 1, + + .Class = CDC_CSCP_CDCClass, + .SubClass = CDC_CSCP_ACMSubclass, + .Protocol = CDC_CSCP_ATCommandProtocol, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_Functional_Header = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalHeader_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x00, + + .CDCSpecification = VERSION_BCD(01.10), + }, + + .CDC_Functional_ACM = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalACM_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x02, + + .Capabilities = 0x04, + }, + + .CDC_Functional_Union = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalUnion_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x06, + + .MasterInterfaceNumber = 0, + .SlaveInterfaceNumber = 1, + }, + + .CDC_NotificationEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_IN | CDC_NOTIFICATION_EPNUM), + .Attributes = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_NOTIFICATION_EPSIZE, + .PollingIntervalMS = 0xFF + }, + + .CDC_DCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 1, + .AlternateSetting = 0, + + .TotalEndpoints = 2, + + .Class = CDC_CSCP_CDCDataClass, + .SubClass = CDC_CSCP_NoDataSubclass, + .Protocol = CDC_CSCP_NoDataProtocol, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_DataOutEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_OUT | CDC_RX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + }, + + .CDC_DataInEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_IN | CDC_TX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + } +}; + +/** Language descriptor structure. This descriptor, located in SRAM memory, is returned when the host requests + * the string descriptor with index 0 (the first index). It is actually an array of 16-bit integers, which indicate + * via the language ID table available at USB.org what languages the device supports for its string descriptors. + */ +const USB_Descriptor_String_t LanguageString = +{ + .Header = {.Size = USB_STRING_LEN(1), .Type = DTYPE_String}, + + .UnicodeString = {LANGUAGE_ID_ENG} +}; + +/** Product descriptor string. This is a Unicode string containing the product's details in human readable form, + * and is read out upon request by the host when the appropriate string ID is requested, listed in the Device + * Descriptor. + */ +const USB_Descriptor_String_t ProductString = +{ + .Header = {.Size = USB_STRING_LEN(16), .Type = DTYPE_String}, + #if DEVICE_PID == 0x9207 + .UnicodeString = L"LilyPadUSB " + #else + .UnicodeString = L"USB IO board " + #endif +}; + +const USB_Descriptor_String_t ManufNameString = +{ + .Header = {.Size = USB_STRING_LEN(20), .Type = DTYPE_String}, + #if DEVICE_VID == 0x1B4F + .UnicodeString = L"SparkFun Electronics" + #else + .UnicodeString = L"Unknown " + #endif +}; + +/** This function is called by the library when in device mode, and must be overridden (see LUFA library "USB Descriptors" + * documentation) by the application code so that the address and size of a requested descriptor can be given + * to the USB library. When the device receives a Get Descriptor request on the control endpoint, this function + * is called so that the descriptor details can be passed back and the appropriate descriptor sent back to the + * USB host. + */ +uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + const void** const DescriptorAddress) +{ + const uint8_t DescriptorType = (wValue >> 8); + const uint8_t DescriptorNumber = (wValue & 0xFF); + + const void* Address = NULL; + uint16_t Size = NO_DESCRIPTOR; + + switch (DescriptorType) + { + case DTYPE_Device: + Address = &DeviceDescriptor; + Size = sizeof(USB_Descriptor_Device_t); + break; + case DTYPE_Configuration: + Address = &ConfigurationDescriptor; + Size = sizeof(USB_Descriptor_Configuration_t); + break; + case DTYPE_String: + if (!(DescriptorNumber)) + { + Address = &LanguageString; + Size = LanguageString.Header.Size; + } + else if (DescriptorNumber == DeviceDescriptor.ProductStrIndex) + { + Address = &ProductString; + Size = ProductString.Header.Size; + } else if (DescriptorNumber == DeviceDescriptor.ManufacturerStrIndex) + { + Address = &ManufNameString; + Size = ManufNameString.Header.Size; + } + + break; + } + + *DescriptorAddress = Address; + return Size; +} + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.h b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.h new file mode 100644 index 0000000..c843bec --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Descriptors.h @@ -0,0 +1,139 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Descriptors.c. + */ + +#ifndef _DESCRIPTORS_H_ +#define _DESCRIPTORS_H_ + + /* Includes: */ + #include + + /* Macros: */ + #if defined(__AVR_AT90USB1287__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB647__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB1286__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB646__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_ATmega32U6__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega32U4__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x87 + #elif defined(__AVR_ATmega16U4__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega32U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x8A + #elif defined(__AVR_ATmega16U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_AT90USB162__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_ATmega8U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x93 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_AT90USB82__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x82 + #else + #error The selected AVR part is not currently supported by this bootloader. + #endif + + /** Endpoint number for the CDC control interface event notification endpoint. */ + #define CDC_NOTIFICATION_EPNUM 2 + + /** Endpoint number for the CDC data interface TX (data IN) endpoint. */ + #define CDC_TX_EPNUM 3 + + /** Endpoint number for the CDC data interface RX (data OUT) endpoint. */ + #define CDC_RX_EPNUM 4 + + /** Size of the CDC data interface TX and RX data endpoint banks, in bytes. */ + #define CDC_TXRX_EPSIZE 16 + + /** Size of the CDC control interface notification endpoint bank, in bytes. */ + #define CDC_NOTIFICATION_EPSIZE 8 + + /* Type Defines: */ + /** Type define for the device configuration descriptor structure. This must be defined in the + * application code, as the configuration descriptor contains several sub-descriptors which + * vary between devices, and which describe the device's usage to the host. + */ + typedef struct + { + USB_Descriptor_Configuration_Header_t Config; + + // CDC Control Interface + USB_Descriptor_Interface_t CDC_CCI_Interface; + USB_CDC_Descriptor_FunctionalHeader_t CDC_Functional_Header; + USB_CDC_Descriptor_FunctionalACM_t CDC_Functional_ACM; + USB_CDC_Descriptor_FunctionalUnion_t CDC_Functional_Union; + USB_Descriptor_Endpoint_t CDC_NotificationEndpoint; + + // CDC Data Interface + USB_Descriptor_Interface_t CDC_DCI_Interface; + USB_Descriptor_Endpoint_t CDC_DataOutEndpoint; + USB_Descriptor_Endpoint_t CDC_DataInEndpoint; + } USB_Descriptor_Configuration_t; + + /* Function Prototypes: */ + uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + const void** const DescriptorAddress) + ATTR_WARN_UNUSED_RESULT ATTR_NON_NULL_PTR_ARG(3); + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Makefile b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Makefile new file mode 100644 index 0000000..0b5659a --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Makefile @@ -0,0 +1,716 @@ +# Hey Emacs, this is a -*- makefile -*- +#---------------------------------------------------------------------------- +# WinAVR Makefile Template written by Eric B. Weddington, Jrg Wunsch, et al. +# >> Modified for use with the LUFA project. << +# +# Released to the Public Domain +# +# Additional material for this makefile was written by: +# Peter Fleury +# Tim Henigan +# Colin O'Flynn +# Reiner Patommel +# Markus Pfaff +# Sander Pool +# Frederik Rouleau +# Carlos Lamas +# Dean Camera +# Opendous Inc. +# Denver Gingerich +# +#---------------------------------------------------------------------------- +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF. +# +# make extcoff = Convert ELF to AVR Extended COFF. +# +# make program = Download the hex file to the device, using avrdude. +# Please customize the avrdude settings below first! +# +# make doxygen = Generate DoxyGen documentation for the project (must have +# DoxyGen installed) +# +# make debug = Start either simulavr or avarice as specified for debugging, +# with avr-gdb or avr-insight as the front end for debugging. +# +# make filename.s = Just compile filename.c into the assembler code only. +# +# make filename.i = Create a preprocessed source file for use in submitting +# bug reports to the GCC project. +# +# To rebuild project do "make clean" then "make all". +#---------------------------------------------------------------------------- + +# USB vendor ID (VID) +# official Arduino LLC VID = 0x2341 +# SparkFun VID = 0x1B4F +VID = 0x1B4F + +# USB product ID (PID) +# official Leonardo PID = 0x0036 +# SparkFun LilyPadUSB PID = 0x9207 +PID = 0x9207 + +# MCU name +MCU = atmega32u4 + +# Target architecture (see library "Board Types" documentation). +ARCH = AVR8 + +# Target board (see library "Board Types" documentation, NONE for projects not requiring +# LUFA board drivers). If USER is selected, put custom board drivers in a directory called +# "Board" inside the application directory. +BOARD = USER + +# Processor frequency. +# This will define a symbol, F_CPU, in all source code files equal to the +# processor frequency in Hz. You can then use this symbol in your source code to +# calculate timings. Do NOT tack on a 'UL' at the end, this will be done +# automatically to create a 32-bit value in your source code. +# +# This will be an integer division of F_USB below, as it is sourced by +# F_USB after it has run through any CPU prescalers. Note that this value +# does not *change* the processor frequency - it should merely be updated to +# reflect the processor speed set externally so that the code can use accurate +# software delays. +F_CPU = 8000000 + + +# Input clock frequency. +# This will define a symbol, F_USB, in all source code files equal to the +# input clock frequency (before any prescaling is performed) in Hz. This value may +# differ from F_CPU if prescaling is used on the latter, and is required as the +# raw input clock is fed directly to the PLL sections of the AVR for high speed +# clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL' +# at the end, this will be done automatically to create a 32-bit value in your +# source code. +# +# If no clock division is performed on the input clock inside the AVR (via the +# CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU. +F_USB = $(F_CPU) + + +# Starting byte address of the bootloader, as a byte address - computed via the formula +# BOOT_START = ((FLASH_SIZE_KB - BOOT_SECTION_SIZE_KB) * 1024) +# +# Note that the bootloader size and start address given in AVRStudio is in words and not +# bytes, and so will need to be doubled to obtain the byte address needed by AVR-GCC. +FLASH_SIZE_KB = 32 +BOOT_SECTION_SIZE_KB = 4 +BOOT_START = 0x$(shell echo "obase=16; ($(FLASH_SIZE_KB) - $(BOOT_SECTION_SIZE_KB)) * 1024" | bc) + + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + + +# Target file name (without extension). +TARGET = Caterina + + +# Object files directory +# To put object files in current directory, use a dot (.), do NOT make +# this an empty or blank macro! +OBJDIR = . + + +# Path to the LUFA library +LUFA_PATH = LUFA-111009 + + +# LUFA library compile-time options and predefined tokens +LUFA_OPTS = -D USB_DEVICE_ONLY +LUFA_OPTS += -D DEVICE_STATE_AS_GPIOR=0 +LUFA_OPTS += -D ORDERED_EP_CONFIG +LUFA_OPTS += -D FIXED_CONTROL_ENDPOINT_SIZE=8 +LUFA_OPTS += -D FIXED_NUM_CONFIGURATIONS=1 +LUFA_OPTS += -D USE_RAM_DESCRIPTORS +LUFA_OPTS += -D USE_STATIC_OPTIONS="(USB_DEVICE_OPT_FULLSPEED | USB_OPT_REG_ENABLED | USB_OPT_AUTO_PLL)" +LUFA_OPTS += -D NO_INTERNAL_SERIAL +LUFA_OPTS += -D NO_DEVICE_SELF_POWER +LUFA_OPTS += -D NO_DEVICE_REMOTE_WAKEUP +LUFA_OPTS += -D NO_SOF_EVENTS + +#LUFA_OPTS += -D NO_BLOCK_SUPPORT +#LUFA_OPTS += -D NO_EEPROM_BYTE_SUPPORT +#LUFA_OPTS += -D NO_FLASH_BYTE_SUPPORT +LUFA_OPTS += -D NO_LOCK_BYTE_WRITE_SUPPORT + + +# Create the LUFA source path variables by including the LUFA root makefile +include $(LUFA_PATH)/LUFA/makefile + + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c \ + Descriptors.c \ + $(LUFA_SRC_USB) \ + + +# List C++ source files here. (C dependencies are automatically generated.) +CPPSRC = + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + + +# Debugging format. +# Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. +# AVR Studio 4.10 requires dwarf-2. +# AVR [Extended] COFF format requires stabs, plus an avr-objcopy run. +DEBUG = dwarf-2 + + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRAINCDIRS = $(LUFA_PATH)/ + + +# Compiler flag to set the C Standard level. +# c89 = "ANSI" C +# gnu89 = c89 plus GCC extensions +# c99 = ISO C99 standard (not yet fully implemented) +# gnu99 = c99 plus GCC extensions +CSTANDARD = -std=c99 + + +# Place -D or -U options here for C sources +CDEFS = -DF_CPU=$(F_CPU)UL +CDEFS += -DF_USB=$(F_USB)UL +CDEFS += -DBOARD=BOARD_$(BOARD) -DARCH=ARCH_$(ARCH) +CDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CDEFS += -DDEVICE_VID=$(VID)UL +CDEFS += -DDEVICE_PID=$(PID)UL +CDEFS += $(LUFA_OPTS) + + +# Place -D or -U options here for ASM sources +ADEFS = -DF_CPU=$(F_CPU) +ADEFS += -DF_USB=$(F_USB)UL +ADEFS += -DBOARD=BOARD_$(BOARD) +ADEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +ADEFS += $(LUFA_OPTS) + + +# Place -D or -U options here for C++ sources +CPPDEFS = -DF_CPU=$(F_CPU)UL +CPPDEFS += -DF_USB=$(F_USB)UL +CPPDEFS += -DBOARD=BOARD_$(BOARD) +CPPDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CPPDEFS += $(LUFA_OPTS) +#CPPDEFS += -D__STDC_LIMIT_MACROS +#CPPDEFS += -D__STDC_CONSTANT_MACROS + + + +#---------------- Compiler Options C ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char +CFLAGS += -funsigned-bitfields +CFLAGS += -ffunction-sections +CFLAGS += -fno-inline-small-functions +CFLAGS += -fpack-struct +CFLAGS += -fshort-enums +CFLAGS += -fno-strict-aliasing +CFLAGS += -Wall +CFLAGS += -Wstrict-prototypes +#CFLAGS += -mshort-calls +#CFLAGS += -fno-unit-at-a-time +#CFLAGS += -Wundef +#CFLAGS += -Wunreachable-code +#CFLAGS += -Wsign-compare +CFLAGS += -Wa,-adhlns=$(<:%.c=$(OBJDIR)/%.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) + + +#---------------- Compiler Options C++ ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CPPFLAGS = -g$(DEBUG) +CPPFLAGS += $(CPPDEFS) +CPPFLAGS += -O$(OPT) +CPPFLAGS += -funsigned-char +CPPFLAGS += -funsigned-bitfields +CPPFLAGS += -fpack-struct +CPPFLAGS += -fshort-enums +CPPFLAGS += -fno-exceptions +CPPFLAGS += -Wall +CPPFLAGS += -Wundef +#CPPFLAGS += -mshort-calls +#CPPFLAGS += -fno-unit-at-a-time +#CPPFLAGS += -Wstrict-prototypes +#CPPFLAGS += -Wunreachable-code +#CPPFLAGS += -Wsign-compare +CPPFLAGS += -Wa,-adhlns=$(<:%.cpp=$(OBJDIR)/%.lst) +CPPFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +#CPPFLAGS += $(CSTANDARD) + + +#---------------- Assembler Options ---------------- +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +# -listing-cont-lines: Sets the maximum number of continuation lines of hex +# dump that will be displayed for a given single line of source input. +ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:%.S=$(OBJDIR)/%.lst),-gstabs,--listing-cont-lines=100 + + +#---------------- Library Options ---------------- +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt + +# If this is left blank, then it will use the Standard printf version. +PRINTF_LIB = +#PRINTF_LIB = $(PRINTF_LIB_MIN) +#PRINTF_LIB = $(PRINTF_LIB_FLOAT) + + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +# If this is left blank, then it will use the Standard scanf version. +SCANF_LIB = +#SCANF_LIB = $(SCANF_LIB_MIN) +#SCANF_LIB = $(SCANF_LIB_FLOAT) + + +MATH_LIB = -lm + + +# List any extra directories to look for libraries here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRALIBDIRS = + + + +#---------------- External Memory Options ---------------- + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--section-start,.data=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + + + +#---------------- Linker Options ---------------- +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += -Wl,--section-start=.text=$(BOOT_START) +LDFLAGS += -Wl,--relax +LDFLAGS += -Wl,--gc-sections +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(patsubst %,-L%,$(EXTRALIBDIRS)) +LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) +#LDFLAGS += -T linker_script.x + + + +#---------------- Programming Options (avrdude) ---------------- + +# Programming hardware +# Type: avrdude -c ? +# to get a full listing. +# +AVRDUDE_PROGRAMMER = avrispmkII + +# com1 = serial port. Use lpt1 to connect to parallel port. +AVRDUDE_PORT = usb + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep +AVRDUDE_WRITE_FUSES = -U efuse:w:0xce:m +AVRDUDE_WRITE_FUSES += -U hfuse:w:0xd8:m +AVRDUDE_WRITE_FUSES += -U lfuse:w:0xff:m + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +#---------------- Debugging Options ---------------- + +# For simulavr only - target MCU frequency. +DEBUG_MFREQ = $(F_CPU) + +# Set the DEBUG_UI to either gdb or insight. +# DEBUG_UI = gdb +DEBUG_UI = insight + +# Set the debugging back-end to either avarice, simulavr. +DEBUG_BACKEND = avarice +#DEBUG_BACKEND = simulavr + +# GDB Init Filename. +GDBINIT_FILE = __avr_gdbinit + +# When using avarice settings for the JTAG +JTAG_DEV = /dev/com1 + +# Debugging port used to communicate between GDB / avarice / simulavr. +DEBUG_PORT = 4242 + +# Debugging host used to communicate between GDB / avarice / simulavr, normally +# just set to localhost unless doing some sort of crazy debugging when +# avarice is running on a different computer. +DEBUG_HOST = localhost + + + +#============================================================================ + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +AR = avr-ar rcs +NM = avr-nm +AVRDUDE = avrdude -B 1 +REMOVE = rm -f +REMOVEDIR = rm -rf +COPY = cp +WINSHELL = cmd + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling C: +MSG_COMPILING_CPP = Compiling C++: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: +MSG_CREATING_LIBRARY = Creating library: + + + + +# Define all object files. +OBJ = $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + +# Define all listing files. +LST = $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + + +# Compiler flags to generate dependency files. +GENDEPFLAGS = -MMD -MP -MF .dep/$(@F).d + + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_CPPFLAGS = -mmcu=$(MCU) -I. -x c++ $(CPPFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + +# Default target. +all: begin gccversion sizebefore build sizeafter end + +# Change the build target to build a HEX file or a library. +build: elf hex eep lss sym +#build: lib + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym +LIBNAME=lib$(TARGET).a +lib: $(LIBNAME) + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) $(MCU_FLAG) $(FORMAT_FLAG) $(TARGET).elf +MCU_FLAG = $(shell $(SIZE) --help | grep -- --mcu > /dev/null && echo --mcu=$(MCU) ) +FORMAT_FLAG = $(shell $(SIZE) --help | grep -- --format=.*avr > /dev/null && echo --format=avr ) + + +sizebefore: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +sizeafter: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \ + 2>/dev/null; echo; fi + + + +# Display compiler version information. +gccversion : + @$(CC) --version + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) $(AVRDUDE_WRITE_FUSES) + + +# Generate avr-gdb config/init file which does the following: +# define the reset signal, load the target file, connect to target, and set +# a breakpoint at main(). +gdb-config: + @$(REMOVE) $(GDBINIT_FILE) + @echo define reset >> $(GDBINIT_FILE) + @echo SIGNAL SIGHUP >> $(GDBINIT_FILE) + @echo end >> $(GDBINIT_FILE) + @echo file $(TARGET).elf >> $(GDBINIT_FILE) + @echo target remote $(DEBUG_HOST):$(DEBUG_PORT) >> $(GDBINIT_FILE) +ifeq ($(DEBUG_BACKEND),simulavr) + @echo load >> $(GDBINIT_FILE) +endif + @echo break main >> $(GDBINIT_FILE) + +debug: gdb-config $(TARGET).elf +ifeq ($(DEBUG_BACKEND), avarice) + @echo Starting AVaRICE - Press enter when "waiting to connect" message displays. + @$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \ + $(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT) + @$(WINSHELL) /c pause + +else + @$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \ + $(DEBUG_MFREQ) --port $(DEBUG_PORT) +endif + @$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE) + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT = $(OBJCOPY) --debugging +COFFCONVERT += --change-section-address .data-0x800000 +COFFCONVERT += --change-section-address .bss-0x800000 +COFFCONVERT += --change-section-address .noinit-0x800000 +COFFCONVERT += --change-section-address .eeprom-0x810000 + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 --no-change-warnings -O $(FORMAT) $< $@ || exit 0 + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S -z $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Create library from object files. +.SECONDARY : $(TARGET).a +.PRECIOUS : $(OBJ) +%.a: $(OBJ) + @echo + @echo $(MSG_CREATING_LIBRARY) $@ + $(AR) $@ $(OBJ) + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +$(OBJDIR)/%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create object files from C++ source files. +$(OBJDIR)/%.o : %.cpp + @echo + @echo $(MSG_COMPILING_CPP) $< + $(CC) -c $(ALL_CPPFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C++ source files. +%.s : %.cpp + $(CC) -S $(ALL_CPPFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +$(OBJDIR)/%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + +# Create preprocessed source for use in sending a bug report. +%.i : %.c + $(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ + + +# Target: clean project. +clean: begin clean_list end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lss + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) $(SRC:.c=.i) + $(REMOVEDIR) .dep + +doxygen: + @echo Generating Project Documentation \($(TARGET)\)... + @doxygen Doxygen.conf + @echo Documentation Generation Complete. + +clean_doxygen: + rm -rf Documentation + +checksource: + @for f in $(SRC) $(CPPSRC) $(ASRC); do \ + if [ -f $$f ]; then \ + echo "Found Source File: $$f" ; \ + else \ + echo "Source File Not Found: $$f" ; \ + fi; done + + +# Create object files directory +$(shell mkdir $(OBJDIR) 2>/dev/null) + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff doxygen clean \ +clean_list clean_doxygen program debug gdb-config checksource + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Readme.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Readme.txt new file mode 100644 index 0000000..d066611 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina-LilyPadUSB/Readme.txt @@ -0,0 +1,11 @@ +Building the bootloader for the LilyPadUSB +1. Download the LUFA-111009 file (http://fourwalledcubicle.com/blog/2011/10/lufa-111009-released/). +2. Extract that file directly to the Caterina-LilyPadUSB bootloader directory. +3. Open a command prompt in the Caterina-LilyPadUSB bootloader directory. +4. Type 'make'. +5. Enjoy! + +Programming the bootloader for the LilyPadUSB +1. Open a command prompt in the Caterina-LilyPadUSB folder. +2. Connect your programmer- use a 2x3 .1" header, pressed against the programming vias. +3. Type 'make program' into the command prompt. \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Esplora.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Esplora.txt new file mode 100644 index 0000000..aef5df1 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Esplora.txt @@ -0,0 +1,6 @@ +LUFA: 111009 +make: 3.81 +avrdude: 5.11.1 +avr-libc: 1.6.7 +binutils-avr: 2.19 +gcc-avr 4.3.3 diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Leonardo.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Leonardo.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Leonardo.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Micro.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Micro.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina-Micro.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.c b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.c new file mode 100644 index 0000000..0204873 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.c @@ -0,0 +1,714 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Main source file for the CDC class bootloader. This file contains the complete bootloader logic. + */ + +#define INCLUDE_FROM_CATERINA_C +#include "Caterina.h" + +/** Contains the current baud rate and other settings of the first virtual serial port. This must be retained as some + * operating systems will not open the port unless the settings can be set successfully. + */ +static CDC_LineEncoding_t LineEncoding = { .BaudRateBPS = 0, + .CharFormat = CDC_LINEENCODING_OneStopBit, + .ParityType = CDC_PARITY_None, + .DataBits = 8 }; + +/** Current address counter. This stores the current address of the FLASH or EEPROM as set by the host, + * and is used when reading or writing to the AVRs memory (either FLASH or EEPROM depending on the issued + * command.) + */ +static uint32_t CurrAddress; + +/** Flag to indicate if the bootloader should be running, or should exit and allow the application code to run + * via a watchdog reset. When cleared the bootloader will exit, starting the watchdog and entering an infinite + * loop until the AVR restarts and the application runs. + */ +static bool RunBootloader = true; + +/* Pulse generation counters to keep track of the time remaining for each pulse type */ +#define TX_RX_LED_PULSE_PERIOD 100 +uint16_t TxLEDPulse = 0; // time remaining for Tx LED pulse +uint16_t RxLEDPulse = 0; // time remaining for Rx LED pulse + +/* Bootloader timeout timer */ +#define TIMEOUT_PERIOD 8000 +uint16_t Timeout = 0; + +uint16_t bootKey = 0x7777; +volatile uint16_t *const bootKeyPtr = (volatile uint16_t *)0x0800; + +void StartSketch(void) +{ + cli(); + + /* Undo TIMER1 setup and clear the count before running the sketch */ + TIMSK1 = 0; + TCCR1B = 0; + TCNT1H = 0; // 16-bit write to TCNT1 requires high byte be written first + TCNT1L = 0; + + /* Relocate the interrupt vector table to the application section */ + MCUCR = (1 << IVCE); + MCUCR = 0; + + L_LED_OFF(); + TX_LED_OFF(); + RX_LED_OFF(); + + /* jump to beginning of application space */ + __asm__ volatile("jmp 0x0000"); +} + +/* Breathing animation on L LED indicates bootloader is running */ +uint16_t LLEDPulse; +void LEDPulse(void) +{ + LLEDPulse++; + uint8_t p = LLEDPulse >> 8; + if (p > 127) + p = 254-p; + p += p; + if (((uint8_t)LLEDPulse) > p) + L_LED_OFF(); + else + L_LED_ON(); +} + +/** Main program entry point. This routine configures the hardware required by the bootloader, then continuously + * runs the bootloader processing routine until it times out or is instructed to exit. + */ +int main(void) +{ + /* Save the value of the boot key memory before it is overwritten */ + uint16_t bootKeyPtrVal = *bootKeyPtr; + *bootKeyPtr = 0; + + /* Check the reason for the reset so we can act accordingly */ + uint8_t mcusr_state = MCUSR; // store the initial state of the Status register + MCUSR = 0; // clear all reset flags + + /* Watchdog may be configured with a 15 ms period so must disable it before going any further */ + wdt_disable(); + + if (mcusr_state & (1< TIMEOUT_PERIOD) + RunBootloader = false; + + LEDPulse(); + } + + /* Disconnect from the host - USB interface will be reset later along with the AVR */ + USB_Detach(); + + /* Jump to beginning of application space to run the sketch - do not reset */ + StartSketch(); +} + +/** Configures all hardware required for the bootloader. */ +void SetupHardware(void) +{ + /* Disable watchdog if enabled by bootloader/fuses */ + MCUSR &= ~(1 << WDRF); + wdt_disable(); + + /* Disable clock division */ + clock_prescale_set(clock_div_1); + + /* Relocate the interrupt vector table to the bootloader section */ + MCUCR = (1 << IVCE); + MCUCR = (1 << IVSEL); + + LED_SETUP(); + CPU_PRESCALE(0); + L_LED_OFF(); + TX_LED_OFF(); + RX_LED_OFF(); + + /* Initialize TIMER1 to handle bootloader timeout and LED tasks. + * With 16 MHz clock and 1/64 prescaler, timer 1 is clocked at 250 kHz + * Our chosen compare match generates an interrupt every 1 ms. + * This interrupt is disabled selectively when doing memory reading, erasing, + * or writing since SPM has tight timing requirements. + */ + OCR1AH = 0; + OCR1AL = 250; + TIMSK1 = (1 << OCIE1A); // enable timer 1 output compare A match interrupt + TCCR1B = ((1 << CS11) | (1 << CS10)); // 1/64 prescaler on timer 1 input + + /* Initialize USB Subsystem */ + USB_Init(); +} + +//uint16_t ctr = 0; +ISR(TIMER1_COMPA_vect, ISR_BLOCK) +{ + /* Reset counter */ + TCNT1H = 0; + TCNT1L = 0; + + /* Check whether the TX or RX LED one-shot period has elapsed. if so, turn off the LED */ + if (TxLEDPulse && !(--TxLEDPulse)) + TX_LED_OFF(); + if (RxLEDPulse && !(--RxLEDPulse)) + RX_LED_OFF(); + + if (pgm_read_word(0) != 0xFFFF) + Timeout++; +} + +/** Event handler for the USB_ConfigurationChanged event. This configures the device's endpoints ready + * to relay data to and from the attached USB host. + */ +void EVENT_USB_Device_ConfigurationChanged(void) +{ + /* Setup CDC Notification, Rx and Tx Endpoints */ + Endpoint_ConfigureEndpoint(CDC_NOTIFICATION_EPNUM, EP_TYPE_INTERRUPT, + ENDPOINT_DIR_IN, CDC_NOTIFICATION_EPSIZE, + ENDPOINT_BANK_SINGLE); + + Endpoint_ConfigureEndpoint(CDC_TX_EPNUM, EP_TYPE_BULK, + ENDPOINT_DIR_IN, CDC_TXRX_EPSIZE, + ENDPOINT_BANK_SINGLE); + + Endpoint_ConfigureEndpoint(CDC_RX_EPNUM, EP_TYPE_BULK, + ENDPOINT_DIR_OUT, CDC_TXRX_EPSIZE, + ENDPOINT_BANK_SINGLE); +} + +/** Event handler for the USB_ControlRequest event. This is used to catch and process control requests sent to + * the device from the USB host before passing along unhandled control requests to the library for processing + * internally. + */ +void EVENT_USB_Device_ControlRequest(void) +{ + /* Ignore any requests that aren't directed to the CDC interface */ + if ((USB_ControlRequest.bmRequestType & (CONTROL_REQTYPE_TYPE | CONTROL_REQTYPE_RECIPIENT)) != + (REQTYPE_CLASS | REQREC_INTERFACE)) + { + return; + } + + /* Process CDC specific control requests */ + switch (USB_ControlRequest.bRequest) + { + case CDC_REQ_GetLineEncoding: + if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE)) + { + Endpoint_ClearSETUP(); + + /* Write the line coding data to the control endpoint */ + Endpoint_Write_Control_Stream_LE(&LineEncoding, sizeof(CDC_LineEncoding_t)); + Endpoint_ClearOUT(); + } + + break; + case CDC_REQ_SetLineEncoding: + if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE)) + { + Endpoint_ClearSETUP(); + + /* Read the line coding data in from the host into the global struct */ + Endpoint_Read_Control_Stream_LE(&LineEncoding, sizeof(CDC_LineEncoding_t)); + Endpoint_ClearIN(); + } + + break; + } +} + +#if !defined(NO_BLOCK_SUPPORT) +/** Reads or writes a block of EEPROM or FLASH memory to or from the appropriate CDC data endpoint, depending + * on the AVR910 protocol command issued. + * + * \param[in] Command Single character AVR910 protocol command indicating what memory operation to perform + */ +static void ReadWriteMemoryBlock(const uint8_t Command) +{ + uint16_t BlockSize; + char MemoryType; + + bool HighByte = false; + uint8_t LowByte = 0; + + BlockSize = (FetchNextCommandByte() << 8); + BlockSize |= FetchNextCommandByte(); + + MemoryType = FetchNextCommandByte(); + + if ((MemoryType != 'E') && (MemoryType != 'F')) + { + /* Send error byte back to the host */ + WriteNextResponseByte('?'); + + return; + } + + /* Disable timer 1 interrupt - can't afford to process nonessential interrupts + * while doing SPM tasks */ + TIMSK1 = 0; + + /* Check if command is to read memory */ + if (Command == 'g') + { + /* Re-enable RWW section */ + boot_rww_enable(); + + while (BlockSize--) + { + if (MemoryType == 'F') + { + /* Read the next FLASH byte from the current FLASH page */ + #if (FLASHEND > 0xFFFF) + WriteNextResponseByte(pgm_read_byte_far(CurrAddress | HighByte)); + #else + WriteNextResponseByte(pgm_read_byte(CurrAddress | HighByte)); + #endif + + /* If both bytes in current word have been read, increment the address counter */ + if (HighByte) + CurrAddress += 2; + + HighByte = !HighByte; + } + else + { + /* Read the next EEPROM byte into the endpoint */ + WriteNextResponseByte(eeprom_read_byte((uint8_t*)(intptr_t)(CurrAddress >> 1))); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + } + } + else + { + uint32_t PageStartAddress = CurrAddress; + + if (MemoryType == 'F') + { + boot_page_erase(PageStartAddress); + boot_spm_busy_wait(); + } + + while (BlockSize--) + { + if (MemoryType == 'F') + { + /* If both bytes in current word have been written, increment the address counter */ + if (HighByte) + { + /* Write the next FLASH word to the current FLASH page */ + boot_page_fill(CurrAddress, ((FetchNextCommandByte() << 8) | LowByte)); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + else + { + LowByte = FetchNextCommandByte(); + } + + HighByte = !HighByte; + } + else + { + /* Write the next EEPROM byte from the endpoint */ + eeprom_write_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)), FetchNextCommandByte()); + + /* Increment the address counter after use */ + CurrAddress += 2; + } + } + + /* If in FLASH programming mode, commit the page after writing */ + if (MemoryType == 'F') + { + /* Commit the flash page to memory */ + boot_page_write(PageStartAddress); + + /* Wait until write operation has completed */ + boot_spm_busy_wait(); + } + + /* Send response byte back to the host */ + WriteNextResponseByte('\r'); + } + + /* Re-enable timer 1 interrupt disabled earlier in this routine */ + TIMSK1 = (1 << OCIE1A); +} +#endif + +/** Retrieves the next byte from the host in the CDC data OUT endpoint, and clears the endpoint bank if needed + * to allow reception of the next data packet from the host. + * + * \return Next received byte from the host in the CDC data OUT endpoint + */ +static uint8_t FetchNextCommandByte(void) +{ + /* Select the OUT endpoint so that the next data byte can be read */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* If OUT endpoint empty, clear it and wait for the next packet from the host */ + while (!(Endpoint_IsReadWriteAllowed())) + { + Endpoint_ClearOUT(); + + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return 0; + } + } + + /* Fetch the next byte from the OUT endpoint */ + return Endpoint_Read_8(); +} + +/** Writes the next response byte to the CDC data IN endpoint, and sends the endpoint back if needed to free up the + * bank when full ready for the next byte in the packet to the host. + * + * \param[in] Response Next response byte to send to the host + */ +static void WriteNextResponseByte(const uint8_t Response) +{ + /* Select the IN endpoint so that the next data byte can be written */ + Endpoint_SelectEndpoint(CDC_TX_EPNUM); + + /* If IN endpoint full, clear it and wait until ready for the next packet to the host */ + if (!(Endpoint_IsReadWriteAllowed())) + { + Endpoint_ClearIN(); + + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Write the next byte to the IN endpoint */ + Endpoint_Write_8(Response); + + TX_LED_ON(); + TxLEDPulse = TX_RX_LED_PULSE_PERIOD; +} + +#define STK_OK 0x10 +#define STK_INSYNC 0x14 // ' ' +#define CRC_EOP 0x20 // 'SPACE' +#define STK_GET_SYNC 0x30 // '0' + +#define STK_GET_PARAMETER 0x41 // 'A' +#define STK_SET_DEVICE 0x42 // 'B' +#define STK_SET_DEVICE_EXT 0x45 // 'E' +#define STK_LOAD_ADDRESS 0x55 // 'U' +#define STK_UNIVERSAL 0x56 // 'V' +#define STK_PROG_PAGE 0x64 // 'd' +#define STK_READ_PAGE 0x74 // 't' +#define STK_READ_SIGN 0x75 // 'u' + +/** Task to read in AVR910 commands from the CDC data OUT endpoint, process them, perform the required actions + * and send the appropriate response back to the host. + */ +void CDC_Task(void) +{ + /* Select the OUT endpoint */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* Check if endpoint has a command in it sent from the host */ + if (!(Endpoint_IsOUTReceived())) + return; + + RX_LED_ON(); + RxLEDPulse = TX_RX_LED_PULSE_PERIOD; + + /* Read in the bootloader command (first byte sent from host) */ + uint8_t Command = FetchNextCommandByte(); + + if (Command == 'E') + { + /* We nearly run out the bootloader timeout clock, + * leaving just a few hundred milliseconds so the + * bootloder has time to respond and service any + * subsequent requests */ + Timeout = TIMEOUT_PERIOD - 500; + + /* Re-enable RWW section - must be done here in case + * user has disabled verification on upload. */ + boot_rww_enable_safe(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'T') + { + FetchNextCommandByte(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if ((Command == 'L') || (Command == 'P')) + { + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 't') + { + // Return ATMEGA128 part code - this is only to allow AVRProg to use the bootloader + WriteNextResponseByte(0x44); + WriteNextResponseByte(0x00); + } + else if (Command == 'a') + { + // Indicate auto-address increment is supported + WriteNextResponseByte('Y'); + } + else if (Command == 'A') + { + // Set the current address to that given by the host + CurrAddress = (FetchNextCommandByte() << 9); + CurrAddress |= (FetchNextCommandByte() << 1); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'p') + { + // Indicate serial programmer back to the host + WriteNextResponseByte('S'); + } + else if (Command == 'S') + { + // Write the 7-byte software identifier to the endpoint + for (uint8_t CurrByte = 0; CurrByte < 7; CurrByte++) + WriteNextResponseByte(SOFTWARE_IDENTIFIER[CurrByte]); + } + else if (Command == 'V') + { + WriteNextResponseByte('0' + BOOTLOADER_VERSION_MAJOR); + WriteNextResponseByte('0' + BOOTLOADER_VERSION_MINOR); + } + else if (Command == 's') + { + WriteNextResponseByte(AVR_SIGNATURE_3); + WriteNextResponseByte(AVR_SIGNATURE_2); + WriteNextResponseByte(AVR_SIGNATURE_1); + } + else if (Command == 'e') + { + // Clear the application section of flash + for (uint32_t CurrFlashAddress = 0; CurrFlashAddress < BOOT_START_ADDR; CurrFlashAddress += SPM_PAGESIZE) + { + boot_page_erase(CurrFlashAddress); + boot_spm_busy_wait(); + boot_page_write(CurrFlashAddress); + boot_spm_busy_wait(); + } + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + #if !defined(NO_LOCK_BYTE_WRITE_SUPPORT) + else if (Command == 'l') + { + // Set the lock bits to those given by the host + boot_lock_bits_set(FetchNextCommandByte()); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + #endif + else if (Command == 'r') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_LOCK_BITS)); + } + else if (Command == 'F') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS)); + } + else if (Command == 'N') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS)); + } + else if (Command == 'Q') + { + WriteNextResponseByte(boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS)); + } + #if !defined(NO_BLOCK_SUPPORT) + else if (Command == 'b') + { + WriteNextResponseByte('Y'); + + // Send block size to the host + WriteNextResponseByte(SPM_PAGESIZE >> 8); + WriteNextResponseByte(SPM_PAGESIZE & 0xFF); + } + else if ((Command == 'B') || (Command == 'g')) + { + // Keep resetting the timeout counter if we're receiving self-programming instructions + Timeout = 0; + // Delegate the block write/read to a separate function for clarity + ReadWriteMemoryBlock(Command); + } + #endif + #if !defined(NO_FLASH_BYTE_SUPPORT) + else if (Command == 'C') + { + // Write the high byte to the current flash page + boot_page_fill(CurrAddress, FetchNextCommandByte()); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'c') + { + // Write the low byte to the current flash page + boot_page_fill(CurrAddress | 0x01, FetchNextCommandByte()); + + // Increment the address + CurrAddress += 2; + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'm') + { + // Commit the flash page to memory + boot_page_write(CurrAddress); + + // Wait until write operation has completed + boot_spm_busy_wait(); + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'R') + { + #if (FLASHEND > 0xFFFF) + uint16_t ProgramWord = pgm_read_word_far(CurrAddress); + #else + uint16_t ProgramWord = pgm_read_word(CurrAddress); + #endif + + WriteNextResponseByte(ProgramWord >> 8); + WriteNextResponseByte(ProgramWord & 0xFF); + } + #endif + #if !defined(NO_EEPROM_BYTE_SUPPORT) + else if (Command == 'D') + { + // Read the byte from the endpoint and write it to the EEPROM + eeprom_write_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)), FetchNextCommandByte()); + + // Increment the address after use + CurrAddress += 2; + + // Send confirmation byte back to the host + WriteNextResponseByte('\r'); + } + else if (Command == 'd') + { + // Read the EEPROM byte and write it to the endpoint + WriteNextResponseByte(eeprom_read_byte((uint8_t*)((intptr_t)(CurrAddress >> 1)))); + + // Increment the address after use + CurrAddress += 2; + } + #endif + else if (Command != 27) + { + // Unknown (non-sync) command, return fail code + WriteNextResponseByte('?'); + } + + + /* Select the IN endpoint */ + Endpoint_SelectEndpoint(CDC_TX_EPNUM); + + /* Remember if the endpoint is completely full before clearing it */ + bool IsEndpointFull = !(Endpoint_IsReadWriteAllowed()); + + /* Send the endpoint data to the host */ + Endpoint_ClearIN(); + + /* If a full endpoint's worth of data was sent, we need to send an empty packet afterwards to signal end of transfer */ + if (IsEndpointFull) + { + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + Endpoint_ClearIN(); + } + + /* Wait until the data has been sent to the host */ + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + /* Select the OUT endpoint */ + Endpoint_SelectEndpoint(CDC_RX_EPNUM); + + /* Acknowledge the command from the host */ + Endpoint_ClearOUT(); +} + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.h b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.h new file mode 100644 index 0000000..67ff1b3 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Caterina.h @@ -0,0 +1,106 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for BootloaderCDC.c. + */ + +#ifndef _CDC_H_ +#define _CDC_H_ + + /* Includes: */ + #include + #include + #include + #include + #include + #include + #include + + #include "Descriptors.h" + + #include + /* Macros: */ + /** Version major of the CDC bootloader. */ + #define BOOTLOADER_VERSION_MAJOR 0x01 + + /** Version minor of the CDC bootloader. */ + #define BOOTLOADER_VERSION_MINOR 0x00 + + /** Hardware version major of the CDC bootloader. */ + #define BOOTLOADER_HWVERSION_MAJOR 0x01 + + /** Hardware version minor of the CDC bootloader. */ + #define BOOTLOADER_HWVERSION_MINOR 0x00 + + /** Eight character bootloader firmware identifier reported to the host when requested */ + #define SOFTWARE_IDENTIFIER "CATERINA" + + #define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n)) + #define LED_SETUP() DDRC |= (1<<7); DDRB |= (1<<0); DDRD |= (1<<5); + #define L_LED_OFF() PORTC &= ~(1<<7) + #define L_LED_ON() PORTC |= (1<<7) + #define L_LED_TOGGLE() PORTC ^= (1<<7) + #if DEVICE_PID == 0x0037 // polarity of the RX and TX LEDs is reversed on the Micro + #define TX_LED_OFF() PORTD &= ~(1<<5) + #define TX_LED_ON() PORTD |= (1<<5) + #define RX_LED_OFF() PORTB &= ~(1<<0) + #define RX_LED_ON() PORTB |= (1<<0) + #else + #define TX_LED_OFF() PORTD |= (1<<5) + #define TX_LED_ON() PORTD &= ~(1<<5) + #define RX_LED_OFF() PORTB |= (1<<0) + #define RX_LED_ON() PORTB &= ~(1<<0) + #endif + + /* Type Defines: */ + /** Type define for a non-returning pointer to the start of the loaded application in flash memory. */ + typedef void (*AppPtr_t)(void) ATTR_NO_RETURN; + + /* Function Prototypes: */ + void StartSketch(void); + void LEDPulse(void); + + void CDC_Task(void); + void SetupHardware(void); + + void EVENT_USB_Device_ConfigurationChanged(void); + + #if defined(INCLUDE_FROM_CATERINA_C) || defined(__DOXYGEN__) + #if !defined(NO_BLOCK_SUPPORT) + static void ReadWriteMemoryBlock(const uint8_t Command); + #endif + static uint8_t FetchNextCommandByte(void); + static void WriteNextResponseByte(const uint8_t Response); + #endif + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.c b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.c new file mode 100644 index 0000000..9ca7de4 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.c @@ -0,0 +1,266 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * USB Device Descriptors, for library use when in USB device mode. Descriptors are special + * computer-readable structures which the host requests upon device enumeration, to determine + * the device's capabilities and functions. + */ + +#include "Descriptors.h" + +/** Device descriptor structure. This descriptor, located in SRAM memory, describes the overall + * device characteristics, including the supported USB version, control endpoint size and the + * number of device configurations. The descriptor is read out by the USB host when the enumeration + * process begins. + */ +const USB_Descriptor_Device_t DeviceDescriptor = +{ + .Header = {.Size = sizeof(USB_Descriptor_Device_t), .Type = DTYPE_Device}, + + .USBSpecification = VERSION_BCD(01.10), + .Class = CDC_CSCP_CDCClass, + .SubClass = CDC_CSCP_NoSpecificSubclass, + .Protocol = CDC_CSCP_NoSpecificProtocol, + + .Endpoint0Size = FIXED_CONTROL_ENDPOINT_SIZE, + + .VendorID = DEVICE_VID, + .ProductID = DEVICE_PID, + .ReleaseNumber = VERSION_BCD(00.01), + + .ManufacturerStrIndex = 0x02, + .ProductStrIndex = 0x01, + .SerialNumStrIndex = NO_DESCRIPTOR, + + .NumberOfConfigurations = FIXED_NUM_CONFIGURATIONS +}; + +/** Configuration descriptor structure. This descriptor, located in SRAM memory, describes the usage + * of the device in one of its supported configurations, including information about any device interfaces + * and endpoints. The descriptor is read out by the USB host during the enumeration process when selecting + * a configuration so that the host may correctly communicate with the USB device. + */ +const USB_Descriptor_Configuration_t ConfigurationDescriptor = +{ + .Config = + { + .Header = {.Size = sizeof(USB_Descriptor_Configuration_Header_t), .Type = DTYPE_Configuration}, + + .TotalConfigurationSize = sizeof(USB_Descriptor_Configuration_t), + .TotalInterfaces = 2, + + .ConfigurationNumber = 1, + .ConfigurationStrIndex = NO_DESCRIPTOR, + + .ConfigAttributes = USB_CONFIG_ATTR_BUSPOWERED, + + .MaxPowerConsumption = USB_CONFIG_POWER_MA(100) + }, + + .CDC_CCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 0, + .AlternateSetting = 0, + + .TotalEndpoints = 1, + + .Class = CDC_CSCP_CDCClass, + .SubClass = CDC_CSCP_ACMSubclass, + .Protocol = CDC_CSCP_ATCommandProtocol, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_Functional_Header = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalHeader_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x00, + + .CDCSpecification = VERSION_BCD(01.10), + }, + + .CDC_Functional_ACM = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalACM_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x02, + + .Capabilities = 0x04, + }, + + .CDC_Functional_Union = + { + .Header = {.Size = sizeof(USB_CDC_Descriptor_FunctionalUnion_t), .Type = DTYPE_CSInterface}, + .Subtype = 0x06, + + .MasterInterfaceNumber = 0, + .SlaveInterfaceNumber = 1, + }, + + .CDC_NotificationEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_IN | CDC_NOTIFICATION_EPNUM), + .Attributes = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_NOTIFICATION_EPSIZE, + .PollingIntervalMS = 0xFF + }, + + .CDC_DCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 1, + .AlternateSetting = 0, + + .TotalEndpoints = 2, + + .Class = CDC_CSCP_CDCDataClass, + .SubClass = CDC_CSCP_NoDataSubclass, + .Protocol = CDC_CSCP_NoDataProtocol, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_DataOutEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_OUT | CDC_RX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + }, + + .CDC_DataInEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DIR_IN | CDC_TX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + } +}; + +/** Language descriptor structure. This descriptor, located in SRAM memory, is returned when the host requests + * the string descriptor with index 0 (the first index). It is actually an array of 16-bit integers, which indicate + * via the language ID table available at USB.org what languages the device supports for its string descriptors. + */ +const USB_Descriptor_String_t LanguageString = +{ + .Header = {.Size = USB_STRING_LEN(1), .Type = DTYPE_String}, + + .UnicodeString = {LANGUAGE_ID_ENG} +}; + +/** Product descriptor string. This is a Unicode string containing the product's details in human readable form, + * and is read out upon request by the host when the appropriate string ID is requested, listed in the Device + * Descriptor. + */ +const USB_Descriptor_String_t ProductString = +{ + .Header = {.Size = USB_STRING_LEN(16), .Type = DTYPE_String}, + + #if DEVICE_PID == 0x0036 + .UnicodeString = L"Arduino Leonardo" + #elif DEVICE_PID == 0x0037 + .UnicodeString = L"Arduino Micro " + #elif DEVICE_PID == 0x003C + .UnicodeString = L"Arduino Esplora " + #else + .UnicodeString = L"USB IO board " + #endif +}; + +const USB_Descriptor_String_t ManufNameString = +{ + .Header = {.Size = USB_STRING_LEN(11), .Type = DTYPE_String}, + + #if DEVICE_VID == 0x2341 + .UnicodeString = L"Arduino LLC" + #else + .UnicodeString = L"Unknown " + #endif +}; + +/** This function is called by the library when in device mode, and must be overridden (see LUFA library "USB Descriptors" + * documentation) by the application code so that the address and size of a requested descriptor can be given + * to the USB library. When the device receives a Get Descriptor request on the control endpoint, this function + * is called so that the descriptor details can be passed back and the appropriate descriptor sent back to the + * USB host. + */ +uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + const void** const DescriptorAddress) +{ + const uint8_t DescriptorType = (wValue >> 8); + const uint8_t DescriptorNumber = (wValue & 0xFF); + + const void* Address = NULL; + uint16_t Size = NO_DESCRIPTOR; + + switch (DescriptorType) + { + case DTYPE_Device: + Address = &DeviceDescriptor; + Size = sizeof(USB_Descriptor_Device_t); + break; + case DTYPE_Configuration: + Address = &ConfigurationDescriptor; + Size = sizeof(USB_Descriptor_Configuration_t); + break; + case DTYPE_String: + if (!(DescriptorNumber)) + { + Address = &LanguageString; + Size = LanguageString.Header.Size; + } + else if (DescriptorNumber == DeviceDescriptor.ProductStrIndex) + { + Address = &ProductString; + Size = ProductString.Header.Size; + } else if (DescriptorNumber == DeviceDescriptor.ManufacturerStrIndex) + { + Address = &ManufNameString; + Size = ManufNameString.Header.Size; + } + + break; + } + + *DescriptorAddress = Address; + return Size; +} + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.h b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.h new file mode 100644 index 0000000..c843bec --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Descriptors.h @@ -0,0 +1,139 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2011. + + dean [at] fourwalledcubicle [dot] com + www.lufa-lib.org +*/ + +/* + Copyright 2011 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Descriptors.c. + */ + +#ifndef _DESCRIPTORS_H_ +#define _DESCRIPTORS_H_ + + /* Includes: */ + #include + + /* Macros: */ + #if defined(__AVR_AT90USB1287__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB647__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB1286__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB646__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_ATmega32U6__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega32U4__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x87 + #elif defined(__AVR_ATmega16U4__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega32U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x8A + #elif defined(__AVR_ATmega16U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_AT90USB162__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_ATmega8U2__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x93 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_AT90USB82__) + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x82 + #else + #error The selected AVR part is not currently supported by this bootloader. + #endif + + /** Endpoint number for the CDC control interface event notification endpoint. */ + #define CDC_NOTIFICATION_EPNUM 2 + + /** Endpoint number for the CDC data interface TX (data IN) endpoint. */ + #define CDC_TX_EPNUM 3 + + /** Endpoint number for the CDC data interface RX (data OUT) endpoint. */ + #define CDC_RX_EPNUM 4 + + /** Size of the CDC data interface TX and RX data endpoint banks, in bytes. */ + #define CDC_TXRX_EPSIZE 16 + + /** Size of the CDC control interface notification endpoint bank, in bytes. */ + #define CDC_NOTIFICATION_EPSIZE 8 + + /* Type Defines: */ + /** Type define for the device configuration descriptor structure. This must be defined in the + * application code, as the configuration descriptor contains several sub-descriptors which + * vary between devices, and which describe the device's usage to the host. + */ + typedef struct + { + USB_Descriptor_Configuration_Header_t Config; + + // CDC Control Interface + USB_Descriptor_Interface_t CDC_CCI_Interface; + USB_CDC_Descriptor_FunctionalHeader_t CDC_Functional_Header; + USB_CDC_Descriptor_FunctionalACM_t CDC_Functional_ACM; + USB_CDC_Descriptor_FunctionalUnion_t CDC_Functional_Union; + USB_Descriptor_Endpoint_t CDC_NotificationEndpoint; + + // CDC Data Interface + USB_Descriptor_Interface_t CDC_DCI_Interface; + USB_Descriptor_Endpoint_t CDC_DataOutEndpoint; + USB_Descriptor_Endpoint_t CDC_DataInEndpoint; + } USB_Descriptor_Configuration_t; + + /* Function Prototypes: */ + uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + const void** const DescriptorAddress) + ATTR_WARN_UNUSED_RESULT ATTR_NON_NULL_PTR_ARG(3); + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Esplora-prod-firmware-2012-12-10.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Esplora-prod-firmware-2012-12-10.txt new file mode 100644 index 0000000..aef5df1 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Esplora-prod-firmware-2012-12-10.txt @@ -0,0 +1,6 @@ +LUFA: 111009 +make: 3.81 +avrdude: 5.11.1 +avr-libc: 1.6.7 +binutils-avr: 2.19 +gcc-avr 4.3.3 diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-04-26.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-04-26.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-04-26.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-12-10.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-12-10.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Leonardo-prod-firmware-2012-12-10.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Makefile b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Makefile new file mode 100644 index 0000000..884e304 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Makefile @@ -0,0 +1,732 @@ +# Hey Emacs, this is a -*- makefile -*- +#---------------------------------------------------------------------------- +# WinAVR Makefile Template written by Eric B. Weddington, Jrg Wunsch, et al. +# >> Modified for use with the LUFA project. << +# +# Released to the Public Domain +# +# Additional material for this makefile was written by: +# Peter Fleury +# Tim Henigan +# Colin O'Flynn +# Reiner Patommel +# Markus Pfaff +# Sander Pool +# Frederik Rouleau +# Carlos Lamas +# Dean Camera +# Opendous Inc. +# Denver Gingerich +# +#---------------------------------------------------------------------------- +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF. +# +# make extcoff = Convert ELF to AVR Extended COFF. +# +# make program = Download the hex file to the device, using avrdude. +# Please customize the avrdude settings below first! +# +# make doxygen = Generate DoxyGen documentation for the project (must have +# DoxyGen installed) +# +# make debug = Start either simulavr or avarice as specified for debugging, +# with avr-gdb or avr-insight as the front end for debugging. +# +# make filename.s = Just compile filename.c into the assembler code only. +# +# make filename.i = Create a preprocessed source file for use in submitting +# bug reports to the GCC project. +# +# To rebuild project do "make clean" then "make all". +#---------------------------------------------------------------------------- + +# USB vendor ID (VID) +# reuse of this VID by others is forbidden by USB-IF +# official Arduino LLC VID +# VID = 0x2341 + + +# USB product ID (PID) +# official Leonardo PID +# PID = 0x0036 +# official Micro PID +# PID = 0x0037 +# official Esplora PID +# PID = 0x003C + +# MCU name +MCU = atmega32u4 + + +# Target architecture (see library "Board Types" documentation). +ARCH = AVR8 + + +# Target board (see library "Board Types" documentation, NONE for projects not requiring +# LUFA board drivers). If USER is selected, put custom board drivers in a directory called +# "Board" inside the application directory. +BOARD = USER + + +# Processor frequency. +# This will define a symbol, F_CPU, in all source code files equal to the +# processor frequency in Hz. You can then use this symbol in your source code to +# calculate timings. Do NOT tack on a 'UL' at the end, this will be done +# automatically to create a 32-bit value in your source code. +# +# This will be an integer division of F_USB below, as it is sourced by +# F_USB after it has run through any CPU prescalers. Note that this value +# does not *change* the processor frequency - it should merely be updated to +# reflect the processor speed set externally so that the code can use accurate +# software delays. +F_CPU = 16000000 + + +# Input clock frequency. +# This will define a symbol, F_USB, in all source code files equal to the +# input clock frequency (before any prescaling is performed) in Hz. This value may +# differ from F_CPU if prescaling is used on the latter, and is required as the +# raw input clock is fed directly to the PLL sections of the AVR for high speed +# clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL' +# at the end, this will be done automatically to create a 32-bit value in your +# source code. +# +# If no clock division is performed on the input clock inside the AVR (via the +# CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU. +F_USB = $(F_CPU) + + +# Starting byte address of the bootloader, as a byte address - computed via the formula +# BOOT_START = ((FLASH_SIZE_KB - BOOT_SECTION_SIZE_KB) * 1024) +# +# Note that the bootloader size and start address given in AVRStudio is in words and not +# bytes, and so will need to be doubled to obtain the byte address needed by AVR-GCC. +FLASH_SIZE_KB = 32 +BOOT_SECTION_SIZE_KB = 4 +BOOT_START = 0x$(shell echo "obase=16; ($(FLASH_SIZE_KB) - $(BOOT_SECTION_SIZE_KB)) * 1024" | bc) + + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + + +# Target file name (without extension). +TARGET = Caterina + + +# Object files directory +# To put object files in current directory, use a dot (.), do NOT make +# this an empty or blank macro! +OBJDIR = . + + +# Path to the LUFA library +LUFA_PATH = ../../../../../../LUFA/LUFA-111009 + + +# LUFA library compile-time options and predefined tokens +LUFA_OPTS = -D USB_DEVICE_ONLY +LUFA_OPTS += -D DEVICE_STATE_AS_GPIOR=0 +LUFA_OPTS += -D ORDERED_EP_CONFIG +LUFA_OPTS += -D FIXED_CONTROL_ENDPOINT_SIZE=8 +LUFA_OPTS += -D FIXED_NUM_CONFIGURATIONS=1 +LUFA_OPTS += -D USE_RAM_DESCRIPTORS +LUFA_OPTS += -D USE_STATIC_OPTIONS="(USB_DEVICE_OPT_FULLSPEED | USB_OPT_REG_ENABLED | USB_OPT_AUTO_PLL)" +LUFA_OPTS += -D NO_INTERNAL_SERIAL +LUFA_OPTS += -D NO_DEVICE_SELF_POWER +LUFA_OPTS += -D NO_DEVICE_REMOTE_WAKEUP +LUFA_OPTS += -D NO_SOF_EVENTS + +#LUFA_OPTS += -D NO_BLOCK_SUPPORT +#LUFA_OPTS += -D NO_EEPROM_BYTE_SUPPORT +#LUFA_OPTS += -D NO_FLASH_BYTE_SUPPORT +LUFA_OPTS += -D NO_LOCK_BYTE_WRITE_SUPPORT + + +# Create the LUFA source path variables by including the LUFA root makefile +include $(LUFA_PATH)/LUFA/makefile + + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c \ + Descriptors.c \ + $(LUFA_SRC_USB) \ + + +# List C++ source files here. (C dependencies are automatically generated.) +CPPSRC = + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + + +# Debugging format. +# Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. +# AVR Studio 4.10 requires dwarf-2. +# AVR [Extended] COFF format requires stabs, plus an avr-objcopy run. +DEBUG = dwarf-2 + + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRAINCDIRS = $(LUFA_PATH)/ + + +# Compiler flag to set the C Standard level. +# c89 = "ANSI" C +# gnu89 = c89 plus GCC extensions +# c99 = ISO C99 standard (not yet fully implemented) +# gnu99 = c99 plus GCC extensions +CSTANDARD = -std=c99 + + +# Place -D or -U options here for C sources +CDEFS = -DF_CPU=$(F_CPU)UL +CDEFS += -DF_USB=$(F_USB)UL +CDEFS += -DBOARD=BOARD_$(BOARD) -DARCH=ARCH_$(ARCH) +CDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CDEFS += -DDEVICE_VID=$(VID)UL +CDEFS += -DDEVICE_PID=$(PID)UL +CDEFS += $(LUFA_OPTS) + + +# Place -D or -U options here for ASM sources +ADEFS = -DF_CPU=$(F_CPU) +ADEFS += -DF_USB=$(F_USB)UL +ADEFS += -DBOARD=BOARD_$(BOARD) +ADEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +ADEFS += $(LUFA_OPTS) + + +# Place -D or -U options here for C++ sources +CPPDEFS = -DF_CPU=$(F_CPU)UL +CPPDEFS += -DF_USB=$(F_USB)UL +CPPDEFS += -DBOARD=BOARD_$(BOARD) +CPPDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CPPDEFS += $(LUFA_OPTS) +#CPPDEFS += -D__STDC_LIMIT_MACROS +#CPPDEFS += -D__STDC_CONSTANT_MACROS + + + +#---------------- Compiler Options C ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char +CFLAGS += -funsigned-bitfields +CFLAGS += -ffunction-sections +CFLAGS += -fno-inline-small-functions +CFLAGS += -fpack-struct +CFLAGS += -fshort-enums +CFLAGS += -fno-strict-aliasing +CFLAGS += -Wall +CFLAGS += -Wstrict-prototypes +#CFLAGS += -mshort-calls +#CFLAGS += -fno-unit-at-a-time +#CFLAGS += -Wundef +#CFLAGS += -Wunreachable-code +#CFLAGS += -Wsign-compare +CFLAGS += -Wa,-adhlns=$(<:%.c=$(OBJDIR)/%.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) + + +#---------------- Compiler Options C++ ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CPPFLAGS = -g$(DEBUG) +CPPFLAGS += $(CPPDEFS) +CPPFLAGS += -O$(OPT) +CPPFLAGS += -funsigned-char +CPPFLAGS += -funsigned-bitfields +CPPFLAGS += -fpack-struct +CPPFLAGS += -fshort-enums +CPPFLAGS += -fno-exceptions +CPPFLAGS += -Wall +CPPFLAGS += -Wundef +#CPPFLAGS += -mshort-calls +#CPPFLAGS += -fno-unit-at-a-time +#CPPFLAGS += -Wstrict-prototypes +#CPPFLAGS += -Wunreachable-code +#CPPFLAGS += -Wsign-compare +CPPFLAGS += -Wa,-adhlns=$(<:%.cpp=$(OBJDIR)/%.lst) +CPPFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +#CPPFLAGS += $(CSTANDARD) + + +#---------------- Assembler Options ---------------- +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +# -listing-cont-lines: Sets the maximum number of continuation lines of hex +# dump that will be displayed for a given single line of source input. +ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:%.S=$(OBJDIR)/%.lst),-gstabs,--listing-cont-lines=100 + + +#---------------- Library Options ---------------- +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt + +# If this is left blank, then it will use the Standard printf version. +PRINTF_LIB = +#PRINTF_LIB = $(PRINTF_LIB_MIN) +#PRINTF_LIB = $(PRINTF_LIB_FLOAT) + + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +# If this is left blank, then it will use the Standard scanf version. +SCANF_LIB = +#SCANF_LIB = $(SCANF_LIB_MIN) +#SCANF_LIB = $(SCANF_LIB_FLOAT) + + +MATH_LIB = -lm + + +# List any extra directories to look for libraries here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRALIBDIRS = + + + +#---------------- External Memory Options ---------------- + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--section-start,.data=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + + + +#---------------- Linker Options ---------------- +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += -Wl,--section-start=.text=$(BOOT_START) +LDFLAGS += -Wl,--relax +LDFLAGS += -Wl,--gc-sections +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(patsubst %,-L%,$(EXTRALIBDIRS)) +LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) +#LDFLAGS += -T linker_script.x + + + +#---------------- Programming Options (avrdude) ---------------- + +# Programming hardware +# Type: avrdude -c ? +# to get a full listing. +# +AVRDUDE_PROGRAMMER = avrispmkII + +# com1 = serial port. Use lpt1 to connect to parallel port. +AVRDUDE_PORT = usb + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +#---------------- Debugging Options ---------------- + +# For simulavr only - target MCU frequency. +DEBUG_MFREQ = $(F_CPU) + +# Set the DEBUG_UI to either gdb or insight. +# DEBUG_UI = gdb +DEBUG_UI = insight + +# Set the debugging back-end to either avarice, simulavr. +DEBUG_BACKEND = avarice +#DEBUG_BACKEND = simulavr + +# GDB Init Filename. +GDBINIT_FILE = __avr_gdbinit + +# When using avarice settings for the JTAG +JTAG_DEV = /dev/com1 + +# Debugging port used to communicate between GDB / avarice / simulavr. +DEBUG_PORT = 4242 + +# Debugging host used to communicate between GDB / avarice / simulavr, normally +# just set to localhost unless doing some sort of crazy debugging when +# avarice is running on a different computer. +DEBUG_HOST = localhost + + + +#============================================================================ + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +AR = avr-ar rcs +NM = avr-nm +AVRDUDE = /Applications/avrdude -C /Applications/avrdude.conf -B 1 +REMOVE = rm -f +REMOVEDIR = rm -rf +COPY = cp +WINSHELL = cmd + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling C: +MSG_COMPILING_CPP = Compiling C++: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: +MSG_CREATING_LIBRARY = Creating library: + + + + +# Define all object files. +OBJ = $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + +# Define all listing files. +LST = $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + + +# Compiler flags to generate dependency files. +GENDEPFLAGS = -MMD -MP -MF .dep/$(@F).d + + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_CPPFLAGS = -mmcu=$(MCU) -I. -x c++ $(CPPFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + + + +# Default target. +all: begin gccversion sizebefore build sizeafter end + +# Change the build target to build a HEX file or a library. +build: elf hex eep lss sym +#build: lib + + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym +LIBNAME=lib$(TARGET).a +lib: $(LIBNAME) + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) $(MCU_FLAG) $(FORMAT_FLAG) $(TARGET).elf +MCU_FLAG = $(shell $(SIZE) --help | grep -- --mcu > /dev/null && echo --mcu=$(MCU) ) +FORMAT_FLAG = $(shell $(SIZE) --help | grep -- --format=.*avr > /dev/null && echo --format=avr ) + + +sizebefore: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +sizeafter: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \ + 2>/dev/null; echo; fi + + + +# Display compiler version information. +gccversion : + @$(CC) --version + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + + +# Generate avr-gdb config/init file which does the following: +# define the reset signal, load the target file, connect to target, and set +# a breakpoint at main(). +gdb-config: + @$(REMOVE) $(GDBINIT_FILE) + @echo define reset >> $(GDBINIT_FILE) + @echo SIGNAL SIGHUP >> $(GDBINIT_FILE) + @echo end >> $(GDBINIT_FILE) + @echo file $(TARGET).elf >> $(GDBINIT_FILE) + @echo target remote $(DEBUG_HOST):$(DEBUG_PORT) >> $(GDBINIT_FILE) +ifeq ($(DEBUG_BACKEND),simulavr) + @echo load >> $(GDBINIT_FILE) +endif + @echo break main >> $(GDBINIT_FILE) + +debug: gdb-config $(TARGET).elf +ifeq ($(DEBUG_BACKEND), avarice) + @echo Starting AVaRICE - Press enter when "waiting to connect" message displays. + @$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \ + $(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT) + @$(WINSHELL) /c pause + +else + @$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \ + $(DEBUG_MFREQ) --port $(DEBUG_PORT) +endif + @$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE) + + + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT = $(OBJCOPY) --debugging +COFFCONVERT += --change-section-address .data-0x800000 +COFFCONVERT += --change-section-address .bss-0x800000 +COFFCONVERT += --change-section-address .noinit-0x800000 +COFFCONVERT += --change-section-address .eeprom-0x810000 + + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 --no-change-warnings -O $(FORMAT) $< $@ || exit 0 + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S -z $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Create library from object files. +.SECONDARY : $(TARGET).a +.PRECIOUS : $(OBJ) +%.a: $(OBJ) + @echo + @echo $(MSG_CREATING_LIBRARY) $@ + $(AR) $@ $(OBJ) + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +$(OBJDIR)/%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create object files from C++ source files. +$(OBJDIR)/%.o : %.cpp + @echo + @echo $(MSG_COMPILING_CPP) $< + $(CC) -c $(ALL_CPPFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C++ source files. +%.s : %.cpp + $(CC) -S $(ALL_CPPFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +$(OBJDIR)/%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + +# Create preprocessed source for use in sending a bug report. +%.i : %.c + $(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ + + +# Target: clean project. +clean: begin clean_list end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lss + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) $(SRC:.c=.i) + $(REMOVEDIR) .dep + +doxygen: + @echo Generating Project Documentation \($(TARGET)\)... + @doxygen Doxygen.conf + @echo Documentation Generation Complete. + +clean_doxygen: + rm -rf Documentation + +checksource: + @for f in $(SRC) $(CPPSRC) $(ASRC); do \ + if [ -f $$f ]; then \ + echo "Found Source File: $$f" ; \ + else \ + echo "Source File Not Found: $$f" ; \ + fi; done + + +# Create object files directory +$(shell mkdir $(OBJDIR) 2>/dev/null) + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff doxygen clean \ +clean_list clean_doxygen program debug gdb-config checksource + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-11-23.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-11-23.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-11-23.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-12-10.txt b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-12-10.txt new file mode 100644 index 0000000..5beb659 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/caterina/Micro-prod-firmware-2012-12-10.txt @@ -0,0 +1,11 @@ +Builds against LUFA version 111009 +make version 3.81 +avrdude version 5.11 + +All AVR tools except avrdude were installed by CrossPack 20100115: +avr-gcc version 4.3.3 (GCC) +Thread model: single +Configured with: ../configure —prefix=/usr/local/CrossPack-AVR-20100115 —disable-dependency-tracking —disable-nls —disable-werror —target=avr —enable-languages=c,c++ —disable-nls —disable-libssp —with-dwarf2 +avr-libc version 1.6.7 +binutils version 2.19 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/lilypad/src/ATmegaBOOT.c b/libs/arduino-1.0/hardware/arduino/bootloaders/lilypad/src/ATmegaBOOT.c new file mode 100644 index 0000000..915bc57 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/lilypad/src/ATmegaBOOT.c @@ -0,0 +1,979 @@ +/**********************************************************/ +/* Serial Bootloader for Atmel megaAVR Controllers */ +/* */ +/* tested with ATmega8, ATmega128 and ATmega168 */ +/* should work with other mega's, see code for details */ +/* */ +/* ATmegaBOOT.c */ +/* */ +/* 20070626: hacked for Arduino Diecimila (which auto- */ +/* resets when a USB connection is made to it) */ +/* by D. Mellis */ +/* 20060802: hacked for Arduino by D. Cuartielles */ +/* based on a previous hack by D. Mellis */ +/* and D. Cuartielles */ +/* */ +/* Monitor and debug functions were added to the original */ +/* code by Dr. Erik Lins, chip45.com. (See below) */ +/* */ +/* Thanks to Karl Pitrich for fixing a bootloader pin */ +/* problem and more informative LED blinking! */ +/* */ +/* For the latest version see: */ +/* http://www.chip45.com/ */ +/* */ +/* ------------------------------------------------------ */ +/* */ +/* based on stk500boot.c */ +/* Copyright (c) 2003, Jason P. Kyle */ +/* All rights reserved. */ +/* see avr1.org for original file and information */ +/* */ +/* This program is free software; you can redistribute it */ +/* and/or modify it under the terms of the GNU General */ +/* Public License as published by the Free Software */ +/* Foundation; either version 2 of the License, or */ +/* (at your option) any later version. */ +/* */ +/* This program is distributed in the hope that it will */ +/* be useful, but WITHOUT ANY WARRANTY; without even the */ +/* implied warranty of MERCHANTABILITY or FITNESS FOR A */ +/* PARTICULAR PURPOSE. See the GNU General Public */ +/* License for more details. */ +/* */ +/* You should have received a copy of the GNU General */ +/* Public License along with this program; if not, write */ +/* to the Free Software Foundation, Inc., */ +/* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* */ +/* Licence can be viewed at */ +/* http://www.fsf.org/licenses/gpl.txt */ +/* */ +/* Target = Atmel AVR m128,m64,m32,m16,m8,m162,m163,m169, */ +/* m8515,m8535. ATmega161 has a very small boot block so */ +/* isn't supported. */ +/* */ +/* Tested with m168 */ +/**********************************************************/ + +/* $Id$ */ + + +/* some includes */ +#include +#include +#include +#include +#include + + +/* the current avr-libc eeprom functions do not support the ATmega168 */ +/* own eeprom write/read functions are used instead */ +#ifndef __AVR_ATmega168__ +#include +#endif + +/* Use the F_CPU defined in Makefile */ + +/* 20060803: hacked by DojoCorp */ +/* 20070626: hacked by David A. Mellis to decrease waiting time for auto-reset */ +/* set the waiting time for the bootloader */ +/* get this from the Makefile instead */ +/* #define MAX_TIME_COUNT (F_CPU>>4) */ + +/* 20070707: hacked by David A. Mellis - after this many errors give up and launch application */ +#define MAX_ERROR_COUNT 5 + +/* set the UART baud rate */ +/* 20060803: hacked by DojoCorp */ +//#define BAUD_RATE 115200 +#define BAUD_RATE 19200 + + +/* SW_MAJOR and MINOR needs to be updated from time to time to avoid warning message from AVR Studio */ +/* never allow AVR Studio to do an update !!!! */ +#define HW_VER 0x02 +#define SW_MAJOR 0x01 +#define SW_MINOR 0x10 + + +/* Adjust to suit whatever pin your hardware uses to enter the bootloader */ +/* ATmega128 has two UARTS so two pins are used to enter bootloader and select UART */ +/* BL0... means UART0, BL1... means UART1 */ +#ifdef __AVR_ATmega128__ +#define BL_DDR DDRF +#define BL_PORT PORTF +#define BL_PIN PINF +#define BL0 PINF7 +#define BL1 PINF6 +#else +/* other ATmegas have only one UART, so only one pin is defined to enter bootloader */ +#define BL_DDR DDRD +#define BL_PORT PORTD +#define BL_PIN PIND +#define BL PIND6 +#endif + + +/* onboard LED is used to indicate, that the bootloader was entered (3x flashing) */ +/* if monitor functions are included, LED goes on after monitor was entered */ +#ifdef __AVR_ATmega128__ +/* Onboard LED is connected to pin PB7 (e.g. Crumb128, PROBOmega128, Savvy128) */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB7 +#else +/* Onboard LED is connected to pin PB2 (e.g. Crumb8, Crumb168) */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +/* 20060803: hacked by DojoCorp, LED pin is B5 in Arduino */ +/* #define LED PINB2 */ +#define LED PINB5 +#endif + + +/* monitor functions will only be compiled when using ATmega128, due to bootblock size constraints */ +#ifdef __AVR_ATmega128__ +#define MONITOR +#endif + + +/* define various device id's */ +/* manufacturer byte is always the same */ +#define SIG1 0x1E // Yep, Atmel is the only manufacturer of AVR micros. Single source :( + +#if defined __AVR_ATmega128__ +#define SIG2 0x97 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega64__ +#define SIG2 0x96 +#define SIG3 0x02 +#define PAGE_SIZE 0x80U //128 words + +#elif defined __AVR_ATmega32__ +#define SIG2 0x95 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega16__ +#define SIG2 0x94 +#define SIG3 0x03 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8__ +#define SIG2 0x93 +#define SIG3 0x07 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega88__ +#define SIG2 0x93 +#define SIG3 0x0a +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega168__ +#define SIG2 0x94 +#define SIG3 0x06 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega162__ +#define SIG2 0x94 +#define SIG3 0x04 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega163__ +#define SIG2 0x94 +#define SIG3 0x02 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega169__ +#define SIG2 0x94 +#define SIG3 0x05 +#define PAGE_SIZE 0x40U //64 words + +#elif defined __AVR_ATmega8515__ +#define SIG2 0x93 +#define SIG3 0x06 +#define PAGE_SIZE 0x20U //32 words + +#elif defined __AVR_ATmega8535__ +#define SIG2 0x93 +#define SIG3 0x08 +#define PAGE_SIZE 0x20U //32 words +#endif + + +/* function prototypes */ +void putch(char); +char getch(void); +void getNch(uint8_t); +void byte_response(uint8_t); +void nothing_response(void); +char gethex(void); +void puthex(char); +void flash_led(uint8_t); + +/* some variables */ +union address_union { + uint16_t word; + uint8_t byte[2]; +} address; + +union length_union { + uint16_t word; + uint8_t byte[2]; +} length; + +struct flags_struct { + unsigned eeprom : 1; + unsigned rampz : 1; +} flags; + +uint8_t buff[256]; +uint8_t address_high; + +uint8_t pagesz=0x80; + +uint8_t i; +uint8_t bootuart = 0; + +uint8_t error_count = 0; + +void (*app_start)(void) = 0x0000; + + +/* main program starts here */ +int main(void) +{ + uint8_t ch,ch2; + uint16_t w; + + asm volatile("nop\n\t"); + + /* set pin direction for bootloader pin and enable pullup */ + /* for ATmega128, two pins need to be initialized */ +#ifdef __AVR_ATmega128__ + BL_DDR &= ~_BV(BL0); + BL_DDR &= ~_BV(BL1); + BL_PORT |= _BV(BL0); + BL_PORT |= _BV(BL1); +#else + /* We run the bootloader regardless of the state of this pin. Thus, don't + put it in a different state than the other pins. --DAM, 070709 + BL_DDR &= ~_BV(BL); + BL_PORT |= _BV(BL); + */ +#endif + + +#ifdef __AVR_ATmega128__ + /* check which UART should be used for booting */ + if(bit_is_clear(BL_PIN, BL0)) { + bootuart = 1; + } + else if(bit_is_clear(BL_PIN, BL1)) { + bootuart = 2; + } +#endif + + /* check if flash is programmed already, if not start bootloader anyway */ + if(pgm_read_byte_near(0x0000) != 0xFF) { + +#ifdef __AVR_ATmega128__ + /* no UART was selected, start application */ + if(!bootuart) { + app_start(); + } +#else + /* check if bootloader pin is set low */ + /* we don't start this part neither for the m8, nor m168 */ + //if(bit_is_set(BL_PIN, BL)) { + // app_start(); + // } +#endif + } + +#ifdef __AVR_ATmega128__ + /* no bootuart was selected, default to uart 0 */ + if(!bootuart) { + bootuart = 1; + } +#endif + + + /* initialize UART(s) depending on CPU defined */ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR0A = 0x00; + UCSR0C = 0x06; + UCSR0B = _BV(TXEN0)|_BV(RXEN0); + } + if(bootuart == 2) { + UBRR1L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR1H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR1A = 0x00; + UCSR1C = 0x06; + UCSR1B = _BV(TXEN1)|_BV(RXEN1); + } +#elif defined __AVR_ATmega163__ + UBRR = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRRHI = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSRA = 0x00; + UCSRB = _BV(TXEN)|_BV(RXEN); +#elif defined __AVR_ATmega168__ + UBRR0L = (uint8_t)(F_CPU/(BAUD_RATE*16L)-1); + UBRR0H = (F_CPU/(BAUD_RATE*16L)-1) >> 8; + UCSR0B = (1<>8; // set baud rate + UBRRL = (((F_CPU/BAUD_RATE)/16)-1); + UCSRB = (1<> 8; + UCSRA = 0x00; + UCSRC = 0x06; + UCSRB = _BV(TXEN)|_BV(RXEN); +#endif + + /* set LED pin as output */ + LED_DDR |= _BV(LED); + + + /* flash onboard LED to signal entering of bootloader */ +#ifdef __AVR_ATmega128__ + // 4x for UART0, 5x for UART1 + flash_led(NUM_LED_FLASHES + bootuart); +#else + flash_led(NUM_LED_FLASHES); +#endif + + /* 20050803: by DojoCorp, this is one of the parts provoking the + system to stop listening, cancelled from the original */ + //putch('\0'); + + + /* forever loop */ + for (;;) { + + /* get character from UART */ + ch = getch(); + + /* A bunch of if...else if... gives smaller code than switch...case ! */ + + /* Hello is anyone home ? */ + if(ch=='0') { + nothing_response(); + } + + + /* Request programmer ID */ + /* Not using PROGMEM string due to boot block in m128 being beyond 64kB boundry */ + /* Would need to selectively manipulate RAMPZ, and it's only 9 characters anyway so who cares. */ + else if(ch=='1') { + if (getch() == ' ') { + putch(0x14); + putch('A'); + putch('V'); + putch('R'); + putch(' '); + putch('I'); + putch('S'); + putch('P'); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } + } + + + /* AVR ISP/STK500 board commands DON'T CARE so default nothing_response */ + else if(ch=='@') { + ch2 = getch(); + if (ch2>0x85) getch(); + nothing_response(); + } + + + /* AVR ISP/STK500 board requests */ + else if(ch=='A') { + ch2 = getch(); + if(ch2==0x80) byte_response(HW_VER); // Hardware version + else if(ch2==0x81) byte_response(SW_MAJOR); // Software major version + else if(ch2==0x82) byte_response(SW_MINOR); // Software minor version + else if(ch2==0x98) byte_response(0x03); // Unknown but seems to be required by avr studio 3.56 + else byte_response(0x00); // Covers various unnecessary responses we don't care about + } + + + /* Device Parameters DON'T CARE, DEVICE IS FIXED */ + else if(ch=='B') { + getNch(20); + nothing_response(); + } + + + /* Parallel programming stuff DON'T CARE */ + else if(ch=='E') { + getNch(5); + nothing_response(); + } + + + /* Enter programming mode */ + else if(ch=='P') { + nothing_response(); + } + + + /* Leave programming mode */ + else if(ch=='Q') { + nothing_response(); + } + + + /* Erase device, don't care as we will erase one page at a time anyway. */ + else if(ch=='R') { + nothing_response(); + } + + + /* Set address, little endian. EEPROM in bytes, FLASH in words */ + /* Perhaps extra address bytes may be added in future to support > 128kB FLASH. */ + /* This might explain why little endian was used here, big endian used everywhere else. */ + else if(ch=='U') { + address.byte[0] = getch(); + address.byte[1] = getch(); + nothing_response(); + } + + + /* Universal SPI programming command, disabled. Would be used for fuses and lock bits. */ + else if(ch=='V') { + getNch(4); + byte_response(0x00); + } + + + /* Write memory, length is big endian and is in bytes */ + else if(ch=='d') { + length.byte[1] = getch(); + length.byte[0] = getch(); + flags.eeprom = 0; + if (getch() == 'E') flags.eeprom = 1; + for (w=0;w127) address_high = 0x01; //Only possible with m128, m256 will need 3rd address byte. FIXME + else address_high = 0x00; +#ifdef __AVR_ATmega128__ + RAMPZ = address_high; +#endif + address.word = address.word << 1; //address * 2 -> byte location + /* if ((length.byte[0] & 0x01) == 0x01) length.word++; //Even up an odd number of bytes */ + if ((length.byte[0] & 0x01)) length.word++; //Even up an odd number of bytes + cli(); //Disable interrupts, just to be sure + // HACKME: EEPE used to be EEWE + while(bit_is_set(EECR,EEPE)); //Wait for previous EEPROM writes to complete + asm volatile( + "clr r17 \n\t" //page_word_count + "lds r30,address \n\t" //Address of FLASH location (in bytes) + "lds r31,address+1 \n\t" + "ldi r28,lo8(buff) \n\t" //Start of buffer array in RAM + "ldi r29,hi8(buff) \n\t" + "lds r24,length \n\t" //Length of data to be written (in bytes) + "lds r25,length+1 \n\t" + "length_loop: \n\t" //Main loop, repeat for number of words in block + "cpi r17,0x00 \n\t" //If page_word_count=0 then erase page + "brne no_page_erase \n\t" + "wait_spm1: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm1 \n\t" + "ldi r16,0x03 \n\t" //Erase page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "wait_spm2: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm2 \n\t" + + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "no_page_erase: \n\t" + "ld r0,Y+ \n\t" //Write 2 bytes into page buffer + "ld r1,Y+ \n\t" + + "wait_spm3: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm3 \n\t" + "ldi r16,0x01 \n\t" //Load r0,r1 into FLASH page buffer + "sts %0,r16 \n\t" + "spm \n\t" + + "inc r17 \n\t" //page_word_count++ + "cpi r17,%1 \n\t" + "brlo same_page \n\t" //Still same page in FLASH + "write_page: \n\t" + "clr r17 \n\t" //New page, write current one first + "wait_spm4: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm4 \n\t" +#ifdef __AVR_ATmega163__ + "andi r30,0x80 \n\t" // m163 requires Z6:Z1 to be zero during page write +#endif + "ldi r16,0x05 \n\t" //Write page pointed to by Z + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" + "ori r30,0x7E \n\t" // recover Z6:Z1 state after page write (had to be zero during write) +#endif + "wait_spm5: \n\t" + "lds r16,%0 \n\t" //Wait for previous spm to complete + "andi r16,1 \n\t" + "cpi r16,1 \n\t" + "breq wait_spm5 \n\t" + "ldi r16,0x11 \n\t" //Re-enable RWW section + "sts %0,r16 \n\t" + "spm \n\t" +#ifdef __AVR_ATmega163__ + ".word 0xFFFF \n\t" + "nop \n\t" +#endif + "same_page: \n\t" + "adiw r30,2 \n\t" //Next word in FLASH + "sbiw r24,2 \n\t" //length-2 + "breq final_write \n\t" //Finished + "rjmp length_loop \n\t" + "final_write: \n\t" + "cpi r17,0 \n\t" + "breq block_done \n\t" + "adiw r24,2 \n\t" //length+2, fool above check on length after short page write + "rjmp write_page \n\t" + "block_done: \n\t" + "clr __zero_reg__ \n\t" //restore zero register +#if defined __AVR_ATmega168__ + : "=m" (SPMCSR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#else + : "=m" (SPMCR) : "M" (PAGE_SIZE) : "r0","r16","r17","r24","r25","r28","r29","r30","r31" +#endif + ); + /* Should really add a wait for RWW section to be enabled, don't actually need it since we never */ + /* exit the bootloader without a power cycle anyhow */ + } + putch(0x14); + putch(0x10); + } else { + if (++error_count == MAX_ERROR_COUNT) + app_start(); + } + } + + + /* Read memory block mode, length is big endian. */ + else if(ch=='t') { + length.byte[1] = getch(); + length.byte[0] = getch(); +#if defined __AVR_ATmega128__ + if (address.word>0x7FFF) flags.rampz = 1; // No go with m256, FIXME + else flags.rampz = 0; +#endif + if (getch() == 'E') flags.eeprom = 1; + else { + flags.eeprom = 0; + address.word = address.word << 1; // address * 2 -> byte location + } + if (getch() == ' ') { // Command terminator + putch(0x14); + for (w=0;w < length.word;w++) { // Can handle odd and even lengths okay + if (flags.eeprom) { // Byte access EEPROM read +#ifdef __AVR_ATmega168__ + while(EECR & (1<= 'a') { + ah = ah - 'a' + 0x0a; + } else if(ah >= '0') { + ah -= '0'; + } + if(al >= 'a') { + al = al - 'a' + 0x0a; + } else if(al >= '0') { + al -= '0'; + } + return (ah << 4) + al; +} + + +void puthex(char ch) { + char ah,al; + + ah = (ch & 0xf0) >> 4; + if(ah >= 0x0a) { + ah = ah - 0x0a + 'a'; + } else { + ah += '0'; + } + al = (ch & 0x0f); + if(al >= 0x0a) { + al = al - 0x0a + 'a'; + } else { + al += '0'; + } + putch(ah); + putch(al); +} + + +void putch(char ch) +{ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; + } + else if (bootuart == 2) { + while (!(UCSR1A & _BV(UDRE1))); + UDR1 = ch; + } +#elif defined __AVR_ATmega168__ + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; +#else + /* m8,16,32,169,8515,8535,163 */ + while (!(UCSRA & _BV(UDRE))); + UDR = ch; +#endif +} + + +char getch(void) +{ +#ifdef __AVR_ATmega128__ + if(bootuart == 1) { + while(!(UCSR0A & _BV(RXC0))); + return UDR0; + } + else if(bootuart == 2) { + while(!(UCSR1A & _BV(RXC1))); + return UDR1; + } + return 0; +#elif defined __AVR_ATmega168__ + uint32_t count = 0; + while(!(UCSR0A & _BV(RXC0))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR0; +#else + /* m8,16,32,169,8515,8535,163 */ + uint32_t count = 0; + while(!(UCSRA & _BV(RXC))){ + /* 20060803 DojoCorp:: Addon coming from the previous Bootloader*/ + /* HACKME:: here is a good place to count times*/ + count++; + if (count > MAX_TIME_COUNT) + app_start(); + } + return UDR; +#endif +} + + +void getNch(uint8_t count) +{ + uint8_t i; + for(i=0;i $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ + +clean: + rm -rf *.o *.elf *.lst *.map *.sym *.lss *.eep *.srec *.bin *.hex + +install: + avrdude -p m168 -c stk500v2 -P /dev/cu.USA19H1b1P1.1 -e -u -U lock:w:0x3f:m -U efuse:w:0x00:m -U hfuse:w:0xdd:m -U lfuse:w:0xe2:m + avrdude -p m168 -c stk500v2 -P /dev/cu.USA19H1b1P1.1 -e -u -U flash:w:ATmegaBOOT_168.hex -U lock:w:0x0f:m diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/Makefile b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/Makefile new file mode 100644 index 0000000..b9f3ed5 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/Makefile @@ -0,0 +1,451 @@ +# Makefile for ATmegaBOOT +# E.Lins, 18.7.2005 +# $Id$ +# +# Instructions +# +# To make bootloader .hex file: +# make diecimila +# make lilypad +# make ng +# etc... +# +# To burn bootloader .hex file: +# make diecimila_isp +# make lilypad_isp +# make ng_isp +# etc... + +# program name should not be changed... +PROGRAM = optiboot + +# The default behavior is to build using tools that are in the users +# current path variables, but we can also build using an installed +# Arduino user IDE setup, or the Arduino source tree. +# Uncomment this next lines to build within the arduino environment, +# using the arduino-included avrgcc toolset (mac and pc) +# ENV ?= arduino +# ENV ?= arduinodev +# OS ?= macosx +# OS ?= windows + + +# enter the parameters for the avrdude isp tool +ISPTOOL = stk500v2 +ISPPORT = usb +ISPSPEED = -b 115200 + +MCU_TARGET = atmega168 +LDSECTIONS = -Wl,--section-start=.text=0x3e00 -Wl,--section-start=.version=0x3ffe + +# Build environments +# Start of some ugly makefile-isms to allow optiboot to be built +# in several different environments. See the README.TXT file for +# details. + +# default +fixpath = $(1) + +ifeq ($(ENV), arduino) +# For Arduino, we assume that we're connected to the optiboot directory +# included with the arduino distribution, which means that the full set +# of avr-tools are "right up there" in standard places. +TOOLROOT = ../../../tools +GCCROOT = $(TOOLROOT)/avr/bin/ +AVRDUDE_CONF = -C$(TOOLROOT)/avr/etc/avrdude.conf + +ifeq ($(OS), windows) +# On windows, SOME of the tool paths will need to have backslashes instead +# of forward slashes (because they use windows cmd.exe for execution instead +# of a unix/mingw shell?) We also have to ensure that a consistent shell +# is used even if a unix shell is installed (ie as part of WINAVR) +fixpath = $(subst /,\,$1) +SHELL = cmd.exe +endif + +else ifeq ($(ENV), arduinodev) +# Arduino IDE source code environment. Use the unpacked compilers created +# by the build (you'll need to do "ant build" first.) +ifeq ($(OS), macosx) +TOOLROOT = ../../../../build/macosx/work/Arduino.app/Contents/Resources/Java/hardware/tools +endif +ifeq ($(OS), windows) +TOOLROOT = ../../../../build/windows/work/hardware/tools +endif + +GCCROOT = $(TOOLROOT)/avr/bin/ +AVRDUDE_CONF = -C$(TOOLROOT)/avr/etc/avrdude.conf + +else +GCCROOT = +AVRDUDE_CONF = +endif +# +# End of build environment code. + + +# the efuse should really be 0xf8; since, however, only the lower +# three bits of that byte are used on the atmega168, avrdude gets +# confused if you specify 1's for the higher bits, see: +# http://tinker.it/now/2007/02/24/the-tale-of-avrdude-atmega168-and-extended-bits-fuses/ +# +# similarly, the lock bits should be 0xff instead of 0x3f (to +# unlock the bootloader section) and 0xcf instead of 0x2f (to +# lock it), but since the high two bits of the lock byte are +# unused, avrdude would get confused. + +ISPFUSES = $(GCCROOT)avrdude $(AVRDUDE_CONF) -c $(ISPTOOL) \ + -p $(MCU_TARGET) -P $(ISPPORT) $(ISPSPEED) \ + -e -u -U lock:w:0x3f:m -U efuse:w:0x$(EFUSE):m \ + -U hfuse:w:0x$(HFUSE):m -U lfuse:w:0x$(LFUSE):m +ISPFLASH = $(GCCROOT)avrdude $(AVRDUDE_CONF) -c $(ISPTOOL) \ + -p $(MCU_TARGET) -P $(ISPPORT) $(ISPSPEED) \ + -U flash:w:$(PROGRAM)_$(TARGET).hex -U lock:w:0x2f:m + +STK500 = "C:\Program Files\Atmel\AVR Tools\STK500\Stk500.exe" +STK500-1 = $(STK500) -e -d$(MCU_TARGET) -pf -vf -if$(PROGRAM)_$(TARGET).hex \ +-lFF -LFF -f$(HFUSE)$(LFUSE) -EF8 -ms -q -cUSB -I200kHz -s -wt +STK500-2 = $(STK500) -d$(MCU_TARGET) -ms -q -lCF -LCF -cUSB -I200kHz -s -wt + +OBJ = $(PROGRAM).o +OPTIMIZE = -Os -fno-inline-small-functions -fno-split-wide-types -mshort-calls + +DEFS = +LIBS = + +CC = $(GCCROOT)avr-gcc + +# Override is only needed by avr-lib build system. + +override CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) -DF_CPU=$(AVR_FREQ) $(DEFS) +override LDFLAGS = $(LDSECTIONS) -Wl,--relax -Wl,--gc-sections -nostartfiles -nostdlib + +OBJCOPY = $(GCCROOT)avr-objcopy +OBJDUMP = $(call fixpath,$(GCCROOT)avr-objdump) + +SIZE = $(GCCROOT)avr-size + +# Test platforms +# Virtual boot block test +virboot328: TARGET = atmega328 +virboot328: MCU_TARGET = atmega328p +virboot328: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DVIRTUAL_BOOT' +virboot328: AVR_FREQ = 16000000L +virboot328: LDSECTIONS = -Wl,--section-start=.text=0x7e00 -Wl,--section-start=.version=0x7ffe +virboot328: $(PROGRAM)_atmega328.hex +virboot328: $(PROGRAM)_atmega328.lst + +# 20MHz clocked platforms +# +# These are capable of 230400 baud, or 115200 baud on PC (Arduino Avrdude issue) +# + +pro20: TARGET = pro_20mhz +pro20: MCU_TARGET = atmega168 +pro20: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +pro20: AVR_FREQ = 20000000L +pro20: $(PROGRAM)_pro_20mhz.hex +pro20: $(PROGRAM)_pro_20mhz.lst + +pro20_isp: pro20 +pro20_isp: TARGET = pro_20mhz +# 2.7V brownout +pro20_isp: HFUSE = DD +# Full swing xtal (20MHz) 258CK/14CK+4.1ms +pro20_isp: LFUSE = C6 +# 512 byte boot +pro20_isp: EFUSE = 04 +pro20_isp: isp + +# 16MHz clocked platforms +# +# These are capable of 230400 baud, or 115200 baud on PC (Arduino Avrdude issue) +# + +pro16: TARGET = pro_16MHz +pro16: MCU_TARGET = atmega168 +pro16: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +pro16: AVR_FREQ = 16000000L +pro16: $(PROGRAM)_pro_16MHz.hex +pro16: $(PROGRAM)_pro_16MHz.lst + +pro16_isp: pro16 +pro16_isp: TARGET = pro_16MHz +# 2.7V brownout +pro16_isp: HFUSE = DD +# Full swing xtal (20MHz) 258CK/14CK+4.1ms +pro16_isp: LFUSE = C6 +# 512 byte boot +pro16_isp: EFUSE = 04 +pro16_isp: isp + +# Diecimila, Duemilanove with m168, and NG use identical bootloaders +# Call it "atmega168" for generality and clarity, keep "diecimila" for +# backward compatibility of makefile +# +atmega168: TARGET = atmega168 +atmega168: MCU_TARGET = atmega168 +atmega168: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +atmega168: AVR_FREQ = 16000000L +atmega168: $(PROGRAM)_atmega168.hex +atmega168: $(PROGRAM)_atmega168.lst + +atmega168_isp: atmega168 +atmega168_isp: TARGET = atmega168 +# 2.7V brownout +atmega168_isp: HFUSE = DD +# Low power xtal (16MHz) 16KCK/14CK+65ms +atmega168_isp: LFUSE = FF +# 512 byte boot +atmega168_isp: EFUSE = 04 +atmega168_isp: isp + +diecimila: TARGET = diecimila +diecimila: MCU_TARGET = atmega168 +diecimila: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +diecimila: AVR_FREQ = 16000000L +diecimila: $(PROGRAM)_diecimila.hex +diecimila: $(PROGRAM)_diecimila.lst + +diecimila_isp: diecimila +diecimila_isp: TARGET = diecimila +# 2.7V brownout +diecimila_isp: HFUSE = DD +# Low power xtal (16MHz) 16KCK/14CK+65ms +diecimila_isp: LFUSE = FF +# 512 byte boot +diecimila_isp: EFUSE = 04 +diecimila_isp: isp + +atmega328: TARGET = atmega328 +atmega328: MCU_TARGET = atmega328p +atmega328: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +atmega328: AVR_FREQ = 16000000L +atmega328: LDSECTIONS = -Wl,--section-start=.text=0x7e00 -Wl,--section-start=.version=0x7ffe +atmega328: $(PROGRAM)_atmega328.hex +atmega328: $(PROGRAM)_atmega328.lst + +atmega328_isp: atmega328 +atmega328_isp: TARGET = atmega328 +atmega328_isp: MCU_TARGET = atmega328p +# 512 byte boot, SPIEN +atmega328_isp: HFUSE = DE +# Low power xtal (16MHz) 16KCK/14CK+65ms +atmega328_isp: LFUSE = FF +# 2.7V brownout +atmega328_isp: EFUSE = 05 +atmega328_isp: isp + +# Sanguino has a minimum boot size of 1024 bytes, so enable extra functions +# +sanguino: TARGET = atmega644p +sanguino: MCU_TARGET = atmega644p +sanguino: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DBIGBOOT' +sanguino: AVR_FREQ = 16000000L +sanguino: LDSECTIONS = -Wl,--section-start=.text=0xfc00 +sanguino: $(PROGRAM)_atmega644p.hex +sanguino: $(PROGRAM)_atmega644p.lst + +sanguino_isp: sanguino +sanguino_isp: TARGET = atmega644p +sanguino_isp: MCU_TARGET = atmega644p +# 1024 byte boot +sanguino_isp: HFUSE = DE +# Low power xtal (16MHz) 16KCK/14CK+65ms +sanguino_isp: LFUSE = FF +# 2.7V brownout +sanguino_isp: EFUSE = 05 +sanguino_isp: isp + +# Mega has a minimum boot size of 1024 bytes, so enable extra functions +#mega: TARGET = atmega1280 +mega: MCU_TARGET = atmega1280 +mega: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' '-DBIGBOOT' +mega: AVR_FREQ = 16000000L +mega: LDSECTIONS = -Wl,--section-start=.text=0x1fc00 +mega: $(PROGRAM)_atmega1280.hex +mega: $(PROGRAM)_atmega1280.lst + +mega_isp: mega +mega_isp: TARGET = atmega1280 +mega_isp: MCU_TARGET = atmega1280 +# 1024 byte boot +mega_isp: HFUSE = DE +# Low power xtal (16MHz) 16KCK/14CK+65ms +mega_isp: LFUSE = FF +# 2.7V brownout +mega_isp: EFUSE = 05 +mega_isp: isp + +# ATmega8 +# +atmega8: TARGET = atmega8 +atmega8: MCU_TARGET = atmega8 +atmega8: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +atmega8: AVR_FREQ = 16000000L +atmega8: LDSECTIONS = -Wl,--section-start=.text=0x1e00 -Wl,--section-start=.version=0x1ffe +atmega8: $(PROGRAM)_atmega8.hex +atmega8: $(PROGRAM)_atmega8.lst + +atmega8_isp: atmega8 +atmega8_isp: TARGET = atmega8 +atmega8_isp: MCU_TARGET = atmega8 +# SPIEN, CKOPT, Bootsize=512B +atmega8_isp: HFUSE = CC +# 2.7V brownout, Low power xtal (16MHz) 16KCK/14CK+65ms +atmega8_isp: LFUSE = BF +atmega8_isp: isp + +# ATmega88 +# +atmega88: TARGET = atmega88 +atmega88: MCU_TARGET = atmega88 +atmega88: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +atmega88: AVR_FREQ = 16000000L +atmega88: LDSECTIONS = -Wl,--section-start=.text=0x1e00 -Wl,--section-start=.version=0x1ffe +atmega88: $(PROGRAM)_atmega88.hex +atmega88: $(PROGRAM)_atmega88.lst + +atmega88_isp: atmega88 +atmega88_isp: TARGET = atmega88 +atmega88_isp: MCU_TARGET = atmega88 +# 2.7V brownout +atmega88_isp: HFUSE = DD +# Low power xtal (16MHz) 16KCK/14CK+65ms +atemga88_isp: LFUSE = FF +# 512 byte boot +atmega88_isp: EFUSE = 04 +atmega88_isp: isp + + +# 8MHz clocked platforms +# +# These are capable of 115200 baud +# + +lilypad: TARGET = lilypad +lilypad: MCU_TARGET = atmega168 +lilypad: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +lilypad: AVR_FREQ = 8000000L +lilypad: $(PROGRAM)_lilypad.hex +lilypad: $(PROGRAM)_lilypad.lst + +lilypad_isp: lilypad +lilypad_isp: TARGET = lilypad +# 2.7V brownout +lilypad_isp: HFUSE = DD +# Internal 8MHz osc (8MHz) Slow rising power +lilypad_isp: LFUSE = E2 +# 512 byte boot +lilypad_isp: EFUSE = 04 +lilypad_isp: isp + +lilypad_resonator: TARGET = lilypad_resonator +lilypad_resonator: MCU_TARGET = atmega168 +lilypad_resonator: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +lilypad_resonator: AVR_FREQ = 8000000L +lilypad_resonator: $(PROGRAM)_lilypad_resonator.hex +lilypad_resonator: $(PROGRAM)_lilypad_resonator.lst + +lilypad_resonator_isp: lilypad_resonator +lilypad_resonator_isp: TARGET = lilypad_resonator +# 2.7V brownout +lilypad_resonator_isp: HFUSE = DD +# Full swing xtal (20MHz) 258CK/14CK+4.1ms +lilypad_resonator_isp: LFUSE = C6 +# 512 byte boot +lilypad_resonator_isp: EFUSE = 04 +lilypad_resonator_isp: isp + +pro8: TARGET = pro_8MHz +pro8: MCU_TARGET = atmega168 +pro8: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +pro8: AVR_FREQ = 8000000L +pro8: $(PROGRAM)_pro_8MHz.hex +pro8: $(PROGRAM)_pro_8MHz.lst + +pro8_isp: pro8 +pro8_isp: TARGET = pro_8MHz +# 2.7V brownout +pro8_isp: HFUSE = DD +# Full swing xtal (20MHz) 258CK/14CK+4.1ms +pro8_isp: LFUSE = C6 +# 512 byte boot +pro8_isp: EFUSE = 04 +pro8_isp: isp + +atmega328_pro8: TARGET = atmega328_pro_8MHz +atmega328_pro8: MCU_TARGET = atmega328p +atmega328_pro8: CFLAGS += '-DLED_START_FLASHES=3' '-DBAUD_RATE=115200' +atmega328_pro8: AVR_FREQ = 8000000L +atmega328_pro8: LDSECTIONS = -Wl,--section-start=.text=0x7e00 -Wl,--section-start=.version=0x7ffe +atmega328_pro8: $(PROGRAM)_atmega328_pro_8MHz.hex +atmega328_pro8: $(PROGRAM)_atmega328_pro_8MHz.lst + +atmega328_pro8_isp: atmega328_pro8 +atmega328_pro8_isp: TARGET = atmega328_pro_8MHz +atmega328_pro8_isp: MCU_TARGET = atmega328p +# 512 byte boot, SPIEN +atmega328_pro8_isp: HFUSE = DE +# Low power xtal (16MHz) 16KCK/14CK+65ms +atmega328_pro8_isp: LFUSE = FF +# 2.7V brownout +atmega328_pro8_isp: EFUSE = 05 +atmega328_pro8_isp: isp + +# 1MHz clocked platforms +# +# These are capable of 9600 baud +# + +luminet: TARGET = luminet +luminet: MCU_TARGET = attiny84 +luminet: CFLAGS += '-DLED_START_FLASHES=3' '-DSOFT_UART' '-DBAUD_RATE=9600' +luminet: CFLAGS += '-DVIRTUAL_BOOT_PARTITION' +luminet: AVR_FREQ = 1000000L +luminet: LDSECTIONS = -Wl,--section-start=.text=0x1d00 -Wl,--section-start=.version=0x1efe +luminet: $(PROGRAM)_luminet.hex +luminet: $(PROGRAM)_luminet.lst + +luminet_isp: luminet +luminet_isp: TARGET = luminet +luminet_isp: MCU_TARGET = attiny84 +# Brownout disabled +luminet_isp: HFUSE = DF +# 1MHz internal oscillator, slowly rising power +luminet_isp: LFUSE = 62 +# Self-programming enable +luminet_isp: EFUSE = FE +luminet_isp: isp + +# +# Generic build instructions +# +# + +isp: $(TARGET) + $(ISPFUSES) + $(ISPFLASH) + +isp-stk500: $(PROGRAM)_$(TARGET).hex + $(STK500-1) + $(STK500-2) + +%.elf: $(OBJ) + $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) + $(SIZE) $@ + +clean: + rm -rf *.o *.elf *.lst *.map *.sym *.lss *.eep *.srec *.bin *.hex + +%.lst: %.elf + $(OBJDUMP) -h -S $< > $@ + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -j .version --set-section-flags .version=alloc,load -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -j .version --set-section-flags .version=alloc,load -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -j .version --set-section-flags .version=alloc,load -O binary $< $@ diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/README.TXT b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/README.TXT new file mode 100644 index 0000000..cd79cd9 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/README.TXT @@ -0,0 +1,81 @@ +This directory contains the Optiboot small bootloader for AVR +microcontrollers, somewhat modified specifically for the Arduino +environment. + +Optiboot is more fully described here: http://code.google.com/p/optiboot/ +and is the work of Peter Knight (aka Cathedrow), building on work of Jason P +Kyle, Spiff, and Ladyada. Arduino-specific modification are by Bill +Westfield (aka WestfW) + +Arduino-specific issues are tracked as part of the Arduino project +at http://code.google.com/p/arduino + + +------------------------------------------------------------ +Building optiboot for Arduino. + +Production builds of optiboot for Arduino are done on a Mac in "unix mode" +using CrossPack-AVR-20100115. CrossPack tracks WINAVR (for windows), which +is just a package of avr-gcc and related utilities, so similar builds should +work on Windows or Linux systems. + +One of the Arduino-specific changes is modifications to the makefile to +allow building optiboot using only the tools installed as part of the +Arduino environment, or the Arduino source development tree. All three +build procedures should yield identical binaries (.hex files) (although +this may change if compiler versions drift apart between CrossPack and +the Arduino IDE.) + + +Building Optiboot in the Arduino IDE Install. + +Work in the .../hardware/arduino/bootloaders/optiboot/ and use the +"omake " command, which just generates a command that uses +the arduino-included "make" utility with a command like: + make OS=windows ENV=arduino +or make OS=macosx ENV=arduino +On windows, this assumes you're using the windows command shell. If +you're using a cygwin or mingw shell, or have one of those in your +path, the build will probably break due to slash vs backslash issues. +On a Mac, if you have the developer tools installed, you can use the +Apple-supplied version of make. +The makefile uses relative paths ("../../../tools/" and such) to find +the programs it needs, so you need to work in the existing optiboot +directory (or something created at the same "level") for it to work. + + +Building Optiboot in the Arduino Source Development Install. + +In this case, there is no special shell script, and you're assumed to +have "make" installed somewhere in your path. +Build the Arduino source ("ant build") to unpack the tools into the +expected directory. +Work in Arduino/hardware/arduino/bootloaders/optiboot and use + make OS=windows ENV=arduinodev +or make OS=macosx ENV=arduinodev + + +Programming Chips Using the _isp Targets + +The CPU targets have corresponding ISP targets that will actuall +program the bootloader into a chip. "atmega328_isp" for the atmega328, +for example. These will set the fuses and lock bits as appropriate as +well as uploading the bootloader code. + +The makefiles default to using a USB programmer, but you can use +a serial programmer like ArduinoISP by changing the appropriate +variables when you invoke make: + + make ISPTOOL=stk500v1 ISPPORT=/dev/tty.usbserial-A20e1eAN \ + ISPSPEED=-b19200 atmega328_isp + +The "atmega8_isp" target does not currently work, because the mega8 +doesn't have the "extended" fuse that the generic ISP target wants to +pass on to avrdude. You'll need to run avrdude manually. + + +Standard Targets + +I've reduced the pre-built and source-version-controlled targets +(.hex and .lst files included in the git repository) to just the +three basic 16MHz targets: atmega8, atmega16, atmega328. diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/boot.h b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/boot.h new file mode 100644 index 0000000..2639cd8 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/boot.h @@ -0,0 +1,848 @@ +/* Modified to use out for SPM access +** Peter Knight, Optiboot project http://optiboot.googlecode.com +** +** Todo: Tidy up +** +** "_short" routines execute 1 cycle faster and use 1 less word of flash +** by using "out" instruction instead of "sts". +** +** Additional elpm variants that trust the value of RAMPZ +*/ + +/* Copyright (c) 2002, 2003, 2004, 2005, 2006, 2007 Eric B. Weddington + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: boot.h,v 1.27.2.3 2008/09/30 13:58:48 arcanum Exp $ */ + +#ifndef _AVR_BOOT_H_ +#define _AVR_BOOT_H_ 1 + +/** \file */ +/** \defgroup avr_boot : Bootloader Support Utilities + \code + #include + #include + \endcode + + The macros in this module provide a C language interface to the + bootloader support functionality of certain AVR processors. These + macros are designed to work with all sizes of flash memory. + + Global interrupts are not automatically disabled for these macros. It + is left up to the programmer to do this. See the code example below. + Also see the processor datasheet for caveats on having global interrupts + enabled during writing of the Flash. + + \note Not all AVR processors provide bootloader support. See your + processor datasheet to see if it provides bootloader support. + + \todo From email with Marek: On smaller devices (all except ATmega64/128), + __SPM_REG is in the I/O space, accessible with the shorter "in" and "out" + instructions - since the boot loader has a limited size, this could be an + important optimization. + + \par API Usage Example + The following code shows typical usage of the boot API. + + \code + #include + #include + #include + + void boot_program_page (uint32_t page, uint8_t *buf) + { + uint16_t i; + uint8_t sreg; + + // Disable interrupts. + + sreg = SREG; + cli(); + + eeprom_busy_wait (); + + boot_page_erase (page); + boot_spm_busy_wait (); // Wait until the memory is erased. + + for (i=0; i +#include +#include +#include + +/* Check for SPM Control Register in processor. */ +#if defined (SPMCSR) +# define __SPM_REG SPMCSR +#elif defined (SPMCR) +# define __SPM_REG SPMCR +#else +# error AVR processor does not provide bootloader support! +#endif + + +/* Check for SPM Enable bit. */ +#if defined(SPMEN) +# define __SPM_ENABLE SPMEN +#elif defined(SELFPRGEN) +# define __SPM_ENABLE SELFPRGEN +#else +# error Cannot find SPM Enable bit definition! +#endif + +/** \ingroup avr_boot + \def BOOTLOADER_SECTION + + Used to declare a function or variable to be placed into a + new section called .bootloader. This section and its contents + can then be relocated to any address (such as the bootloader + NRWW area) at link-time. */ + +#define BOOTLOADER_SECTION __attribute__ ((section (".bootloader"))) + +/* Create common bit definitions. */ +#ifdef ASB +#define __COMMON_ASB ASB +#else +#define __COMMON_ASB RWWSB +#endif + +#ifdef ASRE +#define __COMMON_ASRE ASRE +#else +#define __COMMON_ASRE RWWSRE +#endif + +/* Define the bit positions of the Boot Lock Bits. */ + +#define BLB12 5 +#define BLB11 4 +#define BLB02 3 +#define BLB01 2 + +/** \ingroup avr_boot + \def boot_spm_interrupt_enable() + Enable the SPM interrupt. */ + +#define boot_spm_interrupt_enable() (__SPM_REG |= (uint8_t)_BV(SPMIE)) + +/** \ingroup avr_boot + \def boot_spm_interrupt_disable() + Disable the SPM interrupt. */ + +#define boot_spm_interrupt_disable() (__SPM_REG &= (uint8_t)~_BV(SPMIE)) + +/** \ingroup avr_boot + \def boot_is_spm_interrupt() + Check if the SPM interrupt is enabled. */ + +#define boot_is_spm_interrupt() (__SPM_REG & (uint8_t)_BV(SPMIE)) + +/** \ingroup avr_boot + \def boot_rww_busy() + Check if the RWW section is busy. */ + +#define boot_rww_busy() (__SPM_REG & (uint8_t)_BV(__COMMON_ASB)) + +/** \ingroup avr_boot + \def boot_spm_busy() + Check if the SPM instruction is busy. */ + +#define boot_spm_busy() (__SPM_REG & (uint8_t)_BV(__SPM_ENABLE)) + +/** \ingroup avr_boot + \def boot_spm_busy_wait() + Wait while the SPM instruction is busy. */ + +#define boot_spm_busy_wait() do{}while(boot_spm_busy()) + +#define __BOOT_PAGE_ERASE (_BV(__SPM_ENABLE) | _BV(PGERS)) +#define __BOOT_PAGE_WRITE (_BV(__SPM_ENABLE) | _BV(PGWRT)) +#define __BOOT_PAGE_FILL _BV(__SPM_ENABLE) +#define __BOOT_RWW_ENABLE (_BV(__SPM_ENABLE) | _BV(__COMMON_ASRE)) +#define __BOOT_LOCK_BITS_SET (_BV(__SPM_ENABLE) | _BV(BLBSET)) + +#define __boot_page_fill_short(address, data) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r0, %3\n\t" \ + "out %0, %1\n\t" \ + "spm\n\t" \ + "clr r1\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_FILL), \ + "z" ((uint16_t)address), \ + "r" ((uint16_t)data) \ + : "r0" \ + ); \ +})) + +#define __boot_page_fill_normal(address, data) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r0, %3\n\t" \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + "clr r1\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_FILL), \ + "z" ((uint16_t)address), \ + "r" ((uint16_t)data) \ + : "r0" \ + ); \ +})) + +#define __boot_page_fill_alternate(address, data)\ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r0, %3\n\t" \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + ".word 0xffff\n\t" \ + "nop\n\t" \ + "clr r1\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_FILL), \ + "z" ((uint16_t)address), \ + "r" ((uint16_t)data) \ + : "r0" \ + ); \ +})) + +#define __boot_page_fill_extended(address, data) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r0, %4\n\t" \ + "movw r30, %A3\n\t" \ + "sts %1, %C3\n\t" \ + "sts %0, %2\n\t" \ + "spm\n\t" \ + "clr r1\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "i" (_SFR_MEM_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_FILL), \ + "r" ((uint32_t)address), \ + "r" ((uint16_t)data) \ + : "r0", "r30", "r31" \ + ); \ +})) + +#define __boot_page_fill_extended_short(address, data) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r0, %4\n\t" \ + "movw r30, %A3\n\t" \ + "out %1, %C3\n\t" \ + "out %0, %2\n\t" \ + "spm\n\t" \ + "clr r1\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "i" (_SFR_IO_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_FILL), \ + "r" ((uint32_t)address), \ + "r" ((uint16_t)data) \ + : "r0", "r30", "r31" \ + ); \ +})) + +#define __boot_page_erase_short(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "out %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_ERASE), \ + "z" ((uint16_t)address) \ + ); \ +})) + + +#define __boot_page_erase_normal(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_ERASE), \ + "z" ((uint16_t)address) \ + ); \ +})) + +#define __boot_page_erase_alternate(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + ".word 0xffff\n\t" \ + "nop\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_ERASE), \ + "z" ((uint16_t)address) \ + ); \ +})) + +#define __boot_page_erase_extended(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r30, %A3\n\t" \ + "sts %1, %C3\n\t" \ + "sts %0, %2\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "i" (_SFR_MEM_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_ERASE), \ + "r" ((uint32_t)address) \ + : "r30", "r31" \ + ); \ +})) +#define __boot_page_erase_extended_short(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r30, %A3\n\t" \ + "out %1, %C3\n\t" \ + "out %0, %2\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "i" (_SFR_IO_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_ERASE), \ + "r" ((uint32_t)address) \ + : "r30", "r31" \ + ); \ +})) + +#define __boot_page_write_short(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "out %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_WRITE), \ + "z" ((uint16_t)address) \ + ); \ +})) + +#define __boot_page_write_normal(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_WRITE), \ + "z" ((uint16_t)address) \ + ); \ +})) + +#define __boot_page_write_alternate(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + ".word 0xffff\n\t" \ + "nop\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_PAGE_WRITE), \ + "z" ((uint16_t)address) \ + ); \ +})) + +#define __boot_page_write_extended(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r30, %A3\n\t" \ + "sts %1, %C3\n\t" \ + "sts %0, %2\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "i" (_SFR_MEM_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_WRITE), \ + "r" ((uint32_t)address) \ + : "r30", "r31" \ + ); \ +})) +#define __boot_page_write_extended_short(address) \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "movw r30, %A3\n\t" \ + "out %1, %C3\n\t" \ + "out %0, %2\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "i" (_SFR_IO_ADDR(RAMPZ)), \ + "r" ((uint8_t)__BOOT_PAGE_WRITE), \ + "r" ((uint32_t)address) \ + : "r30", "r31" \ + ); \ +})) + +#define __boot_rww_enable_short() \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "out %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_RWW_ENABLE) \ + ); \ +})) + +#define __boot_rww_enable() \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_RWW_ENABLE) \ + ); \ +})) + +#define __boot_rww_enable_alternate() \ +(__extension__({ \ + __asm__ __volatile__ \ + ( \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + ".word 0xffff\n\t" \ + "nop\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_RWW_ENABLE) \ + ); \ +})) + +/* From the mega16/mega128 data sheets (maybe others): + + Bits by SPM To set the Boot Loader Lock bits, write the desired data to + R0, write "X0001001" to SPMCR and execute SPM within four clock cycles + after writing SPMCR. The only accessible Lock bits are the Boot Lock bits + that may prevent the Application and Boot Loader section from any + software update by the MCU. + + If bits 5..2 in R0 are cleared (zero), the corresponding Boot Lock bit + will be programmed if an SPM instruction is executed within four cycles + after BLBSET and SPMEN (or SELFPRGEN) are set in SPMCR. The Z-pointer is + don't care during this operation, but for future compatibility it is + recommended to load the Z-pointer with $0001 (same as used for reading the + Lock bits). For future compatibility It is also recommended to set bits 7, + 6, 1, and 0 in R0 to 1 when writing the Lock bits. When programming the + Lock bits the entire Flash can be read during the operation. */ + +#define __boot_lock_bits_set_short(lock_bits) \ +(__extension__({ \ + uint8_t value = (uint8_t)(~(lock_bits)); \ + __asm__ __volatile__ \ + ( \ + "ldi r30, 1\n\t" \ + "ldi r31, 0\n\t" \ + "mov r0, %2\n\t" \ + "out %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_LOCK_BITS_SET), \ + "r" (value) \ + : "r0", "r30", "r31" \ + ); \ +})) + +#define __boot_lock_bits_set(lock_bits) \ +(__extension__({ \ + uint8_t value = (uint8_t)(~(lock_bits)); \ + __asm__ __volatile__ \ + ( \ + "ldi r30, 1\n\t" \ + "ldi r31, 0\n\t" \ + "mov r0, %2\n\t" \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_LOCK_BITS_SET), \ + "r" (value) \ + : "r0", "r30", "r31" \ + ); \ +})) + +#define __boot_lock_bits_set_alternate(lock_bits) \ +(__extension__({ \ + uint8_t value = (uint8_t)(~(lock_bits)); \ + __asm__ __volatile__ \ + ( \ + "ldi r30, 1\n\t" \ + "ldi r31, 0\n\t" \ + "mov r0, %2\n\t" \ + "sts %0, %1\n\t" \ + "spm\n\t" \ + ".word 0xffff\n\t" \ + "nop\n\t" \ + : \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_LOCK_BITS_SET), \ + "r" (value) \ + : "r0", "r30", "r31" \ + ); \ +})) + +/* + Reading lock and fuse bits: + + Similarly to writing the lock bits above, set BLBSET and SPMEN (or + SELFPRGEN) bits in __SPMREG, and then (within four clock cycles) issue an + LPM instruction. + + Z address: contents: + 0x0000 low fuse bits + 0x0001 lock bits + 0x0002 extended fuse bits + 0x0003 high fuse bits + + Sounds confusing, doesn't it? + + Unlike the macros in pgmspace.h, no need to care for non-enhanced + cores here as these old cores do not provide SPM support anyway. + */ + +/** \ingroup avr_boot + \def GET_LOW_FUSE_BITS + address to read the low fuse bits, using boot_lock_fuse_bits_get + */ +#define GET_LOW_FUSE_BITS (0x0000) +/** \ingroup avr_boot + \def GET_LOCK_BITS + address to read the lock bits, using boot_lock_fuse_bits_get + */ +#define GET_LOCK_BITS (0x0001) +/** \ingroup avr_boot + \def GET_EXTENDED_FUSE_BITS + address to read the extended fuse bits, using boot_lock_fuse_bits_get + */ +#define GET_EXTENDED_FUSE_BITS (0x0002) +/** \ingroup avr_boot + \def GET_HIGH_FUSE_BITS + address to read the high fuse bits, using boot_lock_fuse_bits_get + */ +#define GET_HIGH_FUSE_BITS (0x0003) + +/** \ingroup avr_boot + \def boot_lock_fuse_bits_get(address) + + Read the lock or fuse bits at \c address. + + Parameter \c address can be any of GET_LOW_FUSE_BITS, + GET_LOCK_BITS, GET_EXTENDED_FUSE_BITS, or GET_HIGH_FUSE_BITS. + + \note The lock and fuse bits returned are the physical values, + i.e. a bit returned as 0 means the corresponding fuse or lock bit + is programmed. + */ +#define boot_lock_fuse_bits_get_short(address) \ +(__extension__({ \ + uint8_t __result; \ + __asm__ __volatile__ \ + ( \ + "ldi r30, %3\n\t" \ + "ldi r31, 0\n\t" \ + "out %1, %2\n\t" \ + "lpm %0, Z\n\t" \ + : "=r" (__result) \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_LOCK_BITS_SET), \ + "M" (address) \ + : "r0", "r30", "r31" \ + ); \ + __result; \ +})) + +#define boot_lock_fuse_bits_get(address) \ +(__extension__({ \ + uint8_t __result; \ + __asm__ __volatile__ \ + ( \ + "ldi r30, %3\n\t" \ + "ldi r31, 0\n\t" \ + "sts %1, %2\n\t" \ + "lpm %0, Z\n\t" \ + : "=r" (__result) \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t)__BOOT_LOCK_BITS_SET), \ + "M" (address) \ + : "r0", "r30", "r31" \ + ); \ + __result; \ +})) + +/** \ingroup avr_boot + \def boot_signature_byte_get(address) + + Read the Signature Row byte at \c address. For some MCU types, + this function can also retrieve the factory-stored oscillator + calibration bytes. + + Parameter \c address can be 0-0x1f as documented by the datasheet. + \note The values are MCU type dependent. +*/ + +#define __BOOT_SIGROW_READ (_BV(__SPM_ENABLE) | _BV(SIGRD)) + +#define boot_signature_byte_get_short(addr) \ +(__extension__({ \ + uint16_t __addr16 = (uint16_t)(addr); \ + uint8_t __result; \ + __asm__ __volatile__ \ + ( \ + "out %1, %2\n\t" \ + "lpm %0, Z" "\n\t" \ + : "=r" (__result) \ + : "i" (_SFR_IO_ADDR(__SPM_REG)), \ + "r" ((uint8_t) __BOOT_SIGROW_READ), \ + "z" (__addr16) \ + ); \ + __result; \ +})) + +#define boot_signature_byte_get(addr) \ +(__extension__({ \ + uint16_t __addr16 = (uint16_t)(addr); \ + uint8_t __result; \ + __asm__ __volatile__ \ + ( \ + "sts %1, %2\n\t" \ + "lpm %0, Z" "\n\t" \ + : "=r" (__result) \ + : "i" (_SFR_MEM_ADDR(__SPM_REG)), \ + "r" ((uint8_t) __BOOT_SIGROW_READ), \ + "z" (__addr16) \ + ); \ + __result; \ +})) + +/** \ingroup avr_boot + \def boot_page_fill(address, data) + + Fill the bootloader temporary page buffer for flash + address with data word. + + \note The address is a byte address. The data is a word. The AVR + writes data to the buffer a word at a time, but addresses the buffer + per byte! So, increment your address by 2 between calls, and send 2 + data bytes in a word format! The LSB of the data is written to the lower + address; the MSB of the data is written to the higher address.*/ + +/** \ingroup avr_boot + \def boot_page_erase(address) + + Erase the flash page that contains address. + + \note address is a byte address in flash, not a word address. */ + +/** \ingroup avr_boot + \def boot_page_write(address) + + Write the bootloader temporary page buffer + to flash page that contains address. + + \note address is a byte address in flash, not a word address. */ + +/** \ingroup avr_boot + \def boot_rww_enable() + + Enable the Read-While-Write memory section. */ + +/** \ingroup avr_boot + \def boot_lock_bits_set(lock_bits) + + Set the bootloader lock bits. + + \param lock_bits A mask of which Boot Loader Lock Bits to set. + + \note In this context, a 'set bit' will be written to a zero value. + Note also that only BLBxx bits can be programmed by this command. + + For example, to disallow the SPM instruction from writing to the Boot + Loader memory section of flash, you would use this macro as such: + + \code + boot_lock_bits_set (_BV (BLB11)); + \endcode + + \note Like any lock bits, the Boot Loader Lock Bits, once set, + cannot be cleared again except by a chip erase which will in turn + also erase the boot loader itself. */ + +/* Normal versions of the macros use 16-bit addresses. + Extended versions of the macros use 32-bit addresses. + Alternate versions of the macros use 16-bit addresses and require special + instruction sequences after LPM. + + FLASHEND is defined in the ioXXXX.h file. + USHRT_MAX is defined in . */ + +#if defined(__AVR_ATmega161__) || defined(__AVR_ATmega163__) \ + || defined(__AVR_ATmega323__) + +/* Alternate: ATmega161/163/323 and 16 bit address */ +#define boot_page_fill(address, data) __boot_page_fill_alternate(address, data) +#define boot_page_erase(address) __boot_page_erase_alternate(address) +#define boot_page_write(address) __boot_page_write_alternate(address) +#define boot_rww_enable() __boot_rww_enable_alternate() +#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_alternate(lock_bits) + +#elif (FLASHEND > USHRT_MAX) + +/* Extended: >16 bit address */ +#define boot_page_fill(address, data) __boot_page_fill_extended_short(address, data) +#define boot_page_erase(address) __boot_page_erase_extended_short(address) +#define boot_page_write(address) __boot_page_write_extended_short(address) +#define boot_rww_enable() __boot_rww_enable_short() +#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_short(lock_bits) + +#else + +/* Normal: 16 bit address */ +#define boot_page_fill(address, data) __boot_page_fill_short(address, data) +#define boot_page_erase(address) __boot_page_erase_short(address) +#define boot_page_write(address) __boot_page_write_short(address) +#define boot_rww_enable() __boot_rww_enable_short() +#define boot_lock_bits_set(lock_bits) __boot_lock_bits_set_short(lock_bits) + +#endif + +/** \ingroup avr_boot + + Same as boot_page_fill() except it waits for eeprom and spm operations to + complete before filling the page. */ + +#define boot_page_fill_safe(address, data) \ +do { \ + boot_spm_busy_wait(); \ + eeprom_busy_wait(); \ + boot_page_fill(address, data); \ +} while (0) + +/** \ingroup avr_boot + + Same as boot_page_erase() except it waits for eeprom and spm operations to + complete before erasing the page. */ + +#define boot_page_erase_safe(address) \ +do { \ + boot_spm_busy_wait(); \ + eeprom_busy_wait(); \ + boot_page_erase (address); \ +} while (0) + +/** \ingroup avr_boot + + Same as boot_page_write() except it waits for eeprom and spm operations to + complete before writing the page. */ + +#define boot_page_write_safe(address) \ +do { \ + boot_spm_busy_wait(); \ + eeprom_busy_wait(); \ + boot_page_write (address); \ +} while (0) + +/** \ingroup avr_boot + + Same as boot_rww_enable() except waits for eeprom and spm operations to + complete before enabling the RWW mameory. */ + +#define boot_rww_enable_safe() \ +do { \ + boot_spm_busy_wait(); \ + eeprom_busy_wait(); \ + boot_rww_enable(); \ +} while (0) + +/** \ingroup avr_boot + + Same as boot_lock_bits_set() except waits for eeprom and spm operations to + complete before setting the lock bits. */ + +#define boot_lock_bits_set_safe(lock_bits) \ +do { \ + boot_spm_busy_wait(); \ + eeprom_busy_wait(); \ + boot_lock_bits_set (lock_bits); \ +} while (0) + +#endif /* _AVR_BOOT_H_ */ diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/makeall b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/makeall new file mode 100644 index 0000000..f076bc7 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/makeall @@ -0,0 +1,20 @@ +#!/bin/bash +make clean +# +# The "big three" standard bootloaders. +make atmega8 +make atmega168 +make atmega328 +# +# additional buildable platforms of +# somewhat questionable support level +make lilypad +make lilypad_resonator +make pro8 +make pro16 +make pro20 +make atmega328_pro8 +make sanguino +make mega +make atmega88 +make luminet diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake new file mode 100644 index 0000000..cc7c6bc --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake @@ -0,0 +1,2 @@ +echo ../../../tools/avr/bin/make OS=macosx ENV=arduino $* +../../../tools/avr/bin/make OS=macosx ENV=arduino $* diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake.bat b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake.bat new file mode 100644 index 0000000..f6815da --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/omake.bat @@ -0,0 +1 @@ +..\..\..\tools\avr\utils\bin\make OS=windows ENV=arduino %* diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot.c b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot.c new file mode 100644 index 0000000..d499d85 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot.c @@ -0,0 +1,672 @@ +/**********************************************************/ +/* Optiboot bootloader for Arduino */ +/* */ +/* http://optiboot.googlecode.com */ +/* */ +/* Arduino-maintained version : See README.TXT */ +/* http://code.google.com/p/arduino/ */ +/* */ +/* Heavily optimised bootloader that is faster and */ +/* smaller than the Arduino standard bootloader */ +/* */ +/* Enhancements: */ +/* Fits in 512 bytes, saving 1.5K of code space */ +/* Background page erasing speeds up programming */ +/* Higher baud rate speeds up programming */ +/* Written almost entirely in C */ +/* Customisable timeout with accurate timeconstant */ +/* Optional virtual UART. No hardware UART required. */ +/* Optional virtual boot partition for devices without. */ +/* */ +/* What you lose: */ +/* Implements a skeleton STK500 protocol which is */ +/* missing several features including EEPROM */ +/* programming and non-page-aligned writes */ +/* High baud rate breaks compatibility with standard */ +/* Arduino flash settings */ +/* */ +/* Fully supported: */ +/* ATmega168 based devices (Diecimila etc) */ +/* ATmega328P based devices (Duemilanove etc) */ +/* */ +/* Alpha test */ +/* ATmega1280 based devices (Arduino Mega) */ +/* */ +/* Work in progress: */ +/* ATmega644P based devices (Sanguino) */ +/* ATtiny84 based devices (Luminet) */ +/* */ +/* Does not support: */ +/* USB based devices (eg. Teensy) */ +/* */ +/* Assumptions: */ +/* The code makes several assumptions that reduce the */ +/* code size. They are all true after a hardware reset, */ +/* but may not be true if the bootloader is called by */ +/* other means or on other hardware. */ +/* No interrupts can occur */ +/* UART and Timer 1 are set to their reset state */ +/* SP points to RAMEND */ +/* */ +/* Code builds on code, libraries and optimisations from: */ +/* stk500boot.c by Jason P. Kyle */ +/* Arduino bootloader http://arduino.cc */ +/* Spiff's 1K bootloader http://spiffie.org/know/arduino_1k_bootloader/bootloader.shtml */ +/* avr-libc project http://nongnu.org/avr-libc */ +/* Adaboot http://www.ladyada.net/library/arduino/bootloader.html */ +/* AVR305 Atmel Application Note */ +/* */ +/* This program is free software; you can redistribute it */ +/* and/or modify it under the terms of the GNU General */ +/* Public License as published by the Free Software */ +/* Foundation; either version 2 of the License, or */ +/* (at your option) any later version. */ +/* */ +/* This program is distributed in the hope that it will */ +/* be useful, but WITHOUT ANY WARRANTY; without even the */ +/* implied warranty of MERCHANTABILITY or FITNESS FOR A */ +/* PARTICULAR PURPOSE. See the GNU General Public */ +/* License for more details. */ +/* */ +/* You should have received a copy of the GNU General */ +/* Public License along with this program; if not, write */ +/* to the Free Software Foundation, Inc., */ +/* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* */ +/* Licence can be viewed at */ +/* http://www.fsf.org/licenses/gpl.txt */ +/* */ +/**********************************************************/ + + +/**********************************************************/ +/* */ +/* Optional defines: */ +/* */ +/**********************************************************/ +/* */ +/* BIG_BOOT: */ +/* Build a 1k bootloader, not 512 bytes. This turns on */ +/* extra functionality. */ +/* */ +/* BAUD_RATE: */ +/* Set bootloader baud rate. */ +/* */ +/* LUDICROUS_SPEED: */ +/* 230400 baud :-) */ +/* */ +/* SOFT_UART: */ +/* Use AVR305 soft-UART instead of hardware UART. */ +/* */ +/* LED_START_FLASHES: */ +/* Number of LED flashes on bootup. */ +/* */ +/* LED_DATA_FLASH: */ +/* Flash LED when transferring data. For boards without */ +/* TX or RX LEDs, or for people who like blinky lights. */ +/* */ +/* SUPPORT_EEPROM: */ +/* Support reading and writing from EEPROM. This is not */ +/* used by Arduino, so off by default. */ +/* */ +/* TIMEOUT_MS: */ +/* Bootloader timeout period, in milliseconds. */ +/* 500,1000,2000,4000,8000 supported. */ +/* */ +/**********************************************************/ + +/**********************************************************/ +/* Version Numbers! */ +/* */ +/* Arduino Optiboot now includes this Version number in */ +/* the source and object code. */ +/* */ +/* Version 3 was released as zip from the optiboot */ +/* repository and was distributed with Arduino 0022. */ +/* Version 4 starts with the arduino repository commit */ +/* that brought the arduino repository up-to-date with */ +/* the optiboot source tree changes since v3. */ +/* */ +/**********************************************************/ + +/**********************************************************/ +/* Edit History: */ +/* */ +/* 4.4 WestfW: add initialization of address to keep */ +/* the compiler happy. Change SC'ed targets. */ +/* Return the SW version via READ PARAM */ +/* 4.3 WestfW: catch framing errors in getch(), so that */ +/* AVRISP works without HW kludges. */ +/* http://code.google.com/p/arduino/issues/detail?id=368n*/ +/* 4.2 WestfW: reduce code size, fix timeouts, change */ +/* verifySpace to use WDT instead of appstart */ +/* 4.1 WestfW: put version number in binary. */ +/**********************************************************/ + +#define OPTIBOOT_MAJVER 4 +#define OPTIBOOT_MINVER 4 + +#define MAKESTR(a) #a +#define MAKEVER(a, b) MAKESTR(a*256+b) + +asm(" .section .version\n" + "optiboot_version: .word " MAKEVER(OPTIBOOT_MAJVER, OPTIBOOT_MINVER) "\n" + " .section .text\n"); + +#include +#include +#include + +// uses sts instructions, but this version uses out instructions +// This saves cycles and program memory. +#include "boot.h" + + +// We don't use as those routines have interrupt overhead we don't need. + +#include "pin_defs.h" +#include "stk500.h" + +#ifndef LED_START_FLASHES +#define LED_START_FLASHES 0 +#endif + +#ifdef LUDICROUS_SPEED +#define BAUD_RATE 230400L +#endif + +/* set the UART baud rate defaults */ +#ifndef BAUD_RATE +#if F_CPU >= 8000000L +#define BAUD_RATE 115200L // Highest rate Avrdude win32 will support +#elsif F_CPU >= 1000000L +#define BAUD_RATE 9600L // 19200 also supported, but with significant error +#elsif F_CPU >= 128000L +#define BAUD_RATE 4800L // Good for 128kHz internal RC +#else +#define BAUD_RATE 1200L // Good even at 32768Hz +#endif +#endif + +/* Switch in soft UART for hard baud rates */ +#if (F_CPU/BAUD_RATE) > 280 // > 57600 for 16MHz +#ifndef SOFT_UART +#define SOFT_UART +#endif +#endif + +/* Watchdog settings */ +#define WATCHDOG_OFF (0) +#define WATCHDOG_16MS (_BV(WDE)) +#define WATCHDOG_32MS (_BV(WDP0) | _BV(WDE)) +#define WATCHDOG_64MS (_BV(WDP1) | _BV(WDE)) +#define WATCHDOG_125MS (_BV(WDP1) | _BV(WDP0) | _BV(WDE)) +#define WATCHDOG_250MS (_BV(WDP2) | _BV(WDE)) +#define WATCHDOG_500MS (_BV(WDP2) | _BV(WDP0) | _BV(WDE)) +#define WATCHDOG_1S (_BV(WDP2) | _BV(WDP1) | _BV(WDE)) +#define WATCHDOG_2S (_BV(WDP2) | _BV(WDP1) | _BV(WDP0) | _BV(WDE)) +#ifndef __AVR_ATmega8__ +#define WATCHDOG_4S (_BV(WDP3) | _BV(WDE)) +#define WATCHDOG_8S (_BV(WDP3) | _BV(WDP0) | _BV(WDE)) +#endif + +/* Function Prototypes */ +/* The main function is in init9, which removes the interrupt vector table */ +/* we don't need. It is also 'naked', which means the compiler does not */ +/* generate any entry or exit code itself. */ +int main(void) __attribute__ ((naked)) __attribute__ ((section (".init9"))); +void putch(char); +uint8_t getch(void); +static inline void getNch(uint8_t); /* "static inline" is a compiler hint to reduce code size */ +void verifySpace(); +static inline void flash_led(uint8_t); +uint8_t getLen(); +static inline void watchdogReset(); +void watchdogConfig(uint8_t x); +#ifdef SOFT_UART +void uartDelay() __attribute__ ((naked)); +#endif +void appStart() __attribute__ ((naked)); + +#if defined(__AVR_ATmega168__) +#define RAMSTART (0x100) +#define NRWWSTART (0x3800) +#elif defined(__AVR_ATmega328P__) +#define RAMSTART (0x100) +#define NRWWSTART (0x7000) +#elif defined (__AVR_ATmega644P__) +#define RAMSTART (0x100) +#define NRWWSTART (0xE000) +#elif defined(__AVR_ATtiny84__) +#define RAMSTART (0x100) +#define NRWWSTART (0x0000) +#elif defined(__AVR_ATmega1280__) +#define RAMSTART (0x200) +#define NRWWSTART (0xE000) +#elif defined(__AVR_ATmega8__) || defined(__AVR_ATmega88__) +#define RAMSTART (0x100) +#define NRWWSTART (0x1800) +#endif + +/* C zero initialises all global variables. However, that requires */ +/* These definitions are NOT zero initialised, but that doesn't matter */ +/* This allows us to drop the zero init code, saving us memory */ +#define buff ((uint8_t*)(RAMSTART)) +#ifdef VIRTUAL_BOOT_PARTITION +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + uint8_t ch; + + /* + * Making these local and in registers prevents the need for initializing + * them, and also saves space because code no longer stores to memory. + * (initializing address keeps the compiler happy, but isn't really + * necessary, and uses 4 bytes of flash.) + */ + register uint16_t address = 0; + register uint8_t length; + + // After the zero init loop, this is the first code to run. + // + // This code makes the following assumptions: + // No interrupts will execute + // SP points to RAMEND + // r1 contains zero + // + // If not, uncomment the following instructions: + // cli(); + asm volatile ("clr __zero_reg__"); +#ifdef __AVR_ATmega8__ + SP=RAMEND; // This is done by hardware reset +#endif + + // Adaboot no-wait mod + ch = MCUSR; + MCUSR = 0; + if (!(ch & _BV(EXTRF))) appStart(); + +#if LED_START_FLASHES > 0 + // Set up Timer 1 for timeout counter + TCCR1B = _BV(CS12) | _BV(CS10); // div 1024 +#endif +#ifndef SOFT_UART +#ifdef __AVR_ATmega8__ + UCSRA = _BV(U2X); //Double speed mode USART + UCSRB = _BV(RXEN) | _BV(TXEN); // enable Rx & Tx + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); // config USART; 8N1 + UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); +#else + UCSR0A = _BV(U2X0); //Double speed mode USART0 + UCSR0B = _BV(RXEN0) | _BV(TXEN0); + UCSR0C = _BV(UCSZ00) | _BV(UCSZ01); + UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); +#endif +#endif + + // Set up watchdog to trigger after 500ms + watchdogConfig(WATCHDOG_1S); + + /* Set LED pin as output */ + LED_DDR |= _BV(LED); + +#ifdef SOFT_UART + /* Set TX pin as output */ + UART_DDR |= _BV(UART_TX_BIT); +#endif + +#if LED_START_FLASHES > 0 + /* Flash onboard LED to signal entering of bootloader */ + flash_led(LED_START_FLASHES * 2); +#endif + + /* Forever loop */ + for (;;) { + /* get character from UART */ + ch = getch(); + + if(ch == STK_GET_PARAMETER) { + unsigned char which = getch(); + verifySpace(); + if (which == 0x82) { + /* + * Send optiboot version as "minor SW version" + */ + putch(OPTIBOOT_MINVER); + } else if (which == 0x81) { + putch(OPTIBOOT_MAJVER); + } else { + /* + * GET PARAMETER returns a generic 0x03 reply for + * other parameters - enough to keep Avrdude happy + */ + putch(0x03); + } + } + else if(ch == STK_SET_DEVICE) { + // SET DEVICE is ignored + getNch(20); + } + else if(ch == STK_SET_DEVICE_EXT) { + // SET DEVICE EXT is ignored + getNch(5); + } + else if(ch == STK_LOAD_ADDRESS) { + // LOAD ADDRESS + uint16_t newAddress; + newAddress = getch(); + newAddress = (newAddress & 0xff) | (getch() << 8); +#ifdef RAMPZ + // Transfer top bit to RAMPZ + RAMPZ = (newAddress & 0x8000) ? 1 : 0; +#endif + newAddress += newAddress; // Convert from word address to byte address + address = newAddress; + verifySpace(); + } + else if(ch == STK_UNIVERSAL) { + // UNIVERSAL command is ignored + getNch(4); + putch(0x00); + } + /* Write memory, length is big endian and is in bytes */ + else if(ch == STK_PROG_PAGE) { + // PROGRAM PAGE - we support flash programming only, not EEPROM + uint8_t *bufPtr; + uint16_t addrPtr; + + getch(); /* getlen() */ + length = getch(); + getch(); + + // If we are in RWW section, immediately start page erase + if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + + // While that is going on, read in page contents + bufPtr = buff; + do *bufPtr++ = getch(); + while (--length); + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + + // Read command terminator, start reply + verifySpace(); + + // If only a partial page is to be programmed, the erase might not be complete. + // So check that here + boot_spm_busy_wait(); + +#ifdef VIRTUAL_BOOT_PARTITION + if ((uint16_t)(void*)address == 0) { + // This is the reset vector page. We need to live-patch the code so the + // bootloader runs. + // + // Move RESET vector to WDT vector + uint16_t vect = buff[0] | (buff[1]<<8); + rstVect = vect; + wdtVect = buff[8] | (buff[9]<<8); + vect -= 4; // Instruction is a relative jump (rjmp), so recalculate. + buff[8] = vect & 0xff; + buff[9] = vect >> 8; + + // Add jump to bootloader at RESET vector + buff[0] = 0x7f; + buff[1] = 0xce; // rjmp 0x1d00 instruction + } +#endif + + // Copy buffer into programming buffer + bufPtr = buff; + addrPtr = (uint16_t)(void*)address; + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + addrPtr += 2; + } while (--ch); + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + boot_spm_busy_wait(); + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); +#endif + + } + /* Read memory block mode, length is big endian. */ + else if(ch == STK_READ_PAGE) { + // READ PAGE - we only read flash + getch(); /* getlen() */ + length = getch(); + getch(); + + verifySpace(); +#ifdef VIRTUAL_BOOT_PARTITION + do { + // Undo vector patch in bottom page so verify passes + if (address == 0) ch=rstVect & 0xff; + else if (address == 1) ch=rstVect >> 8; + else if (address == 8) ch=wdtVect & 0xff; + else if (address == 9) ch=wdtVect >> 8; + else ch = pgm_read_byte_near(address); + address++; + putch(ch); + } while (--length); +#else +#ifdef __AVR_ATmega1280__ +// do putch(pgm_read_byte_near(address++)); +// while (--length); + do { + uint8_t result; + __asm__ ("elpm %0,Z\n":"=r"(result):"z"(address)); + putch(result); + address++; + } + while (--length); +#else + do putch(pgm_read_byte_near(address++)); + while (--length); +#endif +#endif + } + + /* Get device signature bytes */ + else if(ch == STK_READ_SIGN) { + // READ SIGN - return what Avrdude wants to hear + verifySpace(); + putch(SIGNATURE_0); + putch(SIGNATURE_1); + putch(SIGNATURE_2); + } + else if (ch == 'Q') { + // Adaboot no-wait mod + watchdogConfig(WATCHDOG_16MS); + verifySpace(); + } + else { + // This covers the response to commands like STK_ENTER_PROGMODE + verifySpace(); + } + putch(STK_OK); + } +} + +void putch(char ch) { +#ifndef SOFT_UART + while (!(UCSR0A & _BV(UDRE0))); + UDR0 = ch; +#else + __asm__ __volatile__ ( + " com %[ch]\n" // ones complement, carry set + " sec\n" + "1: brcc 2f\n" + " cbi %[uartPort],%[uartBit]\n" + " rjmp 3f\n" + "2: sbi %[uartPort],%[uartBit]\n" + " nop\n" + "3: rcall uartDelay\n" + " rcall uartDelay\n" + " lsr %[ch]\n" + " dec %[bitcnt]\n" + " brne 1b\n" + : + : + [bitcnt] "d" (10), + [ch] "r" (ch), + [uartPort] "I" (_SFR_IO_ADDR(UART_PORT)), + [uartBit] "I" (UART_TX_BIT) + : + "r25" + ); +#endif +} + +uint8_t getch(void) { + uint8_t ch; + +#ifdef LED_DATA_FLASH +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif +#endif + +#ifdef SOFT_UART + __asm__ __volatile__ ( + "1: sbic %[uartPin],%[uartBit]\n" // Wait for start edge + " rjmp 1b\n" + " rcall uartDelay\n" // Get to middle of start bit + "2: rcall uartDelay\n" // Wait 1 bit period + " rcall uartDelay\n" // Wait 1 bit period + " clc\n" + " sbic %[uartPin],%[uartBit]\n" + " sec\n" + " dec %[bitCnt]\n" + " breq 3f\n" + " ror %[ch]\n" + " rjmp 2b\n" + "3:\n" + : + [ch] "=r" (ch) + : + [bitCnt] "d" (9), + [uartPin] "I" (_SFR_IO_ADDR(UART_PIN)), + [uartBit] "I" (UART_RX_BIT) + : + "r25" +); +#else + while(!(UCSR0A & _BV(RXC0))) + ; + if (!(UCSR0A & _BV(FE0))) { + /* + * A Framing Error indicates (probably) that something is talking + * to us at the wrong bit rate. Assume that this is because it + * expects to be talking to the application, and DON'T reset the + * watchdog. This should cause the bootloader to abort and run + * the application "soon", if it keeps happening. (Note that we + * don't care that an invalid char is returned...) + */ + watchdogReset(); + } + + ch = UDR0; +#endif + +#ifdef LED_DATA_FLASH +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif +#endif + + return ch; +} + +#ifdef SOFT_UART +// AVR350 equation: #define UART_B_VALUE (((F_CPU/BAUD_RATE)-23)/6) +// Adding 3 to numerator simulates nearest rounding for more accurate baud rates +#define UART_B_VALUE (((F_CPU/BAUD_RATE)-20)/6) +#if UART_B_VALUE > 255 +#error Baud rate too slow for soft UART +#endif + +void uartDelay() { + __asm__ __volatile__ ( + "ldi r25,%[count]\n" + "1:dec r25\n" + "brne 1b\n" + "ret\n" + ::[count] "M" (UART_B_VALUE) + ); +} +#endif + +void getNch(uint8_t count) { + do getch(); while (--count); + verifySpace(); +} + +void verifySpace() { + if (getch() != CRC_EOP) { + watchdogConfig(WATCHDOG_16MS); // shorten WD timeout + while (1) // and busy-loop so that WD causes + ; // a reset and app start. + } + putch(STK_INSYNC); +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + TIFR1 = _BV(TOV1); + while(!(TIFR1 & _BV(TOV1))); +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif + watchdogReset(); + } while (--count); +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + "wdr\n" + ); +} + +void watchdogConfig(uint8_t x) { + WDTCSR = _BV(WDCE) | _BV(WDE); + WDTCSR = x; +} + +void appStart() { + watchdogConfig(WATCHDOG_OFF); + __asm__ __volatile__ ( +#ifdef VIRTUAL_BOOT_PARTITION + // Jump to WDT vector + "ldi r30,4\n" + "clr r31\n" +#else + // Jump to RST vector + "clr r30\n" + "clr r31\n" +#endif + "ijmp\n" + ); +} diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega168.lst b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega168.lst new file mode 100644 index 0000000..06316db --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega168.lst @@ -0,0 +1,598 @@ + +optiboot_atmega168.elf: file format elf32-avr + +Sections: +Idx Name Size VMA LMA File off Algn + 0 .text 000001f4 00003e00 00003e00 00000054 2**1 + CONTENTS, ALLOC, LOAD, READONLY, CODE + 1 .version 00000002 00003ffe 00003ffe 00000248 2**0 + CONTENTS, READONLY + 2 .debug_aranges 00000028 00000000 00000000 0000024a 2**0 + CONTENTS, READONLY, DEBUGGING + 3 .debug_pubnames 0000005f 00000000 00000000 00000272 2**0 + CONTENTS, READONLY, DEBUGGING + 4 .debug_info 000002a8 00000000 00000000 000002d1 2**0 + CONTENTS, READONLY, DEBUGGING + 5 .debug_abbrev 00000178 00000000 00000000 00000579 2**0 + CONTENTS, READONLY, DEBUGGING + 6 .debug_line 00000488 00000000 00000000 000006f1 2**0 + CONTENTS, READONLY, DEBUGGING + 7 .debug_frame 00000080 00000000 00000000 00000b7c 2**2 + CONTENTS, READONLY, DEBUGGING + 8 .debug_str 0000014f 00000000 00000000 00000bfc 2**0 + CONTENTS, READONLY, DEBUGGING + 9 .debug_loc 000002d8 00000000 00000000 00000d4b 2**0 + CONTENTS, READONLY, DEBUGGING + 10 .debug_ranges 00000078 00000000 00000000 00001023 2**0 + CONTENTS, READONLY, DEBUGGING + +Disassembly of section .text: + +00003e00
: +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 3e00: 11 24 eor r1, r1 +#ifdef __AVR_ATmega8__ + SP=RAMEND; // This is done by hardware reset +#endif + + // Adaboot no-wait mod + ch = MCUSR; + 3e02: 84 b7 in r24, 0x34 ; 52 + MCUSR = 0; + 3e04: 14 be out 0x34, r1 ; 52 + if (!(ch & _BV(EXTRF))) appStart(); + 3e06: 81 ff sbrs r24, 1 + 3e08: f0 d0 rcall .+480 ; 0x3fea + +#if LED_START_FLASHES > 0 + // Set up Timer 1 for timeout counter + TCCR1B = _BV(CS12) | _BV(CS10); // div 1024 + 3e0a: 85 e0 ldi r24, 0x05 ; 5 + 3e0c: 80 93 81 00 sts 0x0081, r24 + UCSRA = _BV(U2X); //Double speed mode USART + UCSRB = _BV(RXEN) | _BV(TXEN); // enable Rx & Tx + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); // config USART; 8N1 + UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); +#else + UCSR0A = _BV(U2X0); //Double speed mode USART0 + 3e10: 82 e0 ldi r24, 0x02 ; 2 + 3e12: 80 93 c0 00 sts 0x00C0, r24 + UCSR0B = _BV(RXEN0) | _BV(TXEN0); + 3e16: 88 e1 ldi r24, 0x18 ; 24 + 3e18: 80 93 c1 00 sts 0x00C1, r24 + UCSR0C = _BV(UCSZ00) | _BV(UCSZ01); + 3e1c: 86 e0 ldi r24, 0x06 ; 6 + 3e1e: 80 93 c2 00 sts 0x00C2, r24 + UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); + 3e22: 80 e1 ldi r24, 0x10 ; 16 + 3e24: 80 93 c4 00 sts 0x00C4, r24 +#endif +#endif + + // Set up watchdog to trigger after 500ms + watchdogConfig(WATCHDOG_1S); + 3e28: 8e e0 ldi r24, 0x0E ; 14 + 3e2a: c9 d0 rcall .+402 ; 0x3fbe + + /* Set LED pin as output */ + LED_DDR |= _BV(LED); + 3e2c: 25 9a sbi 0x04, 5 ; 4 + 3e2e: 86 e0 ldi r24, 0x06 ; 6 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 3e30: 20 e3 ldi r18, 0x30 ; 48 + 3e32: 3c ef ldi r19, 0xFC ; 252 + TIFR1 = _BV(TOV1); + 3e34: 91 e0 ldi r25, 0x01 ; 1 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 3e36: 30 93 85 00 sts 0x0085, r19 + 3e3a: 20 93 84 00 sts 0x0084, r18 + TIFR1 = _BV(TOV1); + 3e3e: 96 bb out 0x16, r25 ; 22 + while(!(TIFR1 & _BV(TOV1))); + 3e40: b0 9b sbis 0x16, 0 ; 22 + 3e42: fe cf rjmp .-4 ; 0x3e40 +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); + 3e44: 1d 9a sbi 0x03, 5 ; 3 +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 3e46: a8 95 wdr + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif + watchdogReset(); + } while (--count); + 3e48: 81 50 subi r24, 0x01 ; 1 + 3e4a: a9 f7 brne .-22 ; 0x3e36 + 3e4c: cc 24 eor r12, r12 + 3e4e: dd 24 eor r13, r13 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 3e50: 88 24 eor r8, r8 + 3e52: 83 94 inc r8 + addrPtr += 2; + } while (--ch); + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 3e54: b5 e0 ldi r27, 0x05 ; 5 + 3e56: ab 2e mov r10, r27 + boot_spm_busy_wait(); + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 3e58: a1 e1 ldi r26, 0x11 ; 17 + 3e5a: 9a 2e mov r9, r26 + do *bufPtr++ = getch(); + while (--length); + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 3e5c: f3 e0 ldi r31, 0x03 ; 3 + 3e5e: bf 2e mov r11, r31 +#endif + + /* Forever loop */ + for (;;) { + /* get character from UART */ + ch = getch(); + 3e60: a2 d0 rcall .+324 ; 0x3fa6 + + if(ch == STK_GET_PARAMETER) { + 3e62: 81 34 cpi r24, 0x41 ; 65 + 3e64: 61 f4 brne .+24 ; 0x3e7e + unsigned char which = getch(); + 3e66: 9f d0 rcall .+318 ; 0x3fa6 + 3e68: 08 2f mov r16, r24 + verifySpace(); + 3e6a: af d0 rcall .+350 ; 0x3fca + if (which == 0x82) { + 3e6c: 02 38 cpi r16, 0x82 ; 130 + 3e6e: 11 f0 breq .+4 ; 0x3e74 + /* + * Send optiboot version as "minor SW version" + */ + putch(OPTIBOOT_MINVER); + } else if (which == 0x81) { + 3e70: 01 38 cpi r16, 0x81 ; 129 + 3e72: 11 f4 brne .+4 ; 0x3e78 + putch(OPTIBOOT_MAJVER); + 3e74: 84 e0 ldi r24, 0x04 ; 4 + 3e76: 01 c0 rjmp .+2 ; 0x3e7a + } else { + /* + * GET PARAMETER returns a generic 0x03 reply for + * other parameters - enough to keep Avrdude happy + */ + putch(0x03); + 3e78: 83 e0 ldi r24, 0x03 ; 3 + 3e7a: 8d d0 rcall .+282 ; 0x3f96 + 3e7c: 89 c0 rjmp .+274 ; 0x3f90 + } + } + else if(ch == STK_SET_DEVICE) { + 3e7e: 82 34 cpi r24, 0x42 ; 66 + 3e80: 11 f4 brne .+4 ; 0x3e86 + // SET DEVICE is ignored + getNch(20); + 3e82: 84 e1 ldi r24, 0x14 ; 20 + 3e84: 03 c0 rjmp .+6 ; 0x3e8c + } + else if(ch == STK_SET_DEVICE_EXT) { + 3e86: 85 34 cpi r24, 0x45 ; 69 + 3e88: 19 f4 brne .+6 ; 0x3e90 + // SET DEVICE EXT is ignored + getNch(5); + 3e8a: 85 e0 ldi r24, 0x05 ; 5 + 3e8c: a6 d0 rcall .+332 ; 0x3fda + 3e8e: 80 c0 rjmp .+256 ; 0x3f90 + } + else if(ch == STK_LOAD_ADDRESS) { + 3e90: 85 35 cpi r24, 0x55 ; 85 + 3e92: 79 f4 brne .+30 ; 0x3eb2 + // LOAD ADDRESS + uint16_t newAddress; + newAddress = getch(); + 3e94: 88 d0 rcall .+272 ; 0x3fa6 + newAddress = (newAddress & 0xff) | (getch() << 8); + 3e96: e8 2e mov r14, r24 + 3e98: ff 24 eor r15, r15 + 3e9a: 85 d0 rcall .+266 ; 0x3fa6 + 3e9c: 08 2f mov r16, r24 + 3e9e: 10 e0 ldi r17, 0x00 ; 0 + 3ea0: 10 2f mov r17, r16 + 3ea2: 00 27 eor r16, r16 + 3ea4: 0e 29 or r16, r14 + 3ea6: 1f 29 or r17, r15 +#ifdef RAMPZ + // Transfer top bit to RAMPZ + RAMPZ = (newAddress & 0x8000) ? 1 : 0; +#endif + newAddress += newAddress; // Convert from word address to byte address + 3ea8: 00 0f add r16, r16 + 3eaa: 11 1f adc r17, r17 + address = newAddress; + verifySpace(); + 3eac: 8e d0 rcall .+284 ; 0x3fca + 3eae: 68 01 movw r12, r16 + 3eb0: 6f c0 rjmp .+222 ; 0x3f90 + } + else if(ch == STK_UNIVERSAL) { + 3eb2: 86 35 cpi r24, 0x56 ; 86 + 3eb4: 21 f4 brne .+8 ; 0x3ebe + // UNIVERSAL command is ignored + getNch(4); + 3eb6: 84 e0 ldi r24, 0x04 ; 4 + 3eb8: 90 d0 rcall .+288 ; 0x3fda + putch(0x00); + 3eba: 80 e0 ldi r24, 0x00 ; 0 + 3ebc: de cf rjmp .-68 ; 0x3e7a + } + /* Write memory, length is big endian and is in bytes */ + else if(ch == STK_PROG_PAGE) { + 3ebe: 84 36 cpi r24, 0x64 ; 100 + 3ec0: 09 f0 breq .+2 ; 0x3ec4 + 3ec2: 40 c0 rjmp .+128 ; 0x3f44 + // PROGRAM PAGE - we support flash programming only, not EEPROM + uint8_t *bufPtr; + uint16_t addrPtr; + + getch(); /* getlen() */ + 3ec4: 70 d0 rcall .+224 ; 0x3fa6 + length = getch(); + 3ec6: 6f d0 rcall .+222 ; 0x3fa6 + 3ec8: 08 2f mov r16, r24 + getch(); + 3eca: 6d d0 rcall .+218 ; 0x3fa6 + + // If we are in RWW section, immediately start page erase + if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 3ecc: 80 e0 ldi r24, 0x00 ; 0 + 3ece: c8 16 cp r12, r24 + 3ed0: 88 e3 ldi r24, 0x38 ; 56 + 3ed2: d8 06 cpc r13, r24 + 3ed4: 18 f4 brcc .+6 ; 0x3edc + 3ed6: f6 01 movw r30, r12 + 3ed8: b7 be out 0x37, r11 ; 55 + 3eda: e8 95 spm + 3edc: c0 e0 ldi r28, 0x00 ; 0 + 3ede: d1 e0 ldi r29, 0x01 ; 1 + + // While that is going on, read in page contents + bufPtr = buff; + do *bufPtr++ = getch(); + 3ee0: 62 d0 rcall .+196 ; 0x3fa6 + 3ee2: 89 93 st Y+, r24 + while (--length); + 3ee4: 0c 17 cp r16, r28 + 3ee6: e1 f7 brne .-8 ; 0x3ee0 + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 3ee8: f0 e0 ldi r31, 0x00 ; 0 + 3eea: cf 16 cp r12, r31 + 3eec: f8 e3 ldi r31, 0x38 ; 56 + 3eee: df 06 cpc r13, r31 + 3ef0: 18 f0 brcs .+6 ; 0x3ef8 + 3ef2: f6 01 movw r30, r12 + 3ef4: b7 be out 0x37, r11 ; 55 + 3ef6: e8 95 spm + + // Read command terminator, start reply + verifySpace(); + 3ef8: 68 d0 rcall .+208 ; 0x3fca + + // If only a partial page is to be programmed, the erase might not be complete. + // So check that here + boot_spm_busy_wait(); + 3efa: 07 b6 in r0, 0x37 ; 55 + 3efc: 00 fc sbrc r0, 0 + 3efe: fd cf rjmp .-6 ; 0x3efa + 3f00: a6 01 movw r20, r12 + 3f02: a0 e0 ldi r26, 0x00 ; 0 + 3f04: b1 e0 ldi r27, 0x01 ; 1 + bufPtr = buff; + addrPtr = (uint16_t)(void*)address; + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + 3f06: 2c 91 ld r18, X + 3f08: 30 e0 ldi r19, 0x00 ; 0 + a |= (*bufPtr++) << 8; + 3f0a: 11 96 adiw r26, 0x01 ; 1 + 3f0c: 8c 91 ld r24, X + 3f0e: 11 97 sbiw r26, 0x01 ; 1 + 3f10: 90 e0 ldi r25, 0x00 ; 0 + 3f12: 98 2f mov r25, r24 + 3f14: 88 27 eor r24, r24 + 3f16: 82 2b or r24, r18 + 3f18: 93 2b or r25, r19 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 3f1a: 12 96 adiw r26, 0x02 ; 2 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 3f1c: fa 01 movw r30, r20 + 3f1e: 0c 01 movw r0, r24 + 3f20: 87 be out 0x37, r8 ; 55 + 3f22: e8 95 spm + 3f24: 11 24 eor r1, r1 + addrPtr += 2; + 3f26: 4e 5f subi r20, 0xFE ; 254 + 3f28: 5f 4f sbci r21, 0xFF ; 255 + } while (--ch); + 3f2a: f1 e0 ldi r31, 0x01 ; 1 + 3f2c: a0 38 cpi r26, 0x80 ; 128 + 3f2e: bf 07 cpc r27, r31 + 3f30: 51 f7 brne .-44 ; 0x3f06 + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 3f32: f6 01 movw r30, r12 + 3f34: a7 be out 0x37, r10 ; 55 + 3f36: e8 95 spm + boot_spm_busy_wait(); + 3f38: 07 b6 in r0, 0x37 ; 55 + 3f3a: 00 fc sbrc r0, 0 + 3f3c: fd cf rjmp .-6 ; 0x3f38 + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 3f3e: 97 be out 0x37, r9 ; 55 + 3f40: e8 95 spm + 3f42: 26 c0 rjmp .+76 ; 0x3f90 +#endif + + } + /* Read memory block mode, length is big endian. */ + else if(ch == STK_READ_PAGE) { + 3f44: 84 37 cpi r24, 0x74 ; 116 + 3f46: b1 f4 brne .+44 ; 0x3f74 + // READ PAGE - we only read flash + getch(); /* getlen() */ + 3f48: 2e d0 rcall .+92 ; 0x3fa6 + length = getch(); + 3f4a: 2d d0 rcall .+90 ; 0x3fa6 + 3f4c: f8 2e mov r15, r24 + getch(); + 3f4e: 2b d0 rcall .+86 ; 0x3fa6 + + verifySpace(); + 3f50: 3c d0 rcall .+120 ; 0x3fca + 3f52: f6 01 movw r30, r12 + 3f54: ef 2c mov r14, r15 + putch(result); + address++; + } + while (--length); +#else + do putch(pgm_read_byte_near(address++)); + 3f56: 8f 01 movw r16, r30 + 3f58: 0f 5f subi r16, 0xFF ; 255 + 3f5a: 1f 4f sbci r17, 0xFF ; 255 + 3f5c: 84 91 lpm r24, Z+ + 3f5e: 1b d0 rcall .+54 ; 0x3f96 + while (--length); + 3f60: ea 94 dec r14 + 3f62: f8 01 movw r30, r16 + 3f64: c1 f7 brne .-16 ; 0x3f56 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 3f66: 08 94 sec + 3f68: c1 1c adc r12, r1 + 3f6a: d1 1c adc r13, r1 + 3f6c: fa 94 dec r15 + 3f6e: cf 0c add r12, r15 + 3f70: d1 1c adc r13, r1 + 3f72: 0e c0 rjmp .+28 ; 0x3f90 +#endif +#endif + } + + /* Get device signature bytes */ + else if(ch == STK_READ_SIGN) { + 3f74: 85 37 cpi r24, 0x75 ; 117 + 3f76: 39 f4 brne .+14 ; 0x3f86 + // READ SIGN - return what Avrdude wants to hear + verifySpace(); + 3f78: 28 d0 rcall .+80 ; 0x3fca + putch(SIGNATURE_0); + 3f7a: 8e e1 ldi r24, 0x1E ; 30 + 3f7c: 0c d0 rcall .+24 ; 0x3f96 + putch(SIGNATURE_1); + 3f7e: 84 e9 ldi r24, 0x94 ; 148 + 3f80: 0a d0 rcall .+20 ; 0x3f96 + putch(SIGNATURE_2); + 3f82: 86 e0 ldi r24, 0x06 ; 6 + 3f84: 7a cf rjmp .-268 ; 0x3e7a + } + else if (ch == 'Q') { + 3f86: 81 35 cpi r24, 0x51 ; 81 + 3f88: 11 f4 brne .+4 ; 0x3f8e + // Adaboot no-wait mod + watchdogConfig(WATCHDOG_16MS); + 3f8a: 88 e0 ldi r24, 0x08 ; 8 + 3f8c: 18 d0 rcall .+48 ; 0x3fbe + verifySpace(); + } + else { + // This covers the response to commands like STK_ENTER_PROGMODE + verifySpace(); + 3f8e: 1d d0 rcall .+58 ; 0x3fca + } + putch(STK_OK); + 3f90: 80 e1 ldi r24, 0x10 ; 16 + 3f92: 01 d0 rcall .+2 ; 0x3f96 + 3f94: 65 cf rjmp .-310 ; 0x3e60 + +00003f96 : + } +} + +void putch(char ch) { + 3f96: 98 2f mov r25, r24 +#ifndef SOFT_UART + while (!(UCSR0A & _BV(UDRE0))); + 3f98: 80 91 c0 00 lds r24, 0x00C0 + 3f9c: 85 ff sbrs r24, 5 + 3f9e: fc cf rjmp .-8 ; 0x3f98 + UDR0 = ch; + 3fa0: 90 93 c6 00 sts 0x00C6, r25 + [uartBit] "I" (UART_TX_BIT) + : + "r25" + ); +#endif +} + 3fa4: 08 95 ret + +00003fa6 : + [uartBit] "I" (UART_RX_BIT) + : + "r25" +); +#else + while(!(UCSR0A & _BV(RXC0))) + 3fa6: 80 91 c0 00 lds r24, 0x00C0 + 3faa: 87 ff sbrs r24, 7 + 3fac: fc cf rjmp .-8 ; 0x3fa6 + ; + if (!(UCSR0A & _BV(FE0))) { + 3fae: 80 91 c0 00 lds r24, 0x00C0 + 3fb2: 84 fd sbrc r24, 4 + 3fb4: 01 c0 rjmp .+2 ; 0x3fb8 +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 3fb6: a8 95 wdr + * don't care that an invalid char is returned...) + */ + watchdogReset(); + } + + ch = UDR0; + 3fb8: 80 91 c6 00 lds r24, 0x00C6 + LED_PIN |= _BV(LED); +#endif +#endif + + return ch; +} + 3fbc: 08 95 ret + +00003fbe : + "wdr\n" + ); +} + +void watchdogConfig(uint8_t x) { + WDTCSR = _BV(WDCE) | _BV(WDE); + 3fbe: e0 e6 ldi r30, 0x60 ; 96 + 3fc0: f0 e0 ldi r31, 0x00 ; 0 + 3fc2: 98 e1 ldi r25, 0x18 ; 24 + 3fc4: 90 83 st Z, r25 + WDTCSR = x; + 3fc6: 80 83 st Z, r24 +} + 3fc8: 08 95 ret + +00003fca : + do getch(); while (--count); + verifySpace(); +} + +void verifySpace() { + if (getch() != CRC_EOP) { + 3fca: ed df rcall .-38 ; 0x3fa6 + 3fcc: 80 32 cpi r24, 0x20 ; 32 + 3fce: 19 f0 breq .+6 ; 0x3fd6 + watchdogConfig(WATCHDOG_16MS); // shorten WD timeout + 3fd0: 88 e0 ldi r24, 0x08 ; 8 + 3fd2: f5 df rcall .-22 ; 0x3fbe + 3fd4: ff cf rjmp .-2 ; 0x3fd4 + while (1) // and busy-loop so that WD causes + ; // a reset and app start. + } + putch(STK_INSYNC); + 3fd6: 84 e1 ldi r24, 0x14 ; 20 +} + 3fd8: de cf rjmp .-68 ; 0x3f96 + +00003fda : + ::[count] "M" (UART_B_VALUE) + ); +} +#endif + +void getNch(uint8_t count) { + 3fda: 1f 93 push r17 + 3fdc: 18 2f mov r17, r24 + do getch(); while (--count); + 3fde: e3 df rcall .-58 ; 0x3fa6 + 3fe0: 11 50 subi r17, 0x01 ; 1 + 3fe2: e9 f7 brne .-6 ; 0x3fde + verifySpace(); + 3fe4: f2 df rcall .-28 ; 0x3fca +} + 3fe6: 1f 91 pop r17 + 3fe8: 08 95 ret + +00003fea : + WDTCSR = _BV(WDCE) | _BV(WDE); + WDTCSR = x; +} + +void appStart() { + watchdogConfig(WATCHDOG_OFF); + 3fea: 80 e0 ldi r24, 0x00 ; 0 + 3fec: e8 df rcall .-48 ; 0x3fbe + __asm__ __volatile__ ( + 3fee: ee 27 eor r30, r30 + 3ff0: ff 27 eor r31, r31 + 3ff2: 09 94 ijmp diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega328.lst b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega328.lst new file mode 100644 index 0000000..d9dd4cc --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega328.lst @@ -0,0 +1,598 @@ + +optiboot_atmega328.elf: file format elf32-avr + +Sections: +Idx Name Size VMA LMA File off Algn + 0 .text 000001f4 00007e00 00007e00 00000054 2**1 + CONTENTS, ALLOC, LOAD, READONLY, CODE + 1 .version 00000002 00007ffe 00007ffe 00000248 2**0 + CONTENTS, READONLY + 2 .debug_aranges 00000028 00000000 00000000 0000024a 2**0 + CONTENTS, READONLY, DEBUGGING + 3 .debug_pubnames 0000005f 00000000 00000000 00000272 2**0 + CONTENTS, READONLY, DEBUGGING + 4 .debug_info 000002a8 00000000 00000000 000002d1 2**0 + CONTENTS, READONLY, DEBUGGING + 5 .debug_abbrev 00000178 00000000 00000000 00000579 2**0 + CONTENTS, READONLY, DEBUGGING + 6 .debug_line 00000488 00000000 00000000 000006f1 2**0 + CONTENTS, READONLY, DEBUGGING + 7 .debug_frame 00000080 00000000 00000000 00000b7c 2**2 + CONTENTS, READONLY, DEBUGGING + 8 .debug_str 0000014f 00000000 00000000 00000bfc 2**0 + CONTENTS, READONLY, DEBUGGING + 9 .debug_loc 000002d8 00000000 00000000 00000d4b 2**0 + CONTENTS, READONLY, DEBUGGING + 10 .debug_ranges 00000078 00000000 00000000 00001023 2**0 + CONTENTS, READONLY, DEBUGGING + +Disassembly of section .text: + +00007e00
: +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 7e00: 11 24 eor r1, r1 +#ifdef __AVR_ATmega8__ + SP=RAMEND; // This is done by hardware reset +#endif + + // Adaboot no-wait mod + ch = MCUSR; + 7e02: 84 b7 in r24, 0x34 ; 52 + MCUSR = 0; + 7e04: 14 be out 0x34, r1 ; 52 + if (!(ch & _BV(EXTRF))) appStart(); + 7e06: 81 ff sbrs r24, 1 + 7e08: f0 d0 rcall .+480 ; 0x7fea + +#if LED_START_FLASHES > 0 + // Set up Timer 1 for timeout counter + TCCR1B = _BV(CS12) | _BV(CS10); // div 1024 + 7e0a: 85 e0 ldi r24, 0x05 ; 5 + 7e0c: 80 93 81 00 sts 0x0081, r24 + UCSRA = _BV(U2X); //Double speed mode USART + UCSRB = _BV(RXEN) | _BV(TXEN); // enable Rx & Tx + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); // config USART; 8N1 + UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); +#else + UCSR0A = _BV(U2X0); //Double speed mode USART0 + 7e10: 82 e0 ldi r24, 0x02 ; 2 + 7e12: 80 93 c0 00 sts 0x00C0, r24 + UCSR0B = _BV(RXEN0) | _BV(TXEN0); + 7e16: 88 e1 ldi r24, 0x18 ; 24 + 7e18: 80 93 c1 00 sts 0x00C1, r24 + UCSR0C = _BV(UCSZ00) | _BV(UCSZ01); + 7e1c: 86 e0 ldi r24, 0x06 ; 6 + 7e1e: 80 93 c2 00 sts 0x00C2, r24 + UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); + 7e22: 80 e1 ldi r24, 0x10 ; 16 + 7e24: 80 93 c4 00 sts 0x00C4, r24 +#endif +#endif + + // Set up watchdog to trigger after 500ms + watchdogConfig(WATCHDOG_1S); + 7e28: 8e e0 ldi r24, 0x0E ; 14 + 7e2a: c9 d0 rcall .+402 ; 0x7fbe + + /* Set LED pin as output */ + LED_DDR |= _BV(LED); + 7e2c: 25 9a sbi 0x04, 5 ; 4 + 7e2e: 86 e0 ldi r24, 0x06 ; 6 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 7e30: 20 e3 ldi r18, 0x30 ; 48 + 7e32: 3c ef ldi r19, 0xFC ; 252 + TIFR1 = _BV(TOV1); + 7e34: 91 e0 ldi r25, 0x01 ; 1 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 7e36: 30 93 85 00 sts 0x0085, r19 + 7e3a: 20 93 84 00 sts 0x0084, r18 + TIFR1 = _BV(TOV1); + 7e3e: 96 bb out 0x16, r25 ; 22 + while(!(TIFR1 & _BV(TOV1))); + 7e40: b0 9b sbis 0x16, 0 ; 22 + 7e42: fe cf rjmp .-4 ; 0x7e40 +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); + 7e44: 1d 9a sbi 0x03, 5 ; 3 +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 7e46: a8 95 wdr + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif + watchdogReset(); + } while (--count); + 7e48: 81 50 subi r24, 0x01 ; 1 + 7e4a: a9 f7 brne .-22 ; 0x7e36 + 7e4c: cc 24 eor r12, r12 + 7e4e: dd 24 eor r13, r13 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 7e50: 88 24 eor r8, r8 + 7e52: 83 94 inc r8 + addrPtr += 2; + } while (--ch); + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 7e54: b5 e0 ldi r27, 0x05 ; 5 + 7e56: ab 2e mov r10, r27 + boot_spm_busy_wait(); + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 7e58: a1 e1 ldi r26, 0x11 ; 17 + 7e5a: 9a 2e mov r9, r26 + do *bufPtr++ = getch(); + while (--length); + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 7e5c: f3 e0 ldi r31, 0x03 ; 3 + 7e5e: bf 2e mov r11, r31 +#endif + + /* Forever loop */ + for (;;) { + /* get character from UART */ + ch = getch(); + 7e60: a2 d0 rcall .+324 ; 0x7fa6 + + if(ch == STK_GET_PARAMETER) { + 7e62: 81 34 cpi r24, 0x41 ; 65 + 7e64: 61 f4 brne .+24 ; 0x7e7e + unsigned char which = getch(); + 7e66: 9f d0 rcall .+318 ; 0x7fa6 + 7e68: 08 2f mov r16, r24 + verifySpace(); + 7e6a: af d0 rcall .+350 ; 0x7fca + if (which == 0x82) { + 7e6c: 02 38 cpi r16, 0x82 ; 130 + 7e6e: 11 f0 breq .+4 ; 0x7e74 + /* + * Send optiboot version as "minor SW version" + */ + putch(OPTIBOOT_MINVER); + } else if (which == 0x81) { + 7e70: 01 38 cpi r16, 0x81 ; 129 + 7e72: 11 f4 brne .+4 ; 0x7e78 + putch(OPTIBOOT_MAJVER); + 7e74: 84 e0 ldi r24, 0x04 ; 4 + 7e76: 01 c0 rjmp .+2 ; 0x7e7a + } else { + /* + * GET PARAMETER returns a generic 0x03 reply for + * other parameters - enough to keep Avrdude happy + */ + putch(0x03); + 7e78: 83 e0 ldi r24, 0x03 ; 3 + 7e7a: 8d d0 rcall .+282 ; 0x7f96 + 7e7c: 89 c0 rjmp .+274 ; 0x7f90 + } + } + else if(ch == STK_SET_DEVICE) { + 7e7e: 82 34 cpi r24, 0x42 ; 66 + 7e80: 11 f4 brne .+4 ; 0x7e86 + // SET DEVICE is ignored + getNch(20); + 7e82: 84 e1 ldi r24, 0x14 ; 20 + 7e84: 03 c0 rjmp .+6 ; 0x7e8c + } + else if(ch == STK_SET_DEVICE_EXT) { + 7e86: 85 34 cpi r24, 0x45 ; 69 + 7e88: 19 f4 brne .+6 ; 0x7e90 + // SET DEVICE EXT is ignored + getNch(5); + 7e8a: 85 e0 ldi r24, 0x05 ; 5 + 7e8c: a6 d0 rcall .+332 ; 0x7fda + 7e8e: 80 c0 rjmp .+256 ; 0x7f90 + } + else if(ch == STK_LOAD_ADDRESS) { + 7e90: 85 35 cpi r24, 0x55 ; 85 + 7e92: 79 f4 brne .+30 ; 0x7eb2 + // LOAD ADDRESS + uint16_t newAddress; + newAddress = getch(); + 7e94: 88 d0 rcall .+272 ; 0x7fa6 + newAddress = (newAddress & 0xff) | (getch() << 8); + 7e96: e8 2e mov r14, r24 + 7e98: ff 24 eor r15, r15 + 7e9a: 85 d0 rcall .+266 ; 0x7fa6 + 7e9c: 08 2f mov r16, r24 + 7e9e: 10 e0 ldi r17, 0x00 ; 0 + 7ea0: 10 2f mov r17, r16 + 7ea2: 00 27 eor r16, r16 + 7ea4: 0e 29 or r16, r14 + 7ea6: 1f 29 or r17, r15 +#ifdef RAMPZ + // Transfer top bit to RAMPZ + RAMPZ = (newAddress & 0x8000) ? 1 : 0; +#endif + newAddress += newAddress; // Convert from word address to byte address + 7ea8: 00 0f add r16, r16 + 7eaa: 11 1f adc r17, r17 + address = newAddress; + verifySpace(); + 7eac: 8e d0 rcall .+284 ; 0x7fca + 7eae: 68 01 movw r12, r16 + 7eb0: 6f c0 rjmp .+222 ; 0x7f90 + } + else if(ch == STK_UNIVERSAL) { + 7eb2: 86 35 cpi r24, 0x56 ; 86 + 7eb4: 21 f4 brne .+8 ; 0x7ebe + // UNIVERSAL command is ignored + getNch(4); + 7eb6: 84 e0 ldi r24, 0x04 ; 4 + 7eb8: 90 d0 rcall .+288 ; 0x7fda + putch(0x00); + 7eba: 80 e0 ldi r24, 0x00 ; 0 + 7ebc: de cf rjmp .-68 ; 0x7e7a + } + /* Write memory, length is big endian and is in bytes */ + else if(ch == STK_PROG_PAGE) { + 7ebe: 84 36 cpi r24, 0x64 ; 100 + 7ec0: 09 f0 breq .+2 ; 0x7ec4 + 7ec2: 40 c0 rjmp .+128 ; 0x7f44 + // PROGRAM PAGE - we support flash programming only, not EEPROM + uint8_t *bufPtr; + uint16_t addrPtr; + + getch(); /* getlen() */ + 7ec4: 70 d0 rcall .+224 ; 0x7fa6 + length = getch(); + 7ec6: 6f d0 rcall .+222 ; 0x7fa6 + 7ec8: 08 2f mov r16, r24 + getch(); + 7eca: 6d d0 rcall .+218 ; 0x7fa6 + + // If we are in RWW section, immediately start page erase + if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 7ecc: 80 e0 ldi r24, 0x00 ; 0 + 7ece: c8 16 cp r12, r24 + 7ed0: 80 e7 ldi r24, 0x70 ; 112 + 7ed2: d8 06 cpc r13, r24 + 7ed4: 18 f4 brcc .+6 ; 0x7edc + 7ed6: f6 01 movw r30, r12 + 7ed8: b7 be out 0x37, r11 ; 55 + 7eda: e8 95 spm + 7edc: c0 e0 ldi r28, 0x00 ; 0 + 7ede: d1 e0 ldi r29, 0x01 ; 1 + + // While that is going on, read in page contents + bufPtr = buff; + do *bufPtr++ = getch(); + 7ee0: 62 d0 rcall .+196 ; 0x7fa6 + 7ee2: 89 93 st Y+, r24 + while (--length); + 7ee4: 0c 17 cp r16, r28 + 7ee6: e1 f7 brne .-8 ; 0x7ee0 + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 7ee8: f0 e0 ldi r31, 0x00 ; 0 + 7eea: cf 16 cp r12, r31 + 7eec: f0 e7 ldi r31, 0x70 ; 112 + 7eee: df 06 cpc r13, r31 + 7ef0: 18 f0 brcs .+6 ; 0x7ef8 + 7ef2: f6 01 movw r30, r12 + 7ef4: b7 be out 0x37, r11 ; 55 + 7ef6: e8 95 spm + + // Read command terminator, start reply + verifySpace(); + 7ef8: 68 d0 rcall .+208 ; 0x7fca + + // If only a partial page is to be programmed, the erase might not be complete. + // So check that here + boot_spm_busy_wait(); + 7efa: 07 b6 in r0, 0x37 ; 55 + 7efc: 00 fc sbrc r0, 0 + 7efe: fd cf rjmp .-6 ; 0x7efa + 7f00: a6 01 movw r20, r12 + 7f02: a0 e0 ldi r26, 0x00 ; 0 + 7f04: b1 e0 ldi r27, 0x01 ; 1 + bufPtr = buff; + addrPtr = (uint16_t)(void*)address; + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + 7f06: 2c 91 ld r18, X + 7f08: 30 e0 ldi r19, 0x00 ; 0 + a |= (*bufPtr++) << 8; + 7f0a: 11 96 adiw r26, 0x01 ; 1 + 7f0c: 8c 91 ld r24, X + 7f0e: 11 97 sbiw r26, 0x01 ; 1 + 7f10: 90 e0 ldi r25, 0x00 ; 0 + 7f12: 98 2f mov r25, r24 + 7f14: 88 27 eor r24, r24 + 7f16: 82 2b or r24, r18 + 7f18: 93 2b or r25, r19 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 7f1a: 12 96 adiw r26, 0x02 ; 2 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 7f1c: fa 01 movw r30, r20 + 7f1e: 0c 01 movw r0, r24 + 7f20: 87 be out 0x37, r8 ; 55 + 7f22: e8 95 spm + 7f24: 11 24 eor r1, r1 + addrPtr += 2; + 7f26: 4e 5f subi r20, 0xFE ; 254 + 7f28: 5f 4f sbci r21, 0xFF ; 255 + } while (--ch); + 7f2a: f1 e0 ldi r31, 0x01 ; 1 + 7f2c: a0 38 cpi r26, 0x80 ; 128 + 7f2e: bf 07 cpc r27, r31 + 7f30: 51 f7 brne .-44 ; 0x7f06 + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 7f32: f6 01 movw r30, r12 + 7f34: a7 be out 0x37, r10 ; 55 + 7f36: e8 95 spm + boot_spm_busy_wait(); + 7f38: 07 b6 in r0, 0x37 ; 55 + 7f3a: 00 fc sbrc r0, 0 + 7f3c: fd cf rjmp .-6 ; 0x7f38 + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 7f3e: 97 be out 0x37, r9 ; 55 + 7f40: e8 95 spm + 7f42: 26 c0 rjmp .+76 ; 0x7f90 +#endif + + } + /* Read memory block mode, length is big endian. */ + else if(ch == STK_READ_PAGE) { + 7f44: 84 37 cpi r24, 0x74 ; 116 + 7f46: b1 f4 brne .+44 ; 0x7f74 + // READ PAGE - we only read flash + getch(); /* getlen() */ + 7f48: 2e d0 rcall .+92 ; 0x7fa6 + length = getch(); + 7f4a: 2d d0 rcall .+90 ; 0x7fa6 + 7f4c: f8 2e mov r15, r24 + getch(); + 7f4e: 2b d0 rcall .+86 ; 0x7fa6 + + verifySpace(); + 7f50: 3c d0 rcall .+120 ; 0x7fca + 7f52: f6 01 movw r30, r12 + 7f54: ef 2c mov r14, r15 + putch(result); + address++; + } + while (--length); +#else + do putch(pgm_read_byte_near(address++)); + 7f56: 8f 01 movw r16, r30 + 7f58: 0f 5f subi r16, 0xFF ; 255 + 7f5a: 1f 4f sbci r17, 0xFF ; 255 + 7f5c: 84 91 lpm r24, Z+ + 7f5e: 1b d0 rcall .+54 ; 0x7f96 + while (--length); + 7f60: ea 94 dec r14 + 7f62: f8 01 movw r30, r16 + 7f64: c1 f7 brne .-16 ; 0x7f56 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 7f66: 08 94 sec + 7f68: c1 1c adc r12, r1 + 7f6a: d1 1c adc r13, r1 + 7f6c: fa 94 dec r15 + 7f6e: cf 0c add r12, r15 + 7f70: d1 1c adc r13, r1 + 7f72: 0e c0 rjmp .+28 ; 0x7f90 +#endif +#endif + } + + /* Get device signature bytes */ + else if(ch == STK_READ_SIGN) { + 7f74: 85 37 cpi r24, 0x75 ; 117 + 7f76: 39 f4 brne .+14 ; 0x7f86 + // READ SIGN - return what Avrdude wants to hear + verifySpace(); + 7f78: 28 d0 rcall .+80 ; 0x7fca + putch(SIGNATURE_0); + 7f7a: 8e e1 ldi r24, 0x1E ; 30 + 7f7c: 0c d0 rcall .+24 ; 0x7f96 + putch(SIGNATURE_1); + 7f7e: 85 e9 ldi r24, 0x95 ; 149 + 7f80: 0a d0 rcall .+20 ; 0x7f96 + putch(SIGNATURE_2); + 7f82: 8f e0 ldi r24, 0x0F ; 15 + 7f84: 7a cf rjmp .-268 ; 0x7e7a + } + else if (ch == 'Q') { + 7f86: 81 35 cpi r24, 0x51 ; 81 + 7f88: 11 f4 brne .+4 ; 0x7f8e + // Adaboot no-wait mod + watchdogConfig(WATCHDOG_16MS); + 7f8a: 88 e0 ldi r24, 0x08 ; 8 + 7f8c: 18 d0 rcall .+48 ; 0x7fbe + verifySpace(); + } + else { + // This covers the response to commands like STK_ENTER_PROGMODE + verifySpace(); + 7f8e: 1d d0 rcall .+58 ; 0x7fca + } + putch(STK_OK); + 7f90: 80 e1 ldi r24, 0x10 ; 16 + 7f92: 01 d0 rcall .+2 ; 0x7f96 + 7f94: 65 cf rjmp .-310 ; 0x7e60 + +00007f96 : + } +} + +void putch(char ch) { + 7f96: 98 2f mov r25, r24 +#ifndef SOFT_UART + while (!(UCSR0A & _BV(UDRE0))); + 7f98: 80 91 c0 00 lds r24, 0x00C0 + 7f9c: 85 ff sbrs r24, 5 + 7f9e: fc cf rjmp .-8 ; 0x7f98 + UDR0 = ch; + 7fa0: 90 93 c6 00 sts 0x00C6, r25 + [uartBit] "I" (UART_TX_BIT) + : + "r25" + ); +#endif +} + 7fa4: 08 95 ret + +00007fa6 : + [uartBit] "I" (UART_RX_BIT) + : + "r25" +); +#else + while(!(UCSR0A & _BV(RXC0))) + 7fa6: 80 91 c0 00 lds r24, 0x00C0 + 7faa: 87 ff sbrs r24, 7 + 7fac: fc cf rjmp .-8 ; 0x7fa6 + ; + if (!(UCSR0A & _BV(FE0))) { + 7fae: 80 91 c0 00 lds r24, 0x00C0 + 7fb2: 84 fd sbrc r24, 4 + 7fb4: 01 c0 rjmp .+2 ; 0x7fb8 +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 7fb6: a8 95 wdr + * don't care that an invalid char is returned...) + */ + watchdogReset(); + } + + ch = UDR0; + 7fb8: 80 91 c6 00 lds r24, 0x00C6 + LED_PIN |= _BV(LED); +#endif +#endif + + return ch; +} + 7fbc: 08 95 ret + +00007fbe : + "wdr\n" + ); +} + +void watchdogConfig(uint8_t x) { + WDTCSR = _BV(WDCE) | _BV(WDE); + 7fbe: e0 e6 ldi r30, 0x60 ; 96 + 7fc0: f0 e0 ldi r31, 0x00 ; 0 + 7fc2: 98 e1 ldi r25, 0x18 ; 24 + 7fc4: 90 83 st Z, r25 + WDTCSR = x; + 7fc6: 80 83 st Z, r24 +} + 7fc8: 08 95 ret + +00007fca : + do getch(); while (--count); + verifySpace(); +} + +void verifySpace() { + if (getch() != CRC_EOP) { + 7fca: ed df rcall .-38 ; 0x7fa6 + 7fcc: 80 32 cpi r24, 0x20 ; 32 + 7fce: 19 f0 breq .+6 ; 0x7fd6 + watchdogConfig(WATCHDOG_16MS); // shorten WD timeout + 7fd0: 88 e0 ldi r24, 0x08 ; 8 + 7fd2: f5 df rcall .-22 ; 0x7fbe + 7fd4: ff cf rjmp .-2 ; 0x7fd4 + while (1) // and busy-loop so that WD causes + ; // a reset and app start. + } + putch(STK_INSYNC); + 7fd6: 84 e1 ldi r24, 0x14 ; 20 +} + 7fd8: de cf rjmp .-68 ; 0x7f96 + +00007fda : + ::[count] "M" (UART_B_VALUE) + ); +} +#endif + +void getNch(uint8_t count) { + 7fda: 1f 93 push r17 + 7fdc: 18 2f mov r17, r24 + do getch(); while (--count); + 7fde: e3 df rcall .-58 ; 0x7fa6 + 7fe0: 11 50 subi r17, 0x01 ; 1 + 7fe2: e9 f7 brne .-6 ; 0x7fde + verifySpace(); + 7fe4: f2 df rcall .-28 ; 0x7fca +} + 7fe6: 1f 91 pop r17 + 7fe8: 08 95 ret + +00007fea : + WDTCSR = _BV(WDCE) | _BV(WDE); + WDTCSR = x; +} + +void appStart() { + watchdogConfig(WATCHDOG_OFF); + 7fea: 80 e0 ldi r24, 0x00 ; 0 + 7fec: e8 df rcall .-48 ; 0x7fbe + __asm__ __volatile__ ( + 7fee: ee 27 eor r30, r30 + 7ff0: ff 27 eor r31, r31 + 7ff2: 09 94 ijmp diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega8.lst b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega8.lst new file mode 100644 index 0000000..d921895 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/optiboot_atmega8.lst @@ -0,0 +1,604 @@ + +optiboot_atmega8.elf: file format elf32-avr + +Sections: +Idx Name Size VMA LMA File off Algn + 0 .text 000001e0 00001e00 00001e00 00000054 2**1 + CONTENTS, ALLOC, LOAD, READONLY, CODE + 1 .version 00000002 00001ffe 00001ffe 00000234 2**0 + CONTENTS, READONLY + 2 .debug_aranges 00000028 00000000 00000000 00000236 2**0 + CONTENTS, READONLY, DEBUGGING + 3 .debug_pubnames 0000005f 00000000 00000000 0000025e 2**0 + CONTENTS, READONLY, DEBUGGING + 4 .debug_info 000002a6 00000000 00000000 000002bd 2**0 + CONTENTS, READONLY, DEBUGGING + 5 .debug_abbrev 00000169 00000000 00000000 00000563 2**0 + CONTENTS, READONLY, DEBUGGING + 6 .debug_line 00000498 00000000 00000000 000006cc 2**0 + CONTENTS, READONLY, DEBUGGING + 7 .debug_frame 00000080 00000000 00000000 00000b64 2**2 + CONTENTS, READONLY, DEBUGGING + 8 .debug_str 0000014f 00000000 00000000 00000be4 2**0 + CONTENTS, READONLY, DEBUGGING + 9 .debug_loc 000002ba 00000000 00000000 00000d33 2**0 + CONTENTS, READONLY, DEBUGGING + 10 .debug_ranges 00000078 00000000 00000000 00000fed 2**0 + CONTENTS, READONLY, DEBUGGING + +Disassembly of section .text: + +00001e00
: +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 1e00: 11 24 eor r1, r1 + // + // If not, uncomment the following instructions: + // cli(); + asm volatile ("clr __zero_reg__"); +#ifdef __AVR_ATmega8__ + SP=RAMEND; // This is done by hardware reset + 1e02: 8f e5 ldi r24, 0x5F ; 95 + 1e04: 94 e0 ldi r25, 0x04 ; 4 + 1e06: 9e bf out 0x3e, r25 ; 62 + 1e08: 8d bf out 0x3d, r24 ; 61 +#endif + + // Adaboot no-wait mod + ch = MCUSR; + 1e0a: 84 b7 in r24, 0x34 ; 52 + MCUSR = 0; + 1e0c: 14 be out 0x34, r1 ; 52 + if (!(ch & _BV(EXTRF))) appStart(); + 1e0e: 81 ff sbrs r24, 1 + 1e10: e2 d0 rcall .+452 ; 0x1fd6 + +#if LED_START_FLASHES > 0 + // Set up Timer 1 for timeout counter + TCCR1B = _BV(CS12) | _BV(CS10); // div 1024 + 1e12: 85 e0 ldi r24, 0x05 ; 5 + 1e14: 8e bd out 0x2e, r24 ; 46 +#endif +#ifndef SOFT_UART +#ifdef __AVR_ATmega8__ + UCSRA = _BV(U2X); //Double speed mode USART + 1e16: 82 e0 ldi r24, 0x02 ; 2 + 1e18: 8b b9 out 0x0b, r24 ; 11 + UCSRB = _BV(RXEN) | _BV(TXEN); // enable Rx & Tx + 1e1a: 88 e1 ldi r24, 0x18 ; 24 + 1e1c: 8a b9 out 0x0a, r24 ; 10 + UCSRC = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0); // config USART; 8N1 + 1e1e: 86 e8 ldi r24, 0x86 ; 134 + 1e20: 80 bd out 0x20, r24 ; 32 + UBRRL = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); + 1e22: 80 e1 ldi r24, 0x10 ; 16 + 1e24: 89 b9 out 0x09, r24 ; 9 + UBRR0L = (uint8_t)( (F_CPU + BAUD_RATE * 4L) / (BAUD_RATE * 8L) - 1 ); +#endif +#endif + + // Set up watchdog to trigger after 500ms + watchdogConfig(WATCHDOG_1S); + 1e26: 8e e0 ldi r24, 0x0E ; 14 + 1e28: c2 d0 rcall .+388 ; 0x1fae + + /* Set LED pin as output */ + LED_DDR |= _BV(LED); + 1e2a: bd 9a sbi 0x17, 5 ; 23 + 1e2c: 96 e0 ldi r25, 0x06 ; 6 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 1e2e: 20 e3 ldi r18, 0x30 ; 48 + 1e30: 3c ef ldi r19, 0xFC ; 252 + TIFR1 = _BV(TOV1); + 1e32: 54 e0 ldi r21, 0x04 ; 4 + while(!(TIFR1 & _BV(TOV1))); +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); + 1e34: 40 e2 ldi r20, 0x20 ; 32 +} + +#if LED_START_FLASHES > 0 +void flash_led(uint8_t count) { + do { + TCNT1 = -(F_CPU/(1024*16)); + 1e36: 3d bd out 0x2d, r19 ; 45 + 1e38: 2c bd out 0x2c, r18 ; 44 + TIFR1 = _BV(TOV1); + 1e3a: 58 bf out 0x38, r21 ; 56 + while(!(TIFR1 & _BV(TOV1))); + 1e3c: 08 b6 in r0, 0x38 ; 56 + 1e3e: 02 fe sbrs r0, 2 + 1e40: fd cf rjmp .-6 ; 0x1e3c +#ifdef __AVR_ATmega8__ + LED_PORT ^= _BV(LED); + 1e42: 88 b3 in r24, 0x18 ; 24 + 1e44: 84 27 eor r24, r20 + 1e46: 88 bb out 0x18, r24 ; 24 +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 1e48: a8 95 wdr + LED_PORT ^= _BV(LED); +#else + LED_PIN |= _BV(LED); +#endif + watchdogReset(); + } while (--count); + 1e4a: 91 50 subi r25, 0x01 ; 1 + 1e4c: a1 f7 brne .-24 ; 0x1e36 + 1e4e: cc 24 eor r12, r12 + 1e50: dd 24 eor r13, r13 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 1e52: 88 24 eor r8, r8 + 1e54: 83 94 inc r8 + addrPtr += 2; + } while (--ch); + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 1e56: b5 e0 ldi r27, 0x05 ; 5 + 1e58: ab 2e mov r10, r27 + boot_spm_busy_wait(); + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 1e5a: a1 e1 ldi r26, 0x11 ; 17 + 1e5c: 9a 2e mov r9, r26 + do *bufPtr++ = getch(); + while (--length); + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 1e5e: f3 e0 ldi r31, 0x03 ; 3 + 1e60: bf 2e mov r11, r31 +#endif + + /* Forever loop */ + for (;;) { + /* get character from UART */ + ch = getch(); + 1e62: 9e d0 rcall .+316 ; 0x1fa0 + + if(ch == STK_GET_PARAMETER) { + 1e64: 81 34 cpi r24, 0x41 ; 65 + 1e66: 61 f4 brne .+24 ; 0x1e80 + unsigned char which = getch(); + 1e68: 9b d0 rcall .+310 ; 0x1fa0 + 1e6a: 08 2f mov r16, r24 + verifySpace(); + 1e6c: a4 d0 rcall .+328 ; 0x1fb6 + if (which == 0x82) { + 1e6e: 02 38 cpi r16, 0x82 ; 130 + 1e70: 11 f0 breq .+4 ; 0x1e76 + /* + * Send optiboot version as "minor SW version" + */ + putch(OPTIBOOT_MINVER); + } else if (which == 0x81) { + 1e72: 01 38 cpi r16, 0x81 ; 129 + 1e74: 11 f4 brne .+4 ; 0x1e7a + putch(OPTIBOOT_MAJVER); + 1e76: 84 e0 ldi r24, 0x04 ; 4 + 1e78: 01 c0 rjmp .+2 ; 0x1e7c + } else { + /* + * GET PARAMETER returns a generic 0x03 reply for + * other parameters - enough to keep Avrdude happy + */ + putch(0x03); + 1e7a: 83 e0 ldi r24, 0x03 ; 3 + 1e7c: 8d d0 rcall .+282 ; 0x1f98 + 1e7e: 89 c0 rjmp .+274 ; 0x1f92 + } + } + else if(ch == STK_SET_DEVICE) { + 1e80: 82 34 cpi r24, 0x42 ; 66 + 1e82: 11 f4 brne .+4 ; 0x1e88 + // SET DEVICE is ignored + getNch(20); + 1e84: 84 e1 ldi r24, 0x14 ; 20 + 1e86: 03 c0 rjmp .+6 ; 0x1e8e + } + else if(ch == STK_SET_DEVICE_EXT) { + 1e88: 85 34 cpi r24, 0x45 ; 69 + 1e8a: 19 f4 brne .+6 ; 0x1e92 + // SET DEVICE EXT is ignored + getNch(5); + 1e8c: 85 e0 ldi r24, 0x05 ; 5 + 1e8e: 9b d0 rcall .+310 ; 0x1fc6 + 1e90: 80 c0 rjmp .+256 ; 0x1f92 + } + else if(ch == STK_LOAD_ADDRESS) { + 1e92: 85 35 cpi r24, 0x55 ; 85 + 1e94: 79 f4 brne .+30 ; 0x1eb4 + // LOAD ADDRESS + uint16_t newAddress; + newAddress = getch(); + 1e96: 84 d0 rcall .+264 ; 0x1fa0 + newAddress = (newAddress & 0xff) | (getch() << 8); + 1e98: e8 2e mov r14, r24 + 1e9a: ff 24 eor r15, r15 + 1e9c: 81 d0 rcall .+258 ; 0x1fa0 + 1e9e: 08 2f mov r16, r24 + 1ea0: 10 e0 ldi r17, 0x00 ; 0 + 1ea2: 10 2f mov r17, r16 + 1ea4: 00 27 eor r16, r16 + 1ea6: 0e 29 or r16, r14 + 1ea8: 1f 29 or r17, r15 +#ifdef RAMPZ + // Transfer top bit to RAMPZ + RAMPZ = (newAddress & 0x8000) ? 1 : 0; +#endif + newAddress += newAddress; // Convert from word address to byte address + 1eaa: 00 0f add r16, r16 + 1eac: 11 1f adc r17, r17 + address = newAddress; + verifySpace(); + 1eae: 83 d0 rcall .+262 ; 0x1fb6 + 1eb0: 68 01 movw r12, r16 + 1eb2: 6f c0 rjmp .+222 ; 0x1f92 + } + else if(ch == STK_UNIVERSAL) { + 1eb4: 86 35 cpi r24, 0x56 ; 86 + 1eb6: 21 f4 brne .+8 ; 0x1ec0 + // UNIVERSAL command is ignored + getNch(4); + 1eb8: 84 e0 ldi r24, 0x04 ; 4 + 1eba: 85 d0 rcall .+266 ; 0x1fc6 + putch(0x00); + 1ebc: 80 e0 ldi r24, 0x00 ; 0 + 1ebe: de cf rjmp .-68 ; 0x1e7c + } + /* Write memory, length is big endian and is in bytes */ + else if(ch == STK_PROG_PAGE) { + 1ec0: 84 36 cpi r24, 0x64 ; 100 + 1ec2: 09 f0 breq .+2 ; 0x1ec6 + 1ec4: 40 c0 rjmp .+128 ; 0x1f46 + // PROGRAM PAGE - we support flash programming only, not EEPROM + uint8_t *bufPtr; + uint16_t addrPtr; + + getch(); /* getlen() */ + 1ec6: 6c d0 rcall .+216 ; 0x1fa0 + length = getch(); + 1ec8: 6b d0 rcall .+214 ; 0x1fa0 + 1eca: 08 2f mov r16, r24 + getch(); + 1ecc: 69 d0 rcall .+210 ; 0x1fa0 + + // If we are in RWW section, immediately start page erase + if (address < NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 1ece: 80 e0 ldi r24, 0x00 ; 0 + 1ed0: c8 16 cp r12, r24 + 1ed2: 88 e1 ldi r24, 0x18 ; 24 + 1ed4: d8 06 cpc r13, r24 + 1ed6: 18 f4 brcc .+6 ; 0x1ede + 1ed8: f6 01 movw r30, r12 + 1eda: b7 be out 0x37, r11 ; 55 + 1edc: e8 95 spm + 1ede: c0 e0 ldi r28, 0x00 ; 0 + 1ee0: d1 e0 ldi r29, 0x01 ; 1 + + // While that is going on, read in page contents + bufPtr = buff; + do *bufPtr++ = getch(); + 1ee2: 5e d0 rcall .+188 ; 0x1fa0 + 1ee4: 89 93 st Y+, r24 + while (--length); + 1ee6: 0c 17 cp r16, r28 + 1ee8: e1 f7 brne .-8 ; 0x1ee2 + + // If we are in NRWW section, page erase has to be delayed until now. + // Todo: Take RAMPZ into account + if (address >= NRWWSTART) __boot_page_erase_short((uint16_t)(void*)address); + 1eea: f0 e0 ldi r31, 0x00 ; 0 + 1eec: cf 16 cp r12, r31 + 1eee: f8 e1 ldi r31, 0x18 ; 24 + 1ef0: df 06 cpc r13, r31 + 1ef2: 18 f0 brcs .+6 ; 0x1efa + 1ef4: f6 01 movw r30, r12 + 1ef6: b7 be out 0x37, r11 ; 55 + 1ef8: e8 95 spm + + // Read command terminator, start reply + verifySpace(); + 1efa: 5d d0 rcall .+186 ; 0x1fb6 + + // If only a partial page is to be programmed, the erase might not be complete. + // So check that here + boot_spm_busy_wait(); + 1efc: 07 b6 in r0, 0x37 ; 55 + 1efe: 00 fc sbrc r0, 0 + 1f00: fd cf rjmp .-6 ; 0x1efc + 1f02: a6 01 movw r20, r12 + 1f04: a0 e0 ldi r26, 0x00 ; 0 + 1f06: b1 e0 ldi r27, 0x01 ; 1 + bufPtr = buff; + addrPtr = (uint16_t)(void*)address; + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + 1f08: 2c 91 ld r18, X + 1f0a: 30 e0 ldi r19, 0x00 ; 0 + a |= (*bufPtr++) << 8; + 1f0c: 11 96 adiw r26, 0x01 ; 1 + 1f0e: 8c 91 ld r24, X + 1f10: 11 97 sbiw r26, 0x01 ; 1 + 1f12: 90 e0 ldi r25, 0x00 ; 0 + 1f14: 98 2f mov r25, r24 + 1f16: 88 27 eor r24, r24 + 1f18: 82 2b or r24, r18 + 1f1a: 93 2b or r25, r19 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 1f1c: 12 96 adiw r26, 0x02 ; 2 + ch = SPM_PAGESIZE / 2; + do { + uint16_t a; + a = *bufPtr++; + a |= (*bufPtr++) << 8; + __boot_page_fill_short((uint16_t)(void*)addrPtr,a); + 1f1e: fa 01 movw r30, r20 + 1f20: 0c 01 movw r0, r24 + 1f22: 87 be out 0x37, r8 ; 55 + 1f24: e8 95 spm + 1f26: 11 24 eor r1, r1 + addrPtr += 2; + 1f28: 4e 5f subi r20, 0xFE ; 254 + 1f2a: 5f 4f sbci r21, 0xFF ; 255 + } while (--ch); + 1f2c: f1 e0 ldi r31, 0x01 ; 1 + 1f2e: a0 34 cpi r26, 0x40 ; 64 + 1f30: bf 07 cpc r27, r31 + 1f32: 51 f7 brne .-44 ; 0x1f08 + + // Write from programming buffer + __boot_page_write_short((uint16_t)(void*)address); + 1f34: f6 01 movw r30, r12 + 1f36: a7 be out 0x37, r10 ; 55 + 1f38: e8 95 spm + boot_spm_busy_wait(); + 1f3a: 07 b6 in r0, 0x37 ; 55 + 1f3c: 00 fc sbrc r0, 0 + 1f3e: fd cf rjmp .-6 ; 0x1f3a + +#if defined(RWWSRE) + // Reenable read access to flash + boot_rww_enable(); + 1f40: 97 be out 0x37, r9 ; 55 + 1f42: e8 95 spm + 1f44: 26 c0 rjmp .+76 ; 0x1f92 +#endif + + } + /* Read memory block mode, length is big endian. */ + else if(ch == STK_READ_PAGE) { + 1f46: 84 37 cpi r24, 0x74 ; 116 + 1f48: b1 f4 brne .+44 ; 0x1f76 + // READ PAGE - we only read flash + getch(); /* getlen() */ + 1f4a: 2a d0 rcall .+84 ; 0x1fa0 + length = getch(); + 1f4c: 29 d0 rcall .+82 ; 0x1fa0 + 1f4e: f8 2e mov r15, r24 + getch(); + 1f50: 27 d0 rcall .+78 ; 0x1fa0 + + verifySpace(); + 1f52: 31 d0 rcall .+98 ; 0x1fb6 + 1f54: f6 01 movw r30, r12 + 1f56: ef 2c mov r14, r15 + putch(result); + address++; + } + while (--length); +#else + do putch(pgm_read_byte_near(address++)); + 1f58: 8f 01 movw r16, r30 + 1f5a: 0f 5f subi r16, 0xFF ; 255 + 1f5c: 1f 4f sbci r17, 0xFF ; 255 + 1f5e: 84 91 lpm r24, Z+ + 1f60: 1b d0 rcall .+54 ; 0x1f98 + while (--length); + 1f62: ea 94 dec r14 + 1f64: f8 01 movw r30, r16 + 1f66: c1 f7 brne .-16 ; 0x1f58 +#define rstVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+4)) +#define wdtVect (*(uint16_t*)(RAMSTART+SPM_PAGESIZE*2+6)) +#endif + +/* main program starts here */ +int main(void) { + 1f68: 08 94 sec + 1f6a: c1 1c adc r12, r1 + 1f6c: d1 1c adc r13, r1 + 1f6e: fa 94 dec r15 + 1f70: cf 0c add r12, r15 + 1f72: d1 1c adc r13, r1 + 1f74: 0e c0 rjmp .+28 ; 0x1f92 +#endif +#endif + } + + /* Get device signature bytes */ + else if(ch == STK_READ_SIGN) { + 1f76: 85 37 cpi r24, 0x75 ; 117 + 1f78: 39 f4 brne .+14 ; 0x1f88 + // READ SIGN - return what Avrdude wants to hear + verifySpace(); + 1f7a: 1d d0 rcall .+58 ; 0x1fb6 + putch(SIGNATURE_0); + 1f7c: 8e e1 ldi r24, 0x1E ; 30 + 1f7e: 0c d0 rcall .+24 ; 0x1f98 + putch(SIGNATURE_1); + 1f80: 83 e9 ldi r24, 0x93 ; 147 + 1f82: 0a d0 rcall .+20 ; 0x1f98 + putch(SIGNATURE_2); + 1f84: 87 e0 ldi r24, 0x07 ; 7 + 1f86: 7a cf rjmp .-268 ; 0x1e7c + } + else if (ch == 'Q') { + 1f88: 81 35 cpi r24, 0x51 ; 81 + 1f8a: 11 f4 brne .+4 ; 0x1f90 + // Adaboot no-wait mod + watchdogConfig(WATCHDOG_16MS); + 1f8c: 88 e0 ldi r24, 0x08 ; 8 + 1f8e: 0f d0 rcall .+30 ; 0x1fae + verifySpace(); + } + else { + // This covers the response to commands like STK_ENTER_PROGMODE + verifySpace(); + 1f90: 12 d0 rcall .+36 ; 0x1fb6 + } + putch(STK_OK); + 1f92: 80 e1 ldi r24, 0x10 ; 16 + 1f94: 01 d0 rcall .+2 ; 0x1f98 + 1f96: 65 cf rjmp .-310 ; 0x1e62 + +00001f98 : + } +} + +void putch(char ch) { +#ifndef SOFT_UART + while (!(UCSR0A & _BV(UDRE0))); + 1f98: 5d 9b sbis 0x0b, 5 ; 11 + 1f9a: fe cf rjmp .-4 ; 0x1f98 + UDR0 = ch; + 1f9c: 8c b9 out 0x0c, r24 ; 12 + [uartBit] "I" (UART_TX_BIT) + : + "r25" + ); +#endif +} + 1f9e: 08 95 ret + +00001fa0 : + [uartBit] "I" (UART_RX_BIT) + : + "r25" +); +#else + while(!(UCSR0A & _BV(RXC0))) + 1fa0: 5f 9b sbis 0x0b, 7 ; 11 + 1fa2: fe cf rjmp .-4 ; 0x1fa0 + ; + if (!(UCSR0A & _BV(FE0))) { + 1fa4: 5c 99 sbic 0x0b, 4 ; 11 + 1fa6: 01 c0 rjmp .+2 ; 0x1faa +} +#endif + +// Watchdog functions. These are only safe with interrupts turned off. +void watchdogReset() { + __asm__ __volatile__ ( + 1fa8: a8 95 wdr + * don't care that an invalid char is returned...) + */ + watchdogReset(); + } + + ch = UDR0; + 1faa: 8c b1 in r24, 0x0c ; 12 + LED_PIN |= _BV(LED); +#endif +#endif + + return ch; +} + 1fac: 08 95 ret + +00001fae : + "wdr\n" + ); +} + +void watchdogConfig(uint8_t x) { + WDTCSR = _BV(WDCE) | _BV(WDE); + 1fae: 98 e1 ldi r25, 0x18 ; 24 + 1fb0: 91 bd out 0x21, r25 ; 33 + WDTCSR = x; + 1fb2: 81 bd out 0x21, r24 ; 33 +} + 1fb4: 08 95 ret + +00001fb6 : + do getch(); while (--count); + verifySpace(); +} + +void verifySpace() { + if (getch() != CRC_EOP) { + 1fb6: f4 df rcall .-24 ; 0x1fa0 + 1fb8: 80 32 cpi r24, 0x20 ; 32 + 1fba: 19 f0 breq .+6 ; 0x1fc2 + watchdogConfig(WATCHDOG_16MS); // shorten WD timeout + 1fbc: 88 e0 ldi r24, 0x08 ; 8 + 1fbe: f7 df rcall .-18 ; 0x1fae + 1fc0: ff cf rjmp .-2 ; 0x1fc0 + while (1) // and busy-loop so that WD causes + ; // a reset and app start. + } + putch(STK_INSYNC); + 1fc2: 84 e1 ldi r24, 0x14 ; 20 +} + 1fc4: e9 cf rjmp .-46 ; 0x1f98 + +00001fc6 : + ::[count] "M" (UART_B_VALUE) + ); +} +#endif + +void getNch(uint8_t count) { + 1fc6: 1f 93 push r17 + 1fc8: 18 2f mov r17, r24 + do getch(); while (--count); + 1fca: ea df rcall .-44 ; 0x1fa0 + 1fcc: 11 50 subi r17, 0x01 ; 1 + 1fce: e9 f7 brne .-6 ; 0x1fca + verifySpace(); + 1fd0: f2 df rcall .-28 ; 0x1fb6 +} + 1fd2: 1f 91 pop r17 + 1fd4: 08 95 ret + +00001fd6 : + WDTCSR = _BV(WDCE) | _BV(WDE); + WDTCSR = x; +} + +void appStart() { + watchdogConfig(WATCHDOG_OFF); + 1fd6: 80 e0 ldi r24, 0x00 ; 0 + 1fd8: ea df rcall .-44 ; 0x1fae + __asm__ __volatile__ ( + 1fda: ee 27 eor r30, r30 + 1fdc: ff 27 eor r31, r31 + 1fde: 09 94 ijmp diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/pin_defs.h b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/pin_defs.h new file mode 100644 index 0000000..27d7772 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/pin_defs.h @@ -0,0 +1,80 @@ +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega88) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega88__) +/* Onboard LED is connected to pin PB5 in Arduino NG, Diecimila, and Duemilanove */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB5 + +/* Ports for soft UART */ +#ifdef SOFT_UART +#define UART_PORT PORTD +#define UART_PIN PIND +#define UART_DDR DDRD +#define UART_TX_BIT 1 +#define UART_RX_BIT 0 +#endif +#endif + +#if defined(__AVR_ATmega8__) + //Name conversion R.Wiersma + #define UCSR0A UCSRA + #define UDR0 UDR + #define UDRE0 UDRE + #define RXC0 RXC + #define FE0 FE + #define TIFR1 TIFR + #define WDTCSR WDTCR +#endif + +/* Luminet support */ +#if defined(__AVR_ATtiny84__) +/* Red LED is connected to pin PA4 */ +#define LED_DDR DDRA +#define LED_PORT PORTA +#define LED_PIN PINA +#define LED PINA4 +/* Ports for soft UART - left port only for now. TX/RX on PA2/PA3 */ +#ifdef SOFT_UART +#define UART_PORT PORTA +#define UART_PIN PINA +#define UART_DDR DDRA +#define UART_TX_BIT 2 +#define UART_RX_BIT 3 +#endif +#endif + +/* Sanguino support */ +#if defined(__AVR_ATmega644P__) +/* Onboard LED is connected to pin PB0 on Sanguino */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB0 + +/* Ports for soft UART */ +#ifdef SOFT_UART +#define UART_PORT PORTD +#define UART_PIN PIND +#define UART_DDR DDRD +#define UART_TX_BIT 1 +#define UART_RX_BIT 0 +#endif +#endif + +/* Mega support */ +#if defined(__AVR_ATmega1280__) +/* Onboard LED is connected to pin PB7 on Arduino Mega */ +#define LED_DDR DDRB +#define LED_PORT PORTB +#define LED_PIN PINB +#define LED PINB7 + +/* Ports for soft UART */ +#ifdef SOFT_UART +#define UART_PORT PORTE +#define UART_PIN PINE +#define UART_DDR DDRE +#define UART_TX_BIT 1 +#define UART_RX_BIT 0 +#endif +#endif diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/stk500.h b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/stk500.h new file mode 100644 index 0000000..ca0dd91 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/optiboot/stk500.h @@ -0,0 +1,39 @@ +/* STK500 constants list, from AVRDUDE */ +#define STK_OK 0x10 +#define STK_FAILED 0x11 // Not used +#define STK_UNKNOWN 0x12 // Not used +#define STK_NODEVICE 0x13 // Not used +#define STK_INSYNC 0x14 // ' ' +#define STK_NOSYNC 0x15 // Not used +#define ADC_CHANNEL_ERROR 0x16 // Not used +#define ADC_MEASURE_OK 0x17 // Not used +#define PWM_CHANNEL_ERROR 0x18 // Not used +#define PWM_ADJUST_OK 0x19 // Not used +#define CRC_EOP 0x20 // 'SPACE' +#define STK_GET_SYNC 0x30 // '0' +#define STK_GET_SIGN_ON 0x31 // '1' +#define STK_SET_PARAMETER 0x40 // '@' +#define STK_GET_PARAMETER 0x41 // 'A' +#define STK_SET_DEVICE 0x42 // 'B' +#define STK_SET_DEVICE_EXT 0x45 // 'E' +#define STK_ENTER_PROGMODE 0x50 // 'P' +#define STK_LEAVE_PROGMODE 0x51 // 'Q' +#define STK_CHIP_ERASE 0x52 // 'R' +#define STK_CHECK_AUTOINC 0x53 // 'S' +#define STK_LOAD_ADDRESS 0x55 // 'U' +#define STK_UNIVERSAL 0x56 // 'V' +#define STK_PROG_FLASH 0x60 // '`' +#define STK_PROG_DATA 0x61 // 'a' +#define STK_PROG_FUSE 0x62 // 'b' +#define STK_PROG_LOCK 0x63 // 'c' +#define STK_PROG_PAGE 0x64 // 'd' +#define STK_PROG_FUSE_EXT 0x65 // 'e' +#define STK_READ_FLASH 0x70 // 'p' +#define STK_READ_DATA 0x71 // 'q' +#define STK_READ_FUSE 0x72 // 'r' +#define STK_READ_LOCK 0x73 // 's' +#define STK_READ_PAGE 0x74 // 't' +#define STK_READ_SIGN 0x75 // 'u' +#define STK_READ_OSCCAL 0x76 // 'v' +#define STK_READ_FUSE_EXT 0x77 // 'w' +#define STK_READ_OSCCAL_EXT 0x78 // 'x' diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/Makefile b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/Makefile new file mode 100644 index 0000000..bfed912 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/Makefile @@ -0,0 +1,588 @@ +# ---------------------------------------------------------------------------- +# Makefile to compile and link stk500boot bootloader +# Author: Peter Fleury +# File: $Id: Makefile,v 1.3 2006/03/04 19:26:17 peter Exp $ +# based on WinAVR Makefile Template written by Eric B. Weddington, Jrg Wunsch, et al. +# +# Adjust F_CPU below to the clock frequency in Mhz of your AVR target +# Adjust BOOTLOADER_ADDRESS to your AVR target +# +#---------------------------------------------------------------------------- +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF. +# +# make extcoff = Convert ELF to AVR Extended COFF. +# +# make program = Download the hex file to the device, using avrdude. +# Please customize the avrdude settings below first! +# +# make debug = Start either simulavr or avarice as specified for debugging, +# with avr-gdb or avr-insight as the front end for debugging. +# +# make filename.s = Just compile filename.c into the assembler code only. +# +# make filename.i = Create a preprocessed source file for use in submitting +# bug reports to the GCC project. +# +# To rebuild project do "make clean" then "make all". +#---------------------------------------------------------------------------- +# = Mark Sproul msproul-at-skychariot.com + + +# MCU name +#MCU = atmega128 + + +# Processor frequency. +# This will define a symbol, F_CPU, in all source code files equal to the +# processor frequency. You can then use this symbol in your source code to +# calculate timings. Do NOT tack on a 'UL' at the end, this will be done +# automatically to create a 32-bit value in your source code. +#F_CPU = 16000000 + + +# Bootloader +# Please adjust if using a different AVR +# 0x0e00*2=0x1C00 for ATmega8 512 words Boot Size +# 0xFC00*2=0x1F800 for ATmega128 1024 words Boot Size +# 0xF800*2=0x1F000 for ATmega1280 +# 0xF000*2=0x1E000 for ATmega1280 +#BOOTLOADER_ADDRESS = 1E000 + + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + + +# Target file name (without extension). +TARGET = stk500boot + + +# List C source files here. (C dependencies are automatically generated.) +SRC = stk500boot.c + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + + +# Debugging format. +# Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. +# AVR Studio 4.10 requires dwarf-2. +# AVR [Extended] COFF format requires stabs, plus an avr-objcopy run. +DEBUG = dwarf-2 + + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRAINCDIRS = + + +# Compiler flag to set the C Standard level. +# c89 = "ANSI" C +# gnu89 = c89 plus GCC extensions +# c99 = ISO C99 standard (not yet fully implemented) +# gnu99 = c99 plus GCC extensions +CSTANDARD = -std=gnu99 + + +# Place -D or -U options here +CDEFS = -DF_CPU=$(F_CPU)UL + + +# Place -I options here +CINCS = + + + +#---------------- Compiler Options ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) $(CINCS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -mno-tablejump +CFLAGS += -Wall -Wstrict-prototypes +CFLAGS += -Wa,-adhlns=$(<:.c=.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) + + +#---------------- Assembler Options ---------------- +# -Wa,...: tell GCC to pass this to the assembler. +# -ahlms: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs + + +#---------------- Library Options ---------------- +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt + +# If this is left blank, then it will use the Standard printf version. +PRINTF_LIB = +#PRINTF_LIB = $(PRINTF_LIB_MIN) +#PRINTF_LIB = $(PRINTF_LIB_FLOAT) + + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +# If this is left blank, then it will use the Standard scanf version. +SCANF_LIB = +#SCANF_LIB = $(SCANF_LIB_MIN) +#SCANF_LIB = $(SCANF_LIB_FLOAT) + + +MATH_LIB = -lm + + + +#---------------- External Memory Options ---------------- + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + + + + +#---------------- Linker Options ---------------- +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) + + +#--------------- bootloader linker Options ------- +# BOOTLOADER_ADDRESS (=Start of Boot Loader section +# in bytes - not words) is defined above. +#LDFLAGS += -Wl,--section-start=.text=$(BOOTLOADER_ADDRESS) -nostartfiles -nodefaultlibs +#LDFLAGS += -Wl,--section-start=.text=$(BOOTLOADER_ADDRESS) -nostartfiles +LDFLAGS += -Wl,--section-start=.text=$(BOOTLOADER_ADDRESS) + +#---------------- Programming Options (avrdude) ---------------- + +# Programming hardware: alf avr910 avrisp bascom bsd +# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500 +# +# Type: avrdude -c ? +# to get a full listing. +# +AVRDUDE_PROGRAMMER = stk500v2 + +# com1 = serial port. Use lpt1 to connect to parallel port. +AVRDUDE_PORT = com1 # programmer connected to serial device + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +#---------------- Debugging Options ---------------- + +# For simulavr only - target MCU frequency. +DEBUG_MFREQ = $(F_CPU) + +# Set the DEBUG_UI to either gdb or insight. +# DEBUG_UI = gdb +DEBUG_UI = insight + +# Set the debugging back-end to either avarice, simulavr. +DEBUG_BACKEND = avarice +#DEBUG_BACKEND = simulavr + +# GDB Init Filename. +GDBINIT_FILE = __avr_gdbinit + +# When using avarice settings for the JTAG +JTAG_DEV = /dev/com1 + +# Debugging port used to communicate between GDB / avarice / simulavr. +DEBUG_PORT = 4242 + +# Debugging host used to communicate between GDB / avarice / simulavr, normally +# just set to localhost unless doing some sort of crazy debugging when +# avarice is running on a different computer. +DEBUG_HOST = localhost + + + +#============================================================================ + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +NM = avr-nm +AVRDUDE = avrdude +REMOVE = rm -f +COPY = cp +WINSHELL = cmd + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: + + + + +# Define all object files. +OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) + +# Define all listing files. +LST = $(SRC:.c=.lst) $(ASRC:.S=.lst) + + +# Compiler flags to generate dependency files. +GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d + + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + +############################################################ +# May 25, 2010 Adding 1280 support +mega1280: MCU = atmega1280 +mega1280: F_CPU = 16000000 +mega1280: BOOTLOADER_ADDRESS = 1E000 +mega1280: CFLAGS += -D_MEGA_BOARD_ +mega1280: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_mega1280.hex + + +############################################################ +# Jul 6, 2010 Adding 2560 support +mega2560: MCU = atmega2560 +mega2560: F_CPU = 16000000 +mega2560: BOOTLOADER_ADDRESS = 3E000 +mega2560: CFLAGS += -D_MEGA_BOARD_ +mega2560: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_mega2560.hex + + +############################################################ +#Initial config on Amber128 board +# avrdude: Device signature = 0x1e9702 +# avrdude: safemode: lfuse reads as 8F +# avrdude: safemode: hfuse reads as CB +# avrdude: safemode: efuse reads as FF +# Jul 17, 2010 Adding 128 support +############################################################ +amber128: MCU = atmega128 +#amber128: F_CPU = 16000000 +amber128: F_CPU = 14745600 +amber128: BOOTLOADER_ADDRESS = 1E000 +amber128: CFLAGS += -D_BOARD_AMBER128_ +amber128: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_amber128.hex + +############################################################ +# Aug 23, 2010 Adding atmega2561 support +m2561: MCU = atmega2561 +m2561: F_CPU = 8000000 +m2561: BOOTLOADER_ADDRESS = 3E000 +m2561: CFLAGS += -D_ANDROID_2561_ -DBAUDRATE=57600 +m2561: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_android2561.hex + + +############################################################ +# avrdude: Device signature = 0x1e9801 +# avrdude: safemode: lfuse reads as EC +# avrdude: safemode: hfuse reads as 18 +# avrdude: safemode: efuse reads as FD +# Aug 23, 2010 Adding cerebot 2560 @ 8mhz +#avrdude -P usb -c usbtiny -p m2560 -v -U flash:w:/Arduino/WiringBootV2_upd1/stk500boot_v2_cerebotplus.hex +############################################################ +cerebot: MCU = atmega2560 +cerebot: F_CPU = 8000000 +cerebot: BOOTLOADER_ADDRESS = 3E000 +cerebot: CFLAGS += -D_CEREBOTPLUS_BOARD_ -DBAUDRATE=38400 -DUART_BAUDRATE_DOUBLE_SPEED=1 +cerebot: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_cerebotplus.hex + + +############################################################ +# Aug 23, 2010 Adding atmega2561 support +penguino: MCU = atmega32 +penguino: F_CPU = 16000000 +penguino: BOOTLOADER_ADDRESS = 7800 +penguino: CFLAGS += -D_PENGUINO_ -DBAUDRATE=57600 +penguino: begin gccversion sizebefore build sizeafter end + mv $(TARGET).hex stk500boot_v2_penguino.hex + + +# Default target. +all: begin gccversion sizebefore build sizeafter end + +build: elf hex eep lss sym +#build: hex eep lss sym + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) --format=avr --mcu=$(MCU) $(TARGET).elf + +sizebefore: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +sizeafter: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \ + 2>/dev/null; echo; fi + + + +# Display compiler version information. +gccversion : + @$(CC) --version + + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + + +# Generate avr-gdb config/init file which does the following: +# define the reset signal, load the target file, connect to target, and set +# a breakpoint at main(). +gdb-config: + @$(REMOVE) $(GDBINIT_FILE) + @echo define reset >> $(GDBINIT_FILE) + @echo SIGNAL SIGHUP >> $(GDBINIT_FILE) + @echo end >> $(GDBINIT_FILE) + @echo file $(TARGET).elf >> $(GDBINIT_FILE) + @echo target remote $(DEBUG_HOST):$(DEBUG_PORT) >> $(GDBINIT_FILE) +ifeq ($(DEBUG_BACKEND),simulavr) + @echo load >> $(GDBINIT_FILE) +endif + @echo break main >> $(GDBINIT_FILE) + +debug: gdb-config $(TARGET).elf +ifeq ($(DEBUG_BACKEND), avarice) + @echo Starting AVaRICE - Press enter when "waiting to connect" message displays. + @$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \ + $(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT) + @$(WINSHELL) /c pause + +else + @$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \ + $(DEBUG_MFREQ) --port $(DEBUG_PORT) +endif + @$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE) + + + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT=$(OBJCOPY) --debugging \ +--change-section-address .data-0x800000 \ +--change-section-address .bss-0x800000 \ +--change-section-address .noinit-0x800000 \ +--change-section-address .eeprom-0x810000 + + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 -O $(FORMAT) $< $@ + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + +# Create preprocessed source for use in sending a bug report. +%.i : %.c + $(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ + + +# Target: clean project. +clean: begin clean_list end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) *.hex + $(REMOVE) *.eep + $(REMOVE) *.cof + $(REMOVE) *.elf + $(REMOVE) *.map + $(REMOVE) *.sym + $(REMOVE) *.lss + $(REMOVE) $(OBJ) + $(REMOVE) $(LST) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) .dep/* + + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff \ +clean clean_list program debug gdb-config + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnproj b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnproj new file mode 100644 index 0000000..d935019 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnproj @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnps b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnps new file mode 100644 index 0000000..f85cde5 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/STK500V2.pnps @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/avrinterruptnames.h b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/avrinterruptnames.h new file mode 100644 index 0000000..0ae80f9 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/avrinterruptnames.h @@ -0,0 +1,742 @@ +//************************************************************************************************** +//* +//* interrupt vector names +//* +//* It is important to note that the vector numbers listed here +//* are the ATMEL documentation numbers. The Arduino numbers are 1 less +//* This is because the Atmel docs start numbering the interrupts at 1 +//* when it is actually vector #0 in the table. +//************************************************************************************************** +//* Jun 1, 2010 Added support for ATmega1281 +//* Jun 30, 2010 Putting in more ifdefs to conserve space +//* Jul 3, 2010 More #ifdefs to conserve space and testing on most of my boards +//* Jul 4, 2010 Started using vector defs for #ifdefs as defined in +//* Jul 13, 2010 Added support for __AVR_ATmega128__ +//* Aug 26, 2010 Added support for __AVR_ATmega2561__ +//************************************************************************************************** + +//#include "avrinterruptnames.h" + +//************************************************************************************************** +//* this defines the interrupt vectors and allows us to compile ONLY those strings that are actually +//* in the target CPU. This way we do not have to keep making changes based on cpu, it will be +//* automatic even if we add a new CPU +#ifndef _AVR_IO_H_ + #include +#endif +//************************************************************************************************** + +#ifdef __MWERKS__ + #define prog_char char + #define PGM_P char * +#endif + + prog_char gAvrInt_RESET[] PROGMEM = "RESET"; +#ifdef INT0_vect + prog_char gAvrInt_INT0[] PROGMEM = "INT0"; +#endif +#ifdef INT1_vect + prog_char gAvrInt_INT1[] PROGMEM = "INT1"; +#endif +#ifdef INT2_vect + prog_char gAvrInt_INT2[] PROGMEM = "INT2"; +#endif +#ifdef INT3_vect + prog_char gAvrInt_INT3[] PROGMEM = "INT3"; +#endif +#ifdef INT4_vect + prog_char gAvrInt_INT4[] PROGMEM = "INT4"; +#endif +#ifdef INT5_vect + prog_char gAvrInt_INT5[] PROGMEM = "INT5"; +#endif +#ifdef INT6_vect + prog_char gAvrInt_INT6[] PROGMEM = "INT6"; +#endif +#ifdef INT7_vect + prog_char gAvrInt_INT7[] PROGMEM = "INT7"; +#endif +#ifdef PCINT0_vect + prog_char gAvrInt_PCINT0[] PROGMEM = "PCINT0"; +#endif +#ifdef PCINT1_vect + prog_char gAvrInt_PCINT1[] PROGMEM = "PCINT1"; +#endif +#ifdef PCINT2_vect + prog_char gAvrInt_PCINT2[] PROGMEM = "PCINT2"; +#endif +#ifdef PCINT3_vect + prog_char gAvrInt_PCINT3[] PROGMEM = "PCINT3"; +#endif +#ifdef WDT_vect + prog_char gAvrInt_WDT[] PROGMEM = "WDT"; +#endif +#ifdef TIMER0_COMP_vect + prog_char gAvrInt_TIMER0_COMP[] PROGMEM = "TIMER0 COMP"; +#endif +#ifdef TIMER0_COMPA_vect + prog_char gAvrInt_TIMER0_COMPA[] PROGMEM = "TIMER0 COMPA"; +#endif +#ifdef TIMER0_COMPB_vect + prog_char gAvrInt_TIMER0_COMPB[] PROGMEM = "TIMER0 COMPB"; +#endif +#ifdef TIMER0_OVF_vect + prog_char gAvrInt_TIMER0_OVF[] PROGMEM = "TIMER0 OVF"; +#endif +#ifdef TIMER1_CAPT_vect + prog_char gAvrInt_TIMER1_CAPT[] PROGMEM = "TIMER1 CAPT"; +#endif +#ifdef TIMER1_COMPA_vect + prog_char gAvrInt_TIMER1_COMPA[] PROGMEM = "TIMER1 COMPA"; +#endif +#ifdef TIMER1_COMPB_vect + prog_char gAvrInt_TIMER1_COMPB[] PROGMEM = "TIMER1 COMPB"; +#endif +#ifdef TIMER1_COMPC_vect + prog_char gAvrInt_TIMER1_COMPC[] PROGMEM = "TIMER1 COMPC"; +#endif +#ifdef TIMER1_OVF_vect + prog_char gAvrInt_TIMER1_OVF[] PROGMEM = "TIMER1 OVF"; +#endif +#ifdef TIMER2_COMP_vect + prog_char gAvrInt_TIMER2_COMP[] PROGMEM = "TIMER2 COMP"; +#endif +#ifdef TIMER2_COMPA_vect + prog_char gAvrInt_TIMER2_COMPA[] PROGMEM = "TIMER2 COMPA"; +#endif +#ifdef TIMER2_COMPB_vect + prog_char gAvrInt_TIMER2_COMPB[] PROGMEM = "TIMER2 COMPB"; +#endif +#ifdef TIMER2_OVF_vect + prog_char gAvrInt_TIMER2_OVF[] PROGMEM = "TIMER2 OVF"; +#endif +#ifdef TIMER3_CAPT_vect + prog_char gAvrInt_TIMER3_CAPT[] PROGMEM = "TIMER3 CAPT"; +#endif +#ifdef TIMER3_COMPA_vect + prog_char gAvrInt_TIMER3_COMPA[] PROGMEM = "TIMER3 COMPA"; +#endif +#ifdef TIMER3_COMPB_vect + prog_char gAvrInt_TIMER3_COMPB[] PROGMEM = "TIMER3 COMPB"; +#endif +#ifdef TIMER3_COMPC_vect + prog_char gAvrInt_TIMER3_COMPC[] PROGMEM = "TIMER3 COMPC"; +#endif +#ifdef TIMER3_OVF_vect + prog_char gAvrInt_TIMER3_OVF[] PROGMEM = "TIMER3 OVF"; +#endif +#ifdef TIMER4_CAPT_vect + prog_char gAvrInt_TIMER4_CAPT[] PROGMEM = "TIMER4 CAPT"; +#endif +#ifdef TIMER4_COMPA_vect + prog_char gAvrInt_TIMER4_COMPA[] PROGMEM = "TIMER4 COMPA"; +#endif +#ifdef TIMER4_COMPB_vect + prog_char gAvrInt_TIMER4_COMPB[] PROGMEM = "TIMER4 COMPB"; +#endif +#ifdef TIMER4_COMPC_vect + prog_char gAvrInt_TIMER4_COMPC[] PROGMEM = "TIMER4 COMPC"; +#endif +#ifdef TIMER4_COMPD_vect + prog_char gAvrInt_TIMER4_COMPD[] PROGMEM = "TIMER4 COMPD"; +#endif +#ifdef TIMER4_OVF_vect + prog_char gAvrInt_TIMER4_OVF[] PROGMEM = "TIMER4 OVF"; +#endif +#ifdef TIMER4_FPF_vect + prog_char gAvrInt_TIMER4_FPF[] PROGMEM = "TIMER4 Fault Protection"; +#endif +#ifdef TIMER5_CAPT_vect + prog_char gAvrInt_TIMER5_CAPT[] PROGMEM = "TIMER5 CAPT"; +#endif +#ifdef TIMER5_COMPA_vect + prog_char gAvrInt_TIMER5_COMPA[] PROGMEM = "TIMER5 COMPA"; +#endif +#ifdef TIMER5_COMPB_vect + prog_char gAvrInt_TIMER5_COMPB[] PROGMEM = "TIMER5 COMPB"; +#endif +#ifdef TIMER5_COMPC_vect + prog_char gAvrInt_TIMER5_COMPC[] PROGMEM = "TIMER5 COMPC"; +#endif +#ifdef TIMER5_OVF_vect + prog_char gAvrInt_TIMER5_OVF[] PROGMEM = "TIMER5 OVF"; +#endif + +//* when there is only 1 usart +#if defined(USART_RX_vect) || defined(USART_RXC_vect) + prog_char gAvrInt_USART_RX[] PROGMEM = "USART RX"; +#endif +#if defined(USART_UDRE_vect) + prog_char gAvrInt_USART_UDRE[] PROGMEM = "USART UDRE"; +#endif +#if defined(USART_TX_vect) || defined(USART_TXC_vect) + prog_char gAvrInt_USART_TX[] PROGMEM = "USART TX"; +#endif + + +//* usart 0 +#if defined(USART0_RX_vect) + prog_char gAvrInt_USART0_RX[] PROGMEM = "USART0 RX"; +#endif +#if defined(USART0_UDRE_vect) + prog_char gAvrInt_USART0_UDRE[] PROGMEM = "USART0 UDRE"; +#endif +#if defined(USART0_TX_vect) + prog_char gAvrInt_USART0_TX[] PROGMEM = "USART0 TX"; +#endif + + +//* usart 1 +#ifdef USART1_RX_vect + prog_char gAvrInt_USART1_RX[] PROGMEM = "USART1 RX"; +#endif +#ifdef USART1_UDRE_vect + prog_char gAvrInt_USART1_UDRE[] PROGMEM = "USART1 UDRE"; +#endif +#ifdef USART1_TX_vect + prog_char gAvrInt_USART1_TX[] PROGMEM = "USART1 TX"; +#endif + +//* usart 2 +#ifdef USART2_RX_vect + prog_char gAvrInt_USART2_RX[] PROGMEM = "USART2 RX"; +#endif +#ifdef USART2_UDRE_vect + prog_char gAvrInt_USART2_UDRE[] PROGMEM = "USART2 UDRE"; +#endif +#ifdef USART2_TX_vect + prog_char gAvrInt_USART2_TX[] PROGMEM = "USART2 TX"; +#endif + +//* usart 3 +#ifdef USART3_RX_vect + prog_char gAvrInt_USART3_RX[] PROGMEM = "USART3 RX"; +#endif +#ifdef USART3_UDRE_vect + prog_char gAvrInt_USART3_UDRE[] PROGMEM = "USART3 UDRE"; +#endif +#ifdef USART3_TX_vect + prog_char gAvrInt_USART3_TX[] PROGMEM = "USART3 TX"; +#endif +#ifdef SPI_STC_vect + prog_char gAvrInt_SPI_STC[] PROGMEM = "SPI STC"; +#endif +#ifdef ADC_vect + prog_char gAvrInt_ADC[] PROGMEM = "ADC"; +#endif +#if defined(ANALOG_COMP_vect) || defined(ANA_COMP_vect) + prog_char gAvrInt_ANALOG_COMP[] PROGMEM = "ANALOG COMP"; +#endif +#if defined(EE_READY_vect) || defined(EE_RDY_vect) + prog_char gAvrInt_EE_READY[] PROGMEM = "EE READY"; +#endif +#ifdef TWI_vect + prog_char gAvrInt_TWI[] PROGMEM = "TWI"; +#endif +#if defined(SPM_READY_vect) || defined(SPM_RDY_vect) + prog_char gAvrInt_SPM_READY[] PROGMEM = "SPM READY"; +#endif +#ifdef USI_START_vect + prog_char gAvrInt_USI_START[] PROGMEM = "USI START"; +#endif +#ifdef USI_OVERFLOW_vect + prog_char gAvrInt_USI_OVERFLOW[] PROGMEM = "USI OVERFLOW"; +#endif +#ifdef USB_GEN_vect + prog_char gAvrInt_USB_General[] PROGMEM = "USB General"; +#endif +#ifdef USB_COM_vect + prog_char gAvrInt_USB_Endpoint[] PROGMEM = "USB Endpoint"; +#endif + +#ifdef LCD_vect + prog_char gAvrInt_LCD_StartFrame[] PROGMEM = "LCD Start of Frame"; +#endif + + +//************************************************************************************************** +//* these do not have vector defs and have to be done by CPU type +#if defined(__AVR_ATmega645__ ) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + prog_char gAvrInt_NOT_USED[] PROGMEM = "NOT_USED"; +#endif +#if defined(__AVR_ATmega32U4__) + prog_char gAvrInt_RESERVED[] PROGMEM = "Reserved"; +#endif + + prog_char gAvrInt_END[] PROGMEM = "*"; + + + + + +//************************************************************************************************** +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) +#pragma mark __AVR_ATmega168__ / __AVR_ATmega328P__ + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_PCINT0, // 4 + gAvrInt_PCINT1, // 5 + gAvrInt_PCINT2, // 6 + gAvrInt_WDT, // 7 + gAvrInt_TIMER2_COMPA, // 8 + gAvrInt_TIMER2_COMPB, // 9 + gAvrInt_TIMER2_OVF, // 10 + gAvrInt_TIMER1_CAPT, // 11 + gAvrInt_TIMER1_COMPA, // 12 + gAvrInt_TIMER1_COMPB, // 13 + gAvrInt_TIMER1_OVF, // 14 + gAvrInt_TIMER0_COMPA, // 15 + gAvrInt_TIMER0_COMPB, // 16 + gAvrInt_TIMER0_OVF, // 17 + gAvrInt_SPI_STC, // 18 + gAvrInt_USART_RX, // 19 + gAvrInt_USART_UDRE, // 20 + gAvrInt_USART_TX, // 21 + gAvrInt_ADC, // 22 + gAvrInt_EE_READY, // 23 + gAvrInt_ANALOG_COMP, // 24 + gAvrInt_TWI, // 25 + gAvrInt_SPM_READY, // 26 +}; + +#endif + +//************************************************************************************************** +#pragma mark __AVR_ATmega169__ +#if defined(__AVR_ATmega169__) + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_PCINT0, // 3 + gAvrInt_PCINT1, // 4 + gAvrInt_TIMER2_COMP, // 5 + gAvrInt_TIMER2_OVF, // 6 + gAvrInt_TIMER1_CAPT, // 7 + gAvrInt_TIMER1_COMPA, // 8 + gAvrInt_TIMER1_COMPB, // 9 + gAvrInt_TIMER1_OVF, // 10 + gAvrInt_TIMER0_COMP, // 11 + gAvrInt_TIMER0_OVF, // 12 + gAvrInt_SPI_STC, // 13 + gAvrInt_USART0_RX, // 14 + gAvrInt_USART0_UDRE, // 15 + gAvrInt_USART0_TX, // 16 + gAvrInt_USI_START, // 17 + gAvrInt_USI_OVERFLOW, // 18 + gAvrInt_ANALOG_COMP, // 19 + gAvrInt_ADC, // 20 + gAvrInt_EE_READY, // 21 + gAvrInt_SPM_READY, // 22 + gAvrInt_LCD_StartFrame, // 23 + +}; + +#endif + + +//************************************************************************************************** +#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) +#pragma mark __AVR_ATmega640__ __AVR_ATmega1280__ __AVR_ATmega1281__ __AVR_ATmega2560__ __AVR_ATmega2561__ + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_INT3, // 5 + gAvrInt_INT4, // 6 + gAvrInt_INT5, // 7 + gAvrInt_INT6, // 8 + gAvrInt_INT7, // 9 + gAvrInt_PCINT0, // 10 + gAvrInt_PCINT1, // 11 +#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + gAvrInt_PCINT2, // 12 +#else + gAvrInt_NOT_USED, // 12 +#endif + gAvrInt_WDT, // 13 + gAvrInt_TIMER2_COMPA, // 14 + gAvrInt_TIMER2_COMPB, // 15 + gAvrInt_TIMER2_OVF, // 16 + gAvrInt_TIMER1_CAPT, // 17 + gAvrInt_TIMER1_COMPA, // 18 + gAvrInt_TIMER1_COMPB, // 19 + gAvrInt_TIMER1_COMPC, // 20 + gAvrInt_TIMER1_OVF, // 21 + gAvrInt_TIMER0_COMPA, // 22 + gAvrInt_TIMER0_COMPB, // 23 + gAvrInt_TIMER0_OVF, // 24 + gAvrInt_SPI_STC, // 25 + + gAvrInt_USART0_RX, // 26 + gAvrInt_USART0_UDRE, // 27 + gAvrInt_USART0_TX, // 28 + gAvrInt_ANALOG_COMP, // 29 + gAvrInt_ADC, // 30 + gAvrInt_EE_READY, // 31 + + gAvrInt_TIMER3_CAPT, // 32 + gAvrInt_TIMER3_COMPA, // 33 + gAvrInt_TIMER3_COMPB, // 34 + gAvrInt_TIMER3_COMPC, // 35 + gAvrInt_TIMER3_OVF, // 36 + + gAvrInt_USART1_RX, // 37 + gAvrInt_USART1_UDRE, // 38 + gAvrInt_USART1_TX, // 39 + gAvrInt_TWI, // 40 + gAvrInt_SPM_READY, // 41 +#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + gAvrInt_TIMER4_CAPT, // 42 +#else + gAvrInt_NOT_USED, // 42 +#endif + gAvrInt_TIMER4_COMPA, // 43 + gAvrInt_TIMER4_COMPB, // 44 + gAvrInt_TIMER4_COMPC, // 45 + gAvrInt_TIMER4_OVF, // 46 +#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + gAvrInt_TIMER5_CAPT, // 47 +#else + gAvrInt_NOT_USED, // 47 +#endif + gAvrInt_TIMER5_COMPA, // 48 + gAvrInt_TIMER5_COMPB, // 49 + gAvrInt_TIMER5_COMPC, // 50 + gAvrInt_TIMER5_OVF, // 51 + +#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + gAvrInt_USART2_RX, // 52 + gAvrInt_USART2_UDRE, // 53 + gAvrInt_USART2_TX, // 54 + + gAvrInt_USART3_RX, // 55 + gAvrInt_USART3_UDRE, // 56 + gAvrInt_USART3_TX, // 57 +#endif + +}; + +#endif + + + +//************************************************************************************************** +#if defined(__AVR_ATmega324P__ ) || defined(__AVR_ATmega644__ ) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega1284P__) +#pragma mark __AVR_ATmega324P__ __AVR_ATmega644__ __AVR_ATmega644P__ __AVR_ATmega1284P__ + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_PCINT0, // 5 + gAvrInt_PCINT1, // 6 + gAvrInt_PCINT2, // 7 + gAvrInt_PCINT3, // 8 + gAvrInt_WDT, // 9 + gAvrInt_TIMER2_COMPA, // 10 + gAvrInt_TIMER2_COMPB, // 11 + gAvrInt_TIMER2_OVF, // 12 + gAvrInt_TIMER1_CAPT, // 13 + gAvrInt_TIMER1_COMPA, // 14 + gAvrInt_TIMER1_COMPB, // 15 + gAvrInt_TIMER1_OVF, // 16 + gAvrInt_TIMER0_COMPA, // 17 + gAvrInt_TIMER0_COMPB, // 18 + gAvrInt_TIMER0_OVF, // 19 + gAvrInt_SPI_STC, // 20 + gAvrInt_USART0_RX, // 21 + gAvrInt_USART0_UDRE, // 22 + gAvrInt_USART0_TX, // 23 + gAvrInt_ANALOG_COMP, // 24 + gAvrInt_ADC, // 25 + gAvrInt_EE_READY, // 26 + gAvrInt_TWI, // 27 + gAvrInt_SPM_READY, // 28 + +#if defined(__AVR_ATmega324P__ ) || defined(__AVR_ATmega644P__) + gAvrInt_USART1_RX, // 29 + gAvrInt_USART1_UDRE, // 30 + gAvrInt_USART1_TX, // 31 +#endif + +}; + + +#endif + +//************************************************************************************************** +#if defined(__AVR_ATmega645__ ) +#pragma mark __AVR_ATmega645__ + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_PCINT0, // 3 + gAvrInt_PCINT1, // 4 + gAvrInt_TIMER2_COMP, // 5 + gAvrInt_TIMER2_OVF, // 6 + gAvrInt_TIMER1_CAPT, // 7 + gAvrInt_TIMER1_COMPA, // 8 + gAvrInt_TIMER1_COMPB, // 9 + gAvrInt_TIMER1_OVF, // 10 + gAvrInt_TIMER0_COMP, // 11 + gAvrInt_TIMER0_OVF, // 12 + gAvrInt_SPI_STC, // 13 + gAvrInt_USART0_RX, // 14 + gAvrInt_USART0_UDRE, // 15 + gAvrInt_USART0_TX, // 16 + gAvrInt_USI_START, // 17 + gAvrInt_USI_OVERFLOW, // 18 + gAvrInt_ANALOG_COMP, // 19 + gAvrInt_ADC, // 20 + gAvrInt_EE_READY, // 21 + gAvrInt_SPM_READY, // 22 + gAvrInt_NOT_USED, // 23 + +#if defined(__AVR_ATmega3250__) || defined(__AVR_ATmega6450__) + gAvrInt_PCINT2, // 24 + gAvrInt_PCINT3, // 25 +#endif +}; + + +#endif + + +//************************************************************************************************** +#if defined(__AVR_ATmega32__ ) +#pragma mark __AVR_ATmega32__ + +#define _INTERRUPT_NAMES_DEFINED_ + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_TIMER2_COMP, // 5 + gAvrInt_TIMER2_OVF, // 6 + gAvrInt_TIMER1_CAPT, // 7 + gAvrInt_TIMER1_COMPA, // 8 + gAvrInt_TIMER1_COMPB, // 9 + gAvrInt_TIMER1_OVF, // 10 + gAvrInt_TIMER0_COMP, // 11 + gAvrInt_TIMER0_OVF, // 12 + gAvrInt_SPI_STC, // 13 + gAvrInt_USART_RX, // 14 + gAvrInt_USART_UDRE, // 15 + gAvrInt_USART_TX, // 16 + gAvrInt_ADC, // 17 + gAvrInt_EE_READY, // 18 + gAvrInt_ANALOG_COMP, // 19 + gAvrInt_TWI, // 20 + gAvrInt_SPM_READY, // 21 + +}; + + +#endif + +//************************************************************************************************** +#if defined(__AVR_ATmega32U4__) +#pragma mark __AVR_ATmega32U4__ +//* teensy 2.0 +//* http://www.pjrc.com/teensy/pinout.html +#define _INTERRUPT_NAMES_DEFINED_ + + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_INT3, // 5 + gAvrInt_RESERVED, // 6 + gAvrInt_RESERVED, // 7 + gAvrInt_INT6, // 8 + gAvrInt_RESERVED, // 9 + gAvrInt_PCINT0, // 10 + gAvrInt_USB_General, // 11 + gAvrInt_USB_Endpoint, // 12 + gAvrInt_WDT, // 13 + gAvrInt_RESERVED, // 14 + gAvrInt_RESERVED, // 15 + gAvrInt_RESERVED, // 16 + gAvrInt_TIMER1_CAPT, // 17 + gAvrInt_TIMER1_COMPA, // 18 + gAvrInt_TIMER1_COMPB, // 19 + gAvrInt_TIMER1_COMPC, // 20 + gAvrInt_TIMER1_OVF, // 21 + gAvrInt_TIMER0_COMPA, // 22 + gAvrInt_TIMER0_COMPB, // 23 + gAvrInt_TIMER0_OVF, // 24 + gAvrInt_SPI_STC, // 25 + + gAvrInt_USART1_RX, // 26 + gAvrInt_USART1_UDRE, // 27 + gAvrInt_USART1_TX, // 28 + gAvrInt_ANALOG_COMP, // 29 + + gAvrInt_ADC, // 30 + gAvrInt_EE_READY, // 31 + + gAvrInt_TIMER3_CAPT, // 32 + gAvrInt_TIMER3_COMPA, // 33 + gAvrInt_TIMER3_COMPB, // 34 + gAvrInt_TIMER3_COMPC, // 35 + gAvrInt_TIMER3_OVF, // 36 + gAvrInt_TWI, // 37 + gAvrInt_SPM_READY, // 38 + + gAvrInt_TIMER4_COMPA, // 39 + gAvrInt_TIMER4_COMPB, // 40 + gAvrInt_TIMER4_COMPD, // 41 + gAvrInt_TIMER4_OVF, // 42 + gAvrInt_TIMER4_FPF, // 43 +}; + +#endif + +//************************************************************************************************** +#if defined(__AVR_AT90USB1286__) +#pragma mark __AVR_AT90USB1286__ +//* teensy++ 2.0 +//* http://www.pjrc.com/teensy/pinout.html +#define _INTERRUPT_NAMES_DEFINED_ + + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_INT3, // 5 + gAvrInt_INT4, // 6 + gAvrInt_INT5, // 7 + gAvrInt_INT6, // 8 + gAvrInt_INT7, // 9 + gAvrInt_PCINT0, // 10 + gAvrInt_USB_General, // 11 + gAvrInt_USB_Endpoint, // 12 + gAvrInt_WDT, // 13 + gAvrInt_TIMER2_COMPA, // 14 + gAvrInt_TIMER2_COMPB, // 15 + gAvrInt_TIMER2_OVF, // 16 + gAvrInt_TIMER1_CAPT, // 17 + gAvrInt_TIMER1_COMPA, // 18 + gAvrInt_TIMER1_COMPB, // 19 + gAvrInt_TIMER1_COMPC, // 20 + gAvrInt_TIMER1_OVF, // 21 + gAvrInt_TIMER0_COMPA, // 22 + gAvrInt_TIMER0_COMPB, // 23 + gAvrInt_TIMER0_OVF, // 24 + gAvrInt_SPI_STC, // 25 + + gAvrInt_USART1_RX, // 26 + gAvrInt_USART1_UDRE, // 27 + gAvrInt_USART1_TX, // 28 + gAvrInt_ANALOG_COMP, // 29 + + gAvrInt_ADC, // 30 + gAvrInt_EE_READY, // 31 + + gAvrInt_TIMER3_CAPT, // 32 + gAvrInt_TIMER3_COMPA, // 33 + gAvrInt_TIMER3_COMPB, // 34 + gAvrInt_TIMER3_COMPC, // 35 + gAvrInt_TIMER3_OVF, // 36 + gAvrInt_TWI, // 37 + gAvrInt_SPM_READY, // 38 + +}; + +#endif + + + + +//************************************************************************************************** +#if defined(__AVR_ATmega128__) +#pragma mark __AVR_ATmega128__ +#define _INTERRUPT_NAMES_DEFINED_ + + +PGM_P gInterruptNameTable[] PROGMEM = +{ + + gAvrInt_RESET, // 1 + gAvrInt_INT0, // 2 + gAvrInt_INT1, // 3 + gAvrInt_INT2, // 4 + gAvrInt_INT3, // 5 + gAvrInt_INT4, // 6 + gAvrInt_INT5, // 7 + gAvrInt_INT6, // 8 + gAvrInt_INT7, // 9 + gAvrInt_TIMER2_COMP, // 10 + gAvrInt_TIMER2_OVF, // 11 + gAvrInt_TIMER1_CAPT, // 12 + gAvrInt_TIMER1_COMPA, // 13 + gAvrInt_TIMER1_COMPB, // 14 + gAvrInt_TIMER1_OVF, // 15 + gAvrInt_TIMER0_COMP, // 16 + gAvrInt_TIMER0_OVF, // 17 + gAvrInt_SPI_STC, // 18 + gAvrInt_USART0_RX, // 19 + gAvrInt_USART0_UDRE, // 20 + gAvrInt_USART0_TX, // 21 + gAvrInt_ADC, // 22 + gAvrInt_EE_READY, // 23 + gAvrInt_ANALOG_COMP, // 24 + gAvrInt_TIMER1_COMPC, // 25 + gAvrInt_TIMER3_CAPT, // 26 + gAvrInt_TIMER3_COMPA, // 27 + gAvrInt_TIMER3_COMPB, // 28 + gAvrInt_TIMER3_COMPC, // 29 + gAvrInt_TIMER3_OVF, // 30 + gAvrInt_USART1_RX, // 31 + gAvrInt_USART1_UDRE, // 32 + gAvrInt_USART1_TX, // 33 + gAvrInt_TWI, // 34 + gAvrInt_SPM_READY, // 35 + +}; + +#endif + +#if !defined(_INTERRUPT_NAMES_DEFINED_) + #warning No interrupt string defs for this cpu +#endif \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/command.h b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/command.h new file mode 100644 index 0000000..03b1b38 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/command.h @@ -0,0 +1,114 @@ +//**** ATMEL AVR - A P P L I C A T I O N N O T E ************************ +//* +//* Title: AVR068 - STK500 Communication Protocol +//* Filename: command.h +//* Version: 1.0 +//* Last updated: 31.01.2005 +//* +//* Support E-mail: avr@atmel.com +//* +//************************************************************************** + +// *****************[ STK message constants ]*************************** + +#define MESSAGE_START 0x1B //= ESC = 27 decimal +#define TOKEN 0x0E + +// *****************[ STK general command constants ]************************** + +#define CMD_SIGN_ON 0x01 +#define CMD_SET_PARAMETER 0x02 +#define CMD_GET_PARAMETER 0x03 +#define CMD_SET_DEVICE_PARAMETERS 0x04 +#define CMD_OSCCAL 0x05 +#define CMD_LOAD_ADDRESS 0x06 +#define CMD_FIRMWARE_UPGRADE 0x07 + + +// *****************[ STK ISP command constants ]****************************** + +#define CMD_ENTER_PROGMODE_ISP 0x10 +#define CMD_LEAVE_PROGMODE_ISP 0x11 +#define CMD_CHIP_ERASE_ISP 0x12 +#define CMD_PROGRAM_FLASH_ISP 0x13 +#define CMD_READ_FLASH_ISP 0x14 +#define CMD_PROGRAM_EEPROM_ISP 0x15 +#define CMD_READ_EEPROM_ISP 0x16 +#define CMD_PROGRAM_FUSE_ISP 0x17 +#define CMD_READ_FUSE_ISP 0x18 +#define CMD_PROGRAM_LOCK_ISP 0x19 +#define CMD_READ_LOCK_ISP 0x1A +#define CMD_READ_SIGNATURE_ISP 0x1B +#define CMD_READ_OSCCAL_ISP 0x1C +#define CMD_SPI_MULTI 0x1D + +// *****************[ STK PP command constants ]******************************* + +#define CMD_ENTER_PROGMODE_PP 0x20 +#define CMD_LEAVE_PROGMODE_PP 0x21 +#define CMD_CHIP_ERASE_PP 0x22 +#define CMD_PROGRAM_FLASH_PP 0x23 +#define CMD_READ_FLASH_PP 0x24 +#define CMD_PROGRAM_EEPROM_PP 0x25 +#define CMD_READ_EEPROM_PP 0x26 +#define CMD_PROGRAM_FUSE_PP 0x27 +#define CMD_READ_FUSE_PP 0x28 +#define CMD_PROGRAM_LOCK_PP 0x29 +#define CMD_READ_LOCK_PP 0x2A +#define CMD_READ_SIGNATURE_PP 0x2B +#define CMD_READ_OSCCAL_PP 0x2C + +#define CMD_SET_CONTROL_STACK 0x2D + +// *****************[ STK HVSP command constants ]***************************** + +#define CMD_ENTER_PROGMODE_HVSP 0x30 +#define CMD_LEAVE_PROGMODE_HVSP 0x31 +#define CMD_CHIP_ERASE_HVSP 0x32 +#define CMD_PROGRAM_FLASH_HVSP ` 0x33 +#define CMD_READ_FLASH_HVSP 0x34 +#define CMD_PROGRAM_EEPROM_HVSP 0x35 +#define CMD_READ_EEPROM_HVSP 0x36 +#define CMD_PROGRAM_FUSE_HVSP 0x37 +#define CMD_READ_FUSE_HVSP 0x38 +#define CMD_PROGRAM_LOCK_HVSP 0x39 +#define CMD_READ_LOCK_HVSP 0x3A +#define CMD_READ_SIGNATURE_HVSP 0x3B +#define CMD_READ_OSCCAL_HVSP 0x3C + +// *****************[ STK status constants ]*************************** + +// Success +#define STATUS_CMD_OK 0x00 + +// Warnings +#define STATUS_CMD_TOUT 0x80 +#define STATUS_RDY_BSY_TOUT 0x81 +#define STATUS_SET_PARAM_MISSING 0x82 + +// Errors +#define STATUS_CMD_FAILED 0xC0 +#define STATUS_CKSUM_ERROR 0xC1 +#define STATUS_CMD_UNKNOWN 0xC9 + +// *****************[ STK parameter constants ]*************************** +#define PARAM_BUILD_NUMBER_LOW 0x80 +#define PARAM_BUILD_NUMBER_HIGH 0x81 +#define PARAM_HW_VER 0x90 +#define PARAM_SW_MAJOR 0x91 +#define PARAM_SW_MINOR 0x92 +#define PARAM_VTARGET 0x94 +#define PARAM_VADJUST 0x95 +#define PARAM_OSC_PSCALE 0x96 +#define PARAM_OSC_CMATCH 0x97 +#define PARAM_SCK_DURATION 0x98 +#define PARAM_TOPCARD_DETECT 0x9A +#define PARAM_STATUS 0x9C +#define PARAM_DATA 0x9D +#define PARAM_RESET_POLARITY 0x9E +#define PARAM_CONTROLLER_INIT 0x9F + +// *****************[ STK answer constants ]*************************** + +#define ANSWER_CKSUM_ERROR 0xB0 + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.c b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.c new file mode 100644 index 0000000..13dec89 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.c @@ -0,0 +1,1996 @@ +/***************************************************************************** +Title: STK500v2 compatible bootloader + Modified for Wiring board ATMega128-16MHz +Author: Peter Fleury http://jump.to/fleury +File: $Id: stk500boot.c,v 1.11 2006/06/25 12:39:17 peter Exp $ +Compiler: avr-gcc 3.4.5 or 4.1 / avr-libc 1.4.3 +Hardware: All AVRs with bootloader support, tested with ATmega8 +License: GNU General Public License + +Modified: Worapoht Kornkaewwattanakul http://www.avride.com +Date: 17 October 2007 +Update: 1st, 29 Dec 2007 : Enable CMD_SPI_MULTI but ignore unused command by return 0x00 byte response.. +Compiler: WINAVR20060421 +Description: add timeout feature like previous Wiring bootloader + +DESCRIPTION: + This program allows an AVR with bootloader capabilities to + read/write its own Flash/EEprom. To enter Programming mode + an input pin is checked. If this pin is pulled low, programming mode + is entered. If not, normal execution is done from $0000 + "reset" vector in Application area. + Size fits into a 1024 word bootloader section + when compiled with avr-gcc 4.1 + (direct replace on Wiring Board without fuse setting changed) + +USAGE: + - Set AVR MCU type and clock-frequency (F_CPU) in the Makefile. + - Set baud rate below (AVRISP only works with 115200 bps) + - compile/link the bootloader with the supplied Makefile + - program the "Boot Flash section size" (BOOTSZ fuses), + for boot-size 1024 words: program BOOTSZ01 + - enable the BOOT Reset Vector (program BOOTRST) + - Upload the hex file to the AVR using any ISP programmer + - Program Boot Lock Mode 3 (program BootLock 11 and BootLock 12 lock bits) // (leave them) + - Reset your AVR while keeping PROG_PIN pulled low // (for enter bootloader by switch) + - Start AVRISP Programmer (AVRStudio/Tools/Program AVR) + - AVRISP will detect the bootloader + - Program your application FLASH file and optional EEPROM file using AVRISP + +Note: + Erasing the device without flashing, through AVRISP GUI button "Erase Device" + is not implemented, due to AVRStudio limitations. + Flash is always erased before programming. + + AVRdude: + Please uncomment #define REMOVE_CMD_SPI_MULTI when using AVRdude. + Comment #define REMOVE_PROGRAM_LOCK_BIT_SUPPORT to reduce code size + Read Fuse Bits and Read/Write Lock Bits is not supported + +NOTES: + Based on Atmel Application Note AVR109 - Self-programming + Based on Atmel Application Note AVR068 - STK500v2 Protocol + +LICENSE: + Copyright (C) 2006 Peter Fleury + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + +*****************************************************************************/ + +//************************************************************************ +//* Edit History +//************************************************************************ +//* Jul 7, 2010 = Mark Sproul msproul@skycharoit.com +//* Jul 7, 2010 Working on mega2560. No Auto-restart +//* Jul 7, 2010 Switched to 8K bytes (4K words) so that we have room for the monitor +//* Jul 8, 2010 Found older version of source that had auto restart, put that code back in +//* Jul 8, 2010 Adding monitor code +//* Jul 11, 2010 Added blinking LED while waiting for download to start +//* Jul 11, 2010 Added EEPROM test +//* Jul 29, 2010 Added recchar_timeout for timing out on bootloading +//* Aug 23, 2010 Added support for atmega2561 +//* Aug 26, 2010 Removed support for BOOT_BY_SWITCH +//************************************************************************ + + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "command.h" + + +#if defined(_MEGA_BOARD_) || defined(_BOARD_AMBER128_) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) + #define ENABLE_MONITOR + static void RunMonitor(void); +#endif + +//#define _DEBUG_SERIAL_ +//#define _DEBUG_WITH_LEDS_ + + +/* + * Uncomment the following lines to save code space + */ +//#define REMOVE_PROGRAM_LOCK_BIT_SUPPORT // disable program lock bits +//#define REMOVE_BOOTLOADER_LED // no LED to show active bootloader +//#define REMOVE_CMD_SPI_MULTI // disable processing of SPI_MULTI commands, Remark this line for AVRDUDE +// + + + +//************************************************************************ +//* LED on pin "PROGLED_PIN" on port "PROGLED_PORT" +//* indicates that bootloader is active +//* PG2 -> LED on Wiring board +//************************************************************************ +#define BLINK_LED_WHILE_WAITING + +#ifdef _MEGA_BOARD_ + #define PROGLED_PORT PORTB + #define PROGLED_DDR DDRB + #define PROGLED_PIN PINB7 +#elif defined( _BOARD_AMBER128_ ) + //* this is for the amber 128 http://www.soc-robotics.com/ + //* onbarod led is PORTE4 + #define PROGLED_PORT PORTD + #define PROGLED_DDR DDRD + #define PROGLED_PIN PINE7 +#elif defined( _CEREBOTPLUS_BOARD_ ) + //* this is for the Cerebot 2560 board + //* onbarod leds are on PORTE4-7 + #define PROGLED_PORT PORTE + #define PROGLED_DDR DDRE + #define PROGLED_PIN PINE7 +#elif defined( _PENGUINO_ ) + //* this is for the Penguino + //* onbarod led is PORTE4 + #define PROGLED_PORT PORTC + #define PROGLED_DDR DDRC + #define PROGLED_PIN PINC6 +#elif defined( _ANDROID_2561_ ) || defined( __AVR_ATmega2561__ ) + //* this is for the Boston Android 2561 + //* onbarod led is PORTE4 + #define PROGLED_PORT PORTA + #define PROGLED_DDR DDRA + #define PROGLED_PIN PINA3 +#else + #define PROGLED_PORT PORTG + #define PROGLED_DDR DDRG + #define PROGLED_PIN PING2 +#endif + + + +/* + * define CPU frequency in Mhz here if not defined in Makefile + */ +#ifndef F_CPU + #define F_CPU 16000000UL +#endif + +/* + * UART Baudrate, AVRStudio AVRISP only accepts 115200 bps + */ + +#ifndef BAUDRATE + #define BAUDRATE 115200 +#endif + +/* + * Enable (1) or disable (0) USART double speed operation + */ +#ifndef UART_BAUDRATE_DOUBLE_SPEED + #if defined (__AVR_ATmega32__) + #define UART_BAUDRATE_DOUBLE_SPEED 0 + #else + #define UART_BAUDRATE_DOUBLE_SPEED 1 + #endif +#endif + +/* + * HW and SW version, reported to AVRISP, must match version of AVRStudio + */ +#define CONFIG_PARAM_BUILD_NUMBER_LOW 0 +#define CONFIG_PARAM_BUILD_NUMBER_HIGH 0 +#define CONFIG_PARAM_HW_VER 0x0F +#define CONFIG_PARAM_SW_MAJOR 2 +#define CONFIG_PARAM_SW_MINOR 0x0A + +/* + * Calculate the address where the bootloader starts from FLASHEND and BOOTSIZE + * (adjust BOOTSIZE below and BOOTLOADER_ADDRESS in Makefile if you want to change the size of the bootloader) + */ +//#define BOOTSIZE 1024 +#if FLASHEND > 0x0F000 + #define BOOTSIZE 8192 +#else + #define BOOTSIZE 2048 +#endif + +#define APP_END (FLASHEND -(2*BOOTSIZE) + 1) + +/* + * Signature bytes are not available in avr-gcc io_xxx.h + */ +#if defined (__AVR_ATmega8__) + #define SIGNATURE_BYTES 0x1E9307 +#elif defined (__AVR_ATmega16__) + #define SIGNATURE_BYTES 0x1E9403 +#elif defined (__AVR_ATmega32__) + #define SIGNATURE_BYTES 0x1E9502 +#elif defined (__AVR_ATmega8515__) + #define SIGNATURE_BYTES 0x1E9306 +#elif defined (__AVR_ATmega8535__) + #define SIGNATURE_BYTES 0x1E9308 +#elif defined (__AVR_ATmega162__) + #define SIGNATURE_BYTES 0x1E9404 +#elif defined (__AVR_ATmega128__) + #define SIGNATURE_BYTES 0x1E9702 +#elif defined (__AVR_ATmega1280__) + #define SIGNATURE_BYTES 0x1E9703 +#elif defined (__AVR_ATmega2560__) + #define SIGNATURE_BYTES 0x1E9801 +#elif defined (__AVR_ATmega2561__) + #define SIGNATURE_BYTES 0x1e9802 +#else + #error "no signature definition for MCU available" +#endif + + +#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) \ + || defined(__AVR_ATmega8515__) || defined(__AVR_ATmega8535__) + /* ATMega8 with one USART */ + #define UART_BAUD_RATE_LOW UBRRL + #define UART_STATUS_REG UCSRA + #define UART_CONTROL_REG UCSRB + #define UART_ENABLE_TRANSMITTER TXEN + #define UART_ENABLE_RECEIVER RXEN + #define UART_TRANSMIT_COMPLETE TXC + #define UART_RECEIVE_COMPLETE RXC + #define UART_DATA_REG UDR + #define UART_DOUBLE_SPEED U2X + +#elif defined(__AVR_ATmega64__) || defined(__AVR_ATmega128__) || defined(__AVR_ATmega162__) \ + || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) + /* ATMega with two USART, use UART0 */ + #define UART_BAUD_RATE_LOW UBRR0L + #define UART_STATUS_REG UCSR0A + #define UART_CONTROL_REG UCSR0B + #define UART_ENABLE_TRANSMITTER TXEN0 + #define UART_ENABLE_RECEIVER RXEN0 + #define UART_TRANSMIT_COMPLETE TXC0 + #define UART_RECEIVE_COMPLETE RXC0 + #define UART_DATA_REG UDR0 + #define UART_DOUBLE_SPEED U2X0 +#else + #error "no UART definition for MCU available" +#endif + + + +/* + * Macro to calculate UBBR from XTAL and baudrate + */ +#if defined(__AVR_ATmega32__) && UART_BAUDRATE_DOUBLE_SPEED + #define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu / 4 / baudRate - 1) / 2) +#elif defined(__AVR_ATmega32__) + #define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu / 8 / baudRate - 1) / 2) +#elif UART_BAUDRATE_DOUBLE_SPEED + #define UART_BAUD_SELECT(baudRate,xtalCpu) (((float)(xtalCpu))/(((float)(baudRate))*8.0)-1.0+0.5) +#else + #define UART_BAUD_SELECT(baudRate,xtalCpu) (((float)(xtalCpu))/(((float)(baudRate))*16.0)-1.0+0.5) +#endif + + +/* + * States used in the receive state machine + */ +#define ST_START 0 +#define ST_GET_SEQ_NUM 1 +#define ST_MSG_SIZE_1 2 +#define ST_MSG_SIZE_2 3 +#define ST_GET_TOKEN 4 +#define ST_GET_DATA 5 +#define ST_GET_CHECK 6 +#define ST_PROCESS 7 + +/* + * use 16bit address variable for ATmegas with <= 64K flash + */ +#if defined(RAMPZ) + typedef uint32_t address_t; +#else + typedef uint16_t address_t; +#endif + +/* + * function prototypes + */ +static void sendchar(char c); +static unsigned char recchar(void); + +/* + * since this bootloader is not linked against the avr-gcc crt1 functions, + * to reduce the code size, we need to provide our own initialization + */ +void __jumpMain (void) __attribute__ ((naked)) __attribute__ ((section (".init9"))); +#include + +//#define SPH_REG 0x3E +//#define SPL_REG 0x3D + +//***************************************************************************** +void __jumpMain(void) +{ +//* July 17, 2010 Added stack pointer initialzation +//* the first line did not do the job on the ATmega128 + + asm volatile ( ".set __stack, %0" :: "i" (RAMEND) ); + +// ldi r16,high(RAMEND) +// out SPH,r16 ; Set stack pointer to top of RAM + +// asm volatile ( "ldi 16, 0x10"); + asm volatile ( "ldi 16, %0" :: "i" (RAMEND >> 8) ); +// asm volatile ( "out 0x3E,16"); +// asm volatile ( "out %0,16" :: "i" (SPH_REG) ); + asm volatile ( "out %0,16" :: "i" (AVR_STACK_POINTER_HI_ADDR) ); + +// asm volatile ( "ldi 16, 0x00"); + asm volatile ( "ldi 16, %0" :: "i" (RAMEND & 0x0ff) ); +// asm volatile ( "out 0x3d,16"); +// asm volatile ( "out %0,16" :: "i" (SPL_REG) ); + asm volatile ( "out %0,16" :: "i" (AVR_STACK_POINTER_LO_ADDR) ); + + + + asm volatile ( "clr __zero_reg__" ); // GCC depends on register r1 set to 0 + asm volatile ( "out %0, __zero_reg__" :: "I" (_SFR_IO_ADDR(SREG)) ); // set SREG to 0 +// asm volatile ( "rjmp main"); // jump to main() + asm volatile ( "jmp main"); // jump to main() +} + + +//***************************************************************************** +void delay_ms(unsigned int timedelay) +{ + unsigned int i; + for (i=0;i> 1) +//***************************************************************************** +static unsigned char recchar_timeout(void) +{ +uint32_t count = 0; + + while (!(UART_STATUS_REG & (1 << UART_RECEIVE_COMPLETE))) + { + // wait for data + count++; + if (count > MAX_TIME_COUNT) + { + unsigned int data; + #if (FLASHEND > 0x0FFFF) + data = pgm_read_word_far(0); //* get the first word of the user program + #else + data = pgm_read_word_near(0); //* get the first word of the user program + #endif + if (data != 0xffff) //* make sure its valid before jumping to it. + { + asm volatile( + "clr r30 \n\t" + "clr r31 \n\t" + "ijmp \n\t" + ); + } + count = 0; + } + } + return UART_DATA_REG; +} + + + +//***************************************************************************** +int main(void) +{ + address_t address = 0; + address_t eraseAddress = 0; + unsigned char msgParseState; + unsigned int ii = 0; + unsigned char checksum = 0; + unsigned char seqNum = 0; + unsigned int msgLength = 0; + unsigned char msgBuffer[285]; + unsigned char c, *p; + unsigned char isLeave = 0; + + unsigned long boot_timeout; + unsigned long boot_timer; + unsigned int boot_state; +#ifdef ENABLE_MONITOR + unsigned int exPointCntr = 0; +#endif + + + boot_timer = 0; + boot_state = 0; + +#ifdef BLINK_LED_WHILE_WAITING + boot_timeout = 20000; //* should be about 1 second +// boot_timeout = 170000; +#else + boot_timeout = 3500000; // 7 seconds , approx 2us per step when optimize "s" +#endif + /* + * Branch to bootloader or application code ? + */ + +#ifndef REMOVE_BOOTLOADER_LED + /* PROG_PIN pulled low, indicate with LED that bootloader is active */ + PROGLED_DDR |= (1< boot_timeout) + { + boot_state = 1; // (after ++ -> boot_state=2 bootloader timeout, jump to main 0x00000 ) + } + #ifdef BLINK_LED_WHILE_WAITING + if ((boot_timer % 7000) == 0) + { + //* toggle the LED + PROGLED_PORT ^= (1<>16) & 0x000000FF; + else if ( signatureIndex == 1 ) + answerByte = (SIGNATURE_BYTES >> 8) & 0x000000FF; + else + answerByte = SIGNATURE_BYTES & 0x000000FF; + } + else if ( msgBuffer[4] & 0x50 ) + { + answerByte = 0; //read fuse/lock bits not implemented, return dummy value + } + else + { + answerByte = 0; // for all others command are not implemented, return dummy value for AVRDUDE happy + // flag = 1; // Remark this line for AVRDUDE + } + if ( !flag ) + { + msgLength = 7; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = 0; + msgBuffer[3] = msgBuffer[4]; + msgBuffer[4] = 0; + msgBuffer[5] = answerByte; + msgBuffer[6] = STATUS_CMD_OK; + } + } + break; + #endif + case CMD_SIGN_ON: + msgLength = 11; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = 8; + msgBuffer[3] = 'A'; + msgBuffer[4] = 'V'; + msgBuffer[5] = 'R'; + msgBuffer[6] = 'I'; + msgBuffer[7] = 'S'; + msgBuffer[8] = 'P'; + msgBuffer[9] = '_'; + msgBuffer[10] = '2'; + break; + + case CMD_GET_PARAMETER: + { + unsigned char value; + + switch(msgBuffer[1]) + { + case PARAM_BUILD_NUMBER_LOW: + value = CONFIG_PARAM_BUILD_NUMBER_LOW; + break; + case PARAM_BUILD_NUMBER_HIGH: + value = CONFIG_PARAM_BUILD_NUMBER_HIGH; + break; + case PARAM_HW_VER: + value = CONFIG_PARAM_HW_VER; + break; + case PARAM_SW_MAJOR: + value = CONFIG_PARAM_SW_MAJOR; + break; + case PARAM_SW_MINOR: + value = CONFIG_PARAM_SW_MINOR; + break; + default: + value = 0; + break; + } + msgLength = 3; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = value; + } + break; + + case CMD_LEAVE_PROGMODE_ISP: + isLeave = 1; + //* fall thru + + case CMD_SET_PARAMETER: + case CMD_ENTER_PROGMODE_ISP: + msgLength = 2; + msgBuffer[1] = STATUS_CMD_OK; + break; + + case CMD_READ_SIGNATURE_ISP: + { + unsigned char signatureIndex = msgBuffer[4]; + unsigned char signature; + + if ( signatureIndex == 0 ) + signature = (SIGNATURE_BYTES >>16) & 0x000000FF; + else if ( signatureIndex == 1 ) + signature = (SIGNATURE_BYTES >> 8) & 0x000000FF; + else + signature = SIGNATURE_BYTES & 0x000000FF; + + msgLength = 4; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = signature; + msgBuffer[3] = STATUS_CMD_OK; + } + break; + + case CMD_READ_LOCK_ISP: + msgLength = 4; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = boot_lock_fuse_bits_get( GET_LOCK_BITS ); + msgBuffer[3] = STATUS_CMD_OK; + break; + + case CMD_READ_FUSE_ISP: + { + unsigned char fuseBits; + + if ( msgBuffer[2] == 0x50 ) + { + if ( msgBuffer[3] == 0x08 ) + fuseBits = boot_lock_fuse_bits_get( GET_EXTENDED_FUSE_BITS ); + else + fuseBits = boot_lock_fuse_bits_get( GET_LOW_FUSE_BITS ); + } + else + { + fuseBits = boot_lock_fuse_bits_get( GET_HIGH_FUSE_BITS ); + } + msgLength = 4; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = fuseBits; + msgBuffer[3] = STATUS_CMD_OK; + } + break; + + #ifndef REMOVE_PROGRAM_LOCK_BIT_SUPPORT + case CMD_PROGRAM_LOCK_ISP: + { + unsigned char lockBits = msgBuffer[4]; + + lockBits = (~lockBits) & 0x3C; // mask BLBxx bits + boot_lock_bits_set(lockBits); // and program it + boot_spm_busy_wait(); + + msgLength = 3; + msgBuffer[1] = STATUS_CMD_OK; + msgBuffer[2] = STATUS_CMD_OK; + } + break; + #endif + case CMD_CHIP_ERASE_ISP: + eraseAddress = 0; + msgLength = 2; + msgBuffer[1] = STATUS_CMD_OK; + break; + + case CMD_LOAD_ADDRESS: + #if defined(RAMPZ) + address = ( ((address_t)(msgBuffer[1])<<24)|((address_t)(msgBuffer[2])<<16)|((address_t)(msgBuffer[3])<<8)|(msgBuffer[4]) )<<1; + #else + address = ( ((msgBuffer[3])<<8)|(msgBuffer[4]) )<<1; //convert word to byte address + #endif + msgLength = 2; + msgBuffer[1] = STATUS_CMD_OK; + break; + + case CMD_PROGRAM_FLASH_ISP: + case CMD_PROGRAM_EEPROM_ISP: + { + unsigned int size = ((msgBuffer[1])<<8) | msgBuffer[2]; + unsigned char *p = msgBuffer+10; + unsigned int data; + unsigned char highByte, lowByte; + address_t tempaddress = address; + + + if ( msgBuffer[0] == CMD_PROGRAM_FLASH_ISP ) + { + // erase only main section (bootloader protection) + if (eraseAddress < APP_END ) + { + boot_page_erase(eraseAddress); // Perform page erase + boot_spm_busy_wait(); // Wait until the memory is erased. + eraseAddress += SPM_PAGESIZE; // point to next page to be erase + } + + /* Write FLASH */ + do { + lowByte = *p++; + highByte = *p++; + + data = (highByte << 8) | lowByte; + boot_page_fill(address,data); + + address = address + 2; // Select next word in memory + size -= 2; // Reduce number of bytes to write by two + } while (size); // Loop until all bytes written + + boot_page_write(tempaddress); + boot_spm_busy_wait(); + boot_rww_enable(); // Re-enable the RWW section + } + else + { + #if (!defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) && !defined(__AVR_ATmega2561__)) + /* write EEPROM */ + do { + EEARL = address; // Setup EEPROM address + EEARH = (address >> 8); + address++; // Select next EEPROM byte + + EEDR = *p++; // get byte from buffer + EECR |= (1<> 8); //MSB + address += 2; // Select next word in memory + size -= 2; + }while (size); + } + else + { + /* Read EEPROM */ + do { + EEARL = address; // Setup EEPROM address + EEARH = ((address >> 8)); + address++; // Select next EEPROM byte + EECR |= (1<>8)&0xFF); + sendchar(c); + checksum ^= c; + + c = msgLength&0x00FF; + sendchar(c); + checksum ^= c; + + sendchar(TOKEN); + checksum ^= TOKEN; + + p = msgBuffer; + while ( msgLength ) + { + c = *p++; + sendchar(c); + checksum ^=c; + msgLength--; + } + sendchar(checksum); + seqNum++; + + #ifndef REMOVE_BOOTLOADER_LED + //* toggle the LED + PROGLED_PORT ^= (1< + + +base address = f000 +avrdude: Device signature = 0x1e9703 +avrdude: safemode: lfuse reads as FF +avrdude: safemode: hfuse reads as D8 +avrdude: safemode: efuse reads as F5 +avrdude> +*/ + +//************************************************************************ +#ifdef ENABLE_MONITOR +#include + +unsigned long gRamIndex; +unsigned long gFlashIndex; +unsigned long gEepromIndex; + + +#define true 1 +#define false 0 + +#if defined(__AVR_ATmega128__) + #define kCPU_NAME "ATmega128" +#elif defined(__AVR_ATmega1280__) + #define kCPU_NAME "ATmega1280" +#elif defined(__AVR_ATmega1281__) + #define kCPU_NAME "ATmega1281" +#elif defined(__AVR_ATmega2560__) + #define kCPU_NAME "ATmega2560" +#elif defined(__AVR_ATmega2561__) + #define kCPU_NAME "ATmega2561" +#endif + +#ifdef _VECTORS_SIZE + #define kInterruptVectorCount (_VECTORS_SIZE / 4) +#else + #define kInterruptVectorCount 23 +#endif + + +void PrintDecInt(int theNumber, int digitCnt); + +#ifdef kCPU_NAME + prog_char gTextMsg_CPU_Name[] PROGMEM = kCPU_NAME; +#else + prog_char gTextMsg_CPU_Name[] PROGMEM = "UNKNOWN"; +#endif + + prog_char gTextMsg_Explorer[] PROGMEM = "Arduino explorer stk500V2 by MLS"; + prog_char gTextMsg_Prompt[] PROGMEM = "Bootloader>"; + prog_char gTextMsg_HUH[] PROGMEM = "Huh?"; + prog_char gTextMsg_COMPILED_ON[] PROGMEM = "Compiled on = "; + prog_char gTextMsg_CPU_Type[] PROGMEM = "CPU Type = "; + prog_char gTextMsg_AVR_ARCH[] PROGMEM = "__AVR_ARCH__ = "; + prog_char gTextMsg_AVR_LIBC[] PROGMEM = "AVR LibC Ver = "; + prog_char gTextMsg_GCC_VERSION[] PROGMEM = "GCC Version = "; + prog_char gTextMsg_CPU_SIGNATURE[] PROGMEM = "CPU signature= "; + prog_char gTextMsg_FUSE_BYTE_LOW[] PROGMEM = "Low fuse = "; + prog_char gTextMsg_FUSE_BYTE_HIGH[] PROGMEM = "High fuse = "; + prog_char gTextMsg_FUSE_BYTE_EXT[] PROGMEM = "Ext fuse = "; + prog_char gTextMsg_FUSE_BYTE_LOCK[] PROGMEM = "Lock fuse = "; + prog_char gTextMsg_GCC_DATE_STR[] PROGMEM = __DATE__; + prog_char gTextMsg_AVR_LIBC_VER_STR[] PROGMEM = __AVR_LIBC_VERSION_STRING__; + prog_char gTextMsg_GCC_VERSION_STR[] PROGMEM = __VERSION__; + prog_char gTextMsg_VECTOR_HEADER[] PROGMEM = "V# ADDR op code instruction addr Interrupt"; + prog_char gTextMsg_noVector[] PROGMEM = "no vector"; + prog_char gTextMsg_rjmp[] PROGMEM = "rjmp "; + prog_char gTextMsg_jmp[] PROGMEM = "jmp "; + prog_char gTextMsg_WHAT_PORT[] PROGMEM = "What port:"; + prog_char gTextMsg_PortNotSupported[] PROGMEM = "Port not supported"; + prog_char gTextMsg_MustBeLetter[] PROGMEM = "Must be a letter"; + prog_char gTextMsg_SPACE[] PROGMEM = " "; + prog_char gTextMsg_WriteToEEprom[] PROGMEM = "Writting EE"; + prog_char gTextMsg_ReadingEEprom[] PROGMEM = "Reading EE"; + prog_char gTextMsg_EEPROMerrorCnt[] PROGMEM = "eeprom error count="; + prog_char gTextMsg_PORT[] PROGMEM = "PORT"; + + +//************************************************************************ +//* Help messages + prog_char gTextMsg_HELP_MSG_0[] PROGMEM = "0=Zero address ctrs"; + prog_char gTextMsg_HELP_MSG_QM[] PROGMEM = "?=CPU stats"; + prog_char gTextMsg_HELP_MSG_AT[] PROGMEM = "@=EEPROM test"; + prog_char gTextMsg_HELP_MSG_B[] PROGMEM = "B=Blink LED"; + prog_char gTextMsg_HELP_MSG_E[] PROGMEM = "E=Dump EEPROM"; + prog_char gTextMsg_HELP_MSG_F[] PROGMEM = "F=Dump FLASH"; + prog_char gTextMsg_HELP_MSG_H[] PROGMEM = "H=Help"; + prog_char gTextMsg_HELP_MSG_L[] PROGMEM = "L=List I/O Ports"; + prog_char gTextMsg_HELP_MSG_Q[] PROGMEM = "Q=Quit & jump to user pgm"; + prog_char gTextMsg_HELP_MSG_R[] PROGMEM = "R=Dump RAM"; + prog_char gTextMsg_HELP_MSG_V[] PROGMEM = "V=show interrupt Vectors"; + prog_char gTextMsg_HELP_MSG_Y[] PROGMEM = "Y=Port blink"; + + prog_char gTextMsg_END[] PROGMEM = "*"; + + +//************************************************************************ +void PrintFromPROGMEM(void *dataPtr, unsigned char offset) +{ +uint8_t ii; +char theChar; + + ii = offset; + theChar = 1; + + while (theChar != 0) + { + theChar = pgm_read_byte_far((uint32_t)dataPtr + ii); + if (theChar != 0) + { + sendchar(theChar); + } + ii++; + } +} + +//************************************************************************ +void PrintNewLine(void) +{ + sendchar(0x0d); + sendchar(0x0a); +} + + +//************************************************************************ +void PrintFromPROGMEMln(void *dataPtr, unsigned char offset) +{ + PrintFromPROGMEM(dataPtr, offset); + + PrintNewLine(); +} + + +//************************************************************************ +void PrintString(char *textString) +{ +char theChar; +int ii; + + theChar = 1; + ii = 0; + while (theChar != 0) + { + theChar = textString[ii]; + if (theChar != 0) + { + sendchar(theChar); + } + ii++; + } +} + +//************************************************************************ +void PrintHexByte(unsigned char theByte) +{ +char theChar; + + theChar = 0x30 + ((theByte >> 4) & 0x0f); + if (theChar > 0x39) + { + theChar += 7; + } + sendchar(theChar ); + + theChar = 0x30 + (theByte & 0x0f); + if (theChar > 0x39) + { + theChar += 7; + } + sendchar(theChar ); +} + +//************************************************************************ +void PrintDecInt(int theNumber, int digitCnt) +{ +int theChar; +int myNumber; + + myNumber = theNumber; + + if ((myNumber > 100) || (digitCnt >= 3)) + { + theChar = 0x30 + myNumber / 100; + sendchar(theChar ); + } + + if ((myNumber > 10) || (digitCnt >= 2)) + { + theChar = 0x30 + ((myNumber % 100) / 10 ); + sendchar(theChar ); + } + theChar = 0x30 + (myNumber % 10); + sendchar(theChar ); +} + + + + +//************************************************************************ +static void PrintCPUstats(void) +{ +unsigned char fuseByte; + + PrintFromPROGMEMln(gTextMsg_Explorer, 0); + + PrintFromPROGMEM(gTextMsg_COMPILED_ON, 0); + PrintFromPROGMEMln(gTextMsg_GCC_DATE_STR, 0); + + PrintFromPROGMEM(gTextMsg_CPU_Type, 0); + PrintFromPROGMEMln(gTextMsg_CPU_Name, 0); + + PrintFromPROGMEM(gTextMsg_AVR_ARCH, 0); + PrintDecInt(__AVR_ARCH__, 1); + PrintNewLine(); + + PrintFromPROGMEM(gTextMsg_GCC_VERSION, 0); + PrintFromPROGMEMln(gTextMsg_GCC_VERSION_STR, 0); + + //* these can be found in avr/version.h + PrintFromPROGMEM(gTextMsg_AVR_LIBC, 0); + PrintFromPROGMEMln(gTextMsg_AVR_LIBC_VER_STR, 0); + +#if defined(SIGNATURE_0) + PrintFromPROGMEM(gTextMsg_CPU_SIGNATURE, 0); + //* these can be found in avr/iomxxx.h + PrintHexByte(SIGNATURE_0); + PrintHexByte(SIGNATURE_1); + PrintHexByte(SIGNATURE_2); + PrintNewLine(); +#endif + + +#if defined(GET_LOW_FUSE_BITS) + //* fuse settings + PrintFromPROGMEM(gTextMsg_FUSE_BYTE_LOW, 0); + fuseByte = boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS); + PrintHexByte(fuseByte); + PrintNewLine(); + + PrintFromPROGMEM(gTextMsg_FUSE_BYTE_HIGH, 0); + fuseByte = boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS); + PrintHexByte(fuseByte); + PrintNewLine(); + + PrintFromPROGMEM(gTextMsg_FUSE_BYTE_EXT, 0); + fuseByte = boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS); + PrintHexByte(fuseByte); + PrintNewLine(); + + PrintFromPROGMEM(gTextMsg_FUSE_BYTE_LOCK, 0); + fuseByte = boot_lock_fuse_bits_get(GET_LOCK_BITS); + PrintHexByte(fuseByte); + PrintNewLine(); + +#endif + +} + +#ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +//************************************************************************ +int analogRead(uint8_t pin) +{ +uint8_t low, high; + + // set the analog reference (high two bits of ADMUX) and select the + // channel (low 4 bits). this also sets ADLAR (left-adjust result) + // to 0 (the default). +// ADMUX = (analog_reference << 6) | (pin & 0x07); + ADMUX = (1 << 6) | (pin & 0x07); + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // the MUX5 bit of ADCSRB selects whether we're reading from channels + // 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high). + ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((pin >> 3) & 0x01) << MUX5); +#endif + + // without a delay, we seem to read from the wrong channel + //delay(1); + + // start the conversion + sbi(ADCSRA, ADSC); + + // ADSC is cleared when the conversion finishes + while (bit_is_set(ADCSRA, ADSC)); + + // we have to read ADCL first; doing so locks both ADCL + // and ADCH until ADCH is read. reading ADCL second would + // cause the results of each conversion to be discarded, + // as ADCL and ADCH would be locked when it completed. + low = ADCL; + high = ADCH; + + // combine the two bytes + return (high << 8) | low; +} + +//************************************************************************ +static void BlinkLED(void) +{ + PROGLED_DDR |= (1< 0) + { + if (myAddressPointer > 0x10000) + { + PrintHexByte((myAddressPointer >> 16) & 0x00ff); + } + PrintHexByte((myAddressPointer >> 8) & 0x00ff); + PrintHexByte(myAddressPointer & 0x00ff); + sendchar(0x20); + sendchar('-'); + sendchar(0x20); + + asciiDump[0] = 0; + for (ii=0; ii<16; ii++) + { + switch(dumpWhat) + { + case kDUMP_FLASH: + theValue = pgm_read_byte_far(myAddressPointer); + break; + + case kDUMP_EEPROM: + theValue = eeprom_read_byte((void *)myAddressPointer); + break; + + case kDUMP_RAM: + theValue = ramPtr[myAddressPointer]; + break; + + } + PrintHexByte(theValue); + sendchar(0x20); + if ((theValue >= 0x20) && (theValue < 0x7f)) + { + asciiDump[ii % 16] = theValue; + } + else + { + asciiDump[ii % 16] = '.'; + } + + myAddressPointer++; + } + asciiDump[16] = 0; + PrintString(asciiDump); + PrintNewLine(); + + numRows--; + } +} + + + +//************************************************************************ +//* returns amount of extended memory +static void EEPROMtest(void) +{ +int ii; +char theChar; +char theEEPROMchar; +int errorCount; + + PrintFromPROGMEMln(gTextMsg_WriteToEEprom, 0); + PrintNewLine(); + ii = 0; + while (((theChar = pgm_read_byte_far(gTextMsg_Explorer + ii)) != '*') && (ii < 512)) + { + eeprom_write_byte((uint8_t *)ii, theChar); + if (theChar == 0) + { + PrintFromPROGMEM(gTextMsg_SPACE, 0); + } + else + { + sendchar(theChar); + } + ii++; + } + + //* no go back through and test + PrintNewLine(); + PrintNewLine(); + PrintFromPROGMEMln(gTextMsg_ReadingEEprom, 0); + PrintNewLine(); + errorCount = 0; + ii = 0; + while (((theChar = pgm_read_byte_far(gTextMsg_Explorer + ii)) != '*') && (ii < 512)) + { + theEEPROMchar = eeprom_read_byte((uint8_t *)ii); + if (theEEPROMchar == 0) + { + PrintFromPROGMEM(gTextMsg_SPACE, 0); + } + else + { + sendchar(theEEPROMchar); + } + if (theEEPROMchar != theChar) + { + errorCount++; + } + ii++; + } + PrintNewLine(); + PrintNewLine(); + PrintFromPROGMEM(gTextMsg_EEPROMerrorCnt, 0); + PrintDecInt(errorCount, 1); + PrintNewLine(); + PrintNewLine(); + + gEepromIndex = 0; //* set index back to zero for next eeprom dump + +} + + + +#if (FLASHEND > 0x08000) + #include "avrinterruptnames.h" + #ifndef _INTERRUPT_NAMES_DEFINED_ + #warning Interrupt vectors not defined + #endif +#endif + +//************************************************************************ +static void VectorDisplay(void) +{ +unsigned long byte1; +unsigned long byte2; +unsigned long byte3; +unsigned long byte4; +unsigned long word1; +unsigned long word2; +int vectorIndex; +unsigned long myMemoryPtr; +unsigned long wordMemoryAddress; +unsigned long realitiveAddr; +unsigned long myFullAddress; +unsigned long absoluteAddr; +#if defined(_INTERRUPT_NAMES_DEFINED_) + long stringPointer; +#endif + + myMemoryPtr = 0; + vectorIndex = 0; + PrintFromPROGMEMln(gTextMsg_CPU_Name, 0); + PrintFromPROGMEMln(gTextMsg_VECTOR_HEADER, 0); + // V# ADDR op code + // 1 - 0000 = C3 BB 00 00 rjmp 03BB >000776 RESET + while (vectorIndex < kInterruptVectorCount) + { + wordMemoryAddress = myMemoryPtr / 2; + // 01 - 0000 = 12 34 + PrintDecInt(vectorIndex + 1, 2); + sendchar(0x20); + sendchar('-'); + sendchar(0x20); + PrintHexByte((wordMemoryAddress >> 8) & 0x00ff); + PrintHexByte((wordMemoryAddress) & 0x00ff); + sendchar(0x20); + sendchar('='); + sendchar(0x20); + + + //* the AVR is LITTLE ENDIAN, swap the byte order + byte1 = pgm_read_byte_far(myMemoryPtr++); + byte2 = pgm_read_byte_far(myMemoryPtr++); + word1 = (byte2 << 8) + byte1; + + byte3 = pgm_read_byte_far(myMemoryPtr++); + byte4 = pgm_read_byte_far(myMemoryPtr++); + word2 = (byte4 << 8) + byte3; + + + PrintHexByte(byte2); + sendchar(0x20); + PrintHexByte(byte1); + sendchar(0x20); + PrintHexByte(byte4); + sendchar(0x20); + PrintHexByte(byte3); + sendchar(0x20); + + if (word1 == 0xffff) + { + PrintFromPROGMEM(gTextMsg_noVector, 0); + } + else if ((word1 & 0xc000) == 0xc000) + { + //* rjmp instruction + realitiveAddr = word1 & 0x3FFF; + absoluteAddr = wordMemoryAddress + realitiveAddr; //* add the offset to the current address + absoluteAddr = absoluteAddr << 1; //* multiply by 2 for byte address + + PrintFromPROGMEM(gTextMsg_rjmp, 0); + PrintHexByte((realitiveAddr >> 8) & 0x00ff); + PrintHexByte((realitiveAddr) & 0x00ff); + sendchar(0x20); + sendchar('>'); + PrintHexByte((absoluteAddr >> 16) & 0x00ff); + PrintHexByte((absoluteAddr >> 8) & 0x00ff); + PrintHexByte((absoluteAddr) & 0x00ff); + + } + else if ((word1 & 0xfE0E) == 0x940c) + { + //* jmp instruction, this is REALLY complicated, refer to the instruction manual (JMP) + myFullAddress = ((byte1 & 0x01) << 16) + + ((byte1 & 0xf0) << 17) + + ((byte2 & 0x01) << 21) + + word2; + + absoluteAddr = myFullAddress << 1; + + PrintFromPROGMEM(gTextMsg_jmp, 0); + PrintHexByte((myFullAddress >> 16) & 0x00ff); + PrintHexByte((myFullAddress >> 8) & 0x00ff); + PrintHexByte((myFullAddress) & 0x00ff); + sendchar(0x20); + sendchar('>'); + PrintHexByte((absoluteAddr >> 16) & 0x00ff); + PrintHexByte((absoluteAddr >> 8) & 0x00ff); + PrintHexByte((absoluteAddr) & 0x00ff); + } + + #if defined(_INTERRUPT_NAMES_DEFINED_) + sendchar(0x20); + stringPointer = pgm_read_word_far(&(gInterruptNameTable[vectorIndex])); + PrintFromPROGMEM((char *)stringPointer, 0); + #endif + PrintNewLine(); + + vectorIndex++; + } +} + +//************************************************************************ +static void PrintAvailablePort(char thePortLetter) +{ + PrintFromPROGMEM(gTextMsg_PORT, 0); + sendchar(thePortLetter); + PrintNewLine(); +} + +//************************************************************************ +static void ListAvailablePorts(void) +{ + +#ifdef DDRA + PrintAvailablePort('A'); +#endif + +#ifdef DDRB + PrintAvailablePort('B'); +#endif + +#ifdef DDRC + PrintAvailablePort('C'); +#endif + +#ifdef DDRD + PrintAvailablePort('D'); +#endif + +#ifdef DDRE + PrintAvailablePort('E'); +#endif + +#ifdef DDRF + PrintAvailablePort('F'); +#endif + +#ifdef DDRG + PrintAvailablePort('G'); +#endif + +#ifdef DDRH + PrintAvailablePort('H'); +#endif + +#ifdef DDRI + PrintAvailablePort('I'); +#endif + +#ifdef DDRJ + PrintAvailablePort('J'); +#endif + +#ifdef DDRK + PrintAvailablePort('K'); +#endif + +#ifdef DDRL + PrintAvailablePort('L'); +#endif + +} + +//************************************************************************ +static void AVR_PortOutput(void) +{ +char portLetter; +char getCharFlag; + + PrintFromPROGMEM(gTextMsg_WHAT_PORT, 0); + + portLetter = recchar(); + portLetter = portLetter & 0x5f; + sendchar(portLetter); + PrintNewLine(); + + if ((portLetter >= 'A') && (portLetter <= 'Z')) + { + getCharFlag = true; + switch(portLetter) + { + #ifdef DDRA + case 'A': + DDRA = 0xff; + while (!Serial_Available()) + { + PORTA ^= 0xff; + delay_ms(200); + } + PORTA = 0; + break; + #endif + + #ifdef DDRB + case 'B': + DDRB = 0xff; + while (!Serial_Available()) + { + PORTB ^= 0xff; + delay_ms(200); + } + PORTB = 0; + break; + #endif + + #ifdef DDRC + case 'C': + DDRC = 0xff; + while (!Serial_Available()) + { + PORTC ^= 0xff; + delay_ms(200); + } + PORTC = 0; + break; + #endif + + #ifdef DDRD + case 'D': + DDRD = 0xff; + while (!Serial_Available()) + { + PORTD ^= 0xff; + delay_ms(200); + } + PORTD = 0; + break; + #endif + + #ifdef DDRE + case 'E': + DDRE = 0xff; + while (!Serial_Available()) + { + PORTE ^= 0xff; + delay_ms(200); + } + PORTE = 0; + break; + #endif + + #ifdef DDRF + case 'F': + DDRF = 0xff; + while (!Serial_Available()) + { + PORTF ^= 0xff; + delay_ms(200); + } + PORTF = 0; + break; + #endif + + #ifdef DDRG + case 'G': + DDRG = 0xff; + while (!Serial_Available()) + { + PORTG ^= 0xff; + delay_ms(200); + } + PORTG = 0; + break; + #endif + + #ifdef DDRH + case 'H': + DDRH = 0xff; + while (!Serial_Available()) + { + PORTH ^= 0xff; + delay_ms(200); + } + PORTH = 0; + break; + #endif + + #ifdef DDRI + case 'I': + DDRI = 0xff; + while (!Serial_Available()) + { + PORTI ^= 0xff; + delay_ms(200); + } + PORTI = 0; + break; + #endif + + #ifdef DDRJ + case 'J': + DDRJ = 0xff; + while (!Serial_Available()) + { + PORTJ ^= 0xff; + delay_ms(200); + } + PORTJ = 0; + break; + #endif + + #ifdef DDRK + case 'K': + DDRK = 0xff; + while (!Serial_Available()) + { + PORTK ^= 0xff; + delay_ms(200); + } + PORTK = 0; + break; + #endif + + #ifdef DDRL + case 'L': + DDRL = 0xff; + while (!Serial_Available()) + { + PORTL ^= 0xff; + delay_ms(200); + } + PORTL = 0; + break; + #endif + + default: + PrintFromPROGMEMln(gTextMsg_PortNotSupported, 0); + getCharFlag = false; + break; + } + if (getCharFlag) + { + recchar(); + } + } + else + { + PrintFromPROGMEMln(gTextMsg_MustBeLetter, 0); + } +} + + +//******************************************************************* +static void PrintHelp(void) +{ + PrintFromPROGMEMln(gTextMsg_HELP_MSG_0, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_QM, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_AT, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_B, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_E, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_F, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_H, 0); + + PrintFromPROGMEMln(gTextMsg_HELP_MSG_L, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_Q, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_R, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_V, 0); + PrintFromPROGMEMln(gTextMsg_HELP_MSG_Y, 0); +} + +//************************************************************************ +static void RunMonitor(void) +{ +char keepGoing; +unsigned char theChar; +int ii, jj; + + for (ii=0; ii<5; ii++) + { + for (jj=0; jj<25; jj++) + { + sendchar('!'); + } + PrintNewLine(); + } + + gRamIndex = 0; + gFlashIndex = 0; + gEepromIndex = 0; + + PrintFromPROGMEMln(gTextMsg_Explorer, 0); + + keepGoing = 1; + while (keepGoing) + { + PrintFromPROGMEM(gTextMsg_Prompt, 0); + theChar = recchar(); + if (theChar >= 0x60) + { + theChar = theChar & 0x5F; + } + #if defined( _CEREBOTPLUS_BOARD_ ) + if (theChar == 0x5F) + { + + } + else + #endif + if (theChar >= 0x20) + { + sendchar(theChar); + sendchar(0x20); + } + + switch(theChar) + { + case '0': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_0, 2); + gFlashIndex = 0; + gRamIndex = 0; + gEepromIndex = 0; + break; + + case '?': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_QM, 2); + PrintCPUstats(); + break; + + case '@': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_AT, 2); + EEPROMtest(); + break; + + case 'B': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_B, 2); + BlinkLED(); + break; + + case 'E': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_E, 2); + DumpHex(kDUMP_EEPROM, gEepromIndex, 16); + gEepromIndex += 256; + if (gEepromIndex > E2END) + { + gEepromIndex = 0; + } + break; + + case 'F': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_F, 2); + DumpHex(kDUMP_FLASH, gFlashIndex, 16); + gFlashIndex += 256; + break; + + case 'H': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_H, 2); + PrintHelp(); + break; + + case 'L': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_L, 2); + ListAvailablePorts(); + break; + + case 'Q': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_Q, 2); + keepGoing = false; + break; + + case 'R': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_R, 2); + DumpHex(kDUMP_RAM, gRamIndex, 16); + gRamIndex += 256; + break; + + case 'V': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_V, 2); + VectorDisplay(); + break; + + case 'Y': + PrintFromPROGMEMln(gTextMsg_HELP_MSG_Y, 2); + AVR_PortOutput(); + break; + + #if defined( _CEREBOTPLUS_BOARD_ ) + case 0x5F: + //* do nothing + break; + #endif + + default: + PrintFromPROGMEMln(gTextMsg_HUH, 0); + break; + } + } +} + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.ppg b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.ppg new file mode 100644 index 0000000..a8929d7 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/bootloaders/stk500v2/stk500boot.ppg @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Arduino.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Arduino.h new file mode 100644 index 0000000..b265825 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Arduino.h @@ -0,0 +1,215 @@ +#ifndef Arduino_h +#define Arduino_h + +#include +#include +#include + +#include +#include +#include + +#include "binary.h" + +#ifdef __cplusplus +extern "C"{ +#endif + +#define HIGH 0x1 +#define LOW 0x0 + +#define INPUT 0x0 +#define OUTPUT 0x1 +#define INPUT_PULLUP 0x2 + +#define true 0x1 +#define false 0x0 + +#define PI 3.1415926535897932384626433832795 +#define HALF_PI 1.5707963267948966192313216916398 +#define TWO_PI 6.283185307179586476925286766559 +#define DEG_TO_RAD 0.017453292519943295769236907684886 +#define RAD_TO_DEG 57.295779513082320876798154814105 + +#define SERIAL 0x0 +#define DISPLAY 0x1 + +#define LSBFIRST 0 +#define MSBFIRST 1 + +#define CHANGE 1 +#define FALLING 2 +#define RISING 3 + +#if defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) || defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) +#define DEFAULT 0 +#define EXTERNAL 1 +#define INTERNAL 2 +#else +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) +#define INTERNAL1V1 2 +#define INTERNAL2V56 3 +#else +#define INTERNAL 3 +#endif +#define DEFAULT 1 +#define EXTERNAL 0 +#endif + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +#define min(a,b) ((a)<(b)?(a):(b)) +#define max(a,b) ((a)>(b)?(a):(b)) +#define abs(x) ((x)>0?(x):-(x)) +#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) +#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define radians(deg) ((deg)*DEG_TO_RAD) +#define degrees(rad) ((rad)*RAD_TO_DEG) +#define sq(x) ((x)*(x)) + +#define interrupts() sei() +#define noInterrupts() cli() + +#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) +#define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() ) +#define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() ) + +#define lowByte(w) ((uint8_t) ((w) & 0xff)) +#define highByte(w) ((uint8_t) ((w) >> 8)) + +#define bitRead(value, bit) (((value) >> (bit)) & 0x01) +#define bitSet(value, bit) ((value) |= (1UL << (bit))) +#define bitClear(value, bit) ((value) &= ~(1UL << (bit))) +#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit)) + + +typedef unsigned int word; + +#define bit(b) (1UL << (b)) + +typedef uint8_t boolean; +typedef uint8_t byte; + +void init(void); + +void pinMode(uint8_t, uint8_t); +void digitalWrite(uint8_t, uint8_t); +int digitalRead(uint8_t); +int analogRead(uint8_t); +void analogReference(uint8_t mode); +void analogWrite(uint8_t, int); + +unsigned long millis(void); +unsigned long micros(void); +void delay(unsigned long); +void delayMicroseconds(unsigned int us); +unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout); + +void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val); +uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder); + +void attachInterrupt(uint8_t, void (*)(void), int mode); +void detachInterrupt(uint8_t); + +void setup(void); +void loop(void); + +// Get the bit location within the hardware port of the given virtual pin. +// This comes from the pins_*.c file for the active board configuration. + +#define analogInPinToBit(P) (P) + +// On the ATmega1280, the addresses of some of the port registers are +// greater than 255, so we can't store them in uint8_t's. +extern const uint16_t PROGMEM port_to_mode_PGM[]; +extern const uint16_t PROGMEM port_to_input_PGM[]; +extern const uint16_t PROGMEM port_to_output_PGM[]; + +extern const uint8_t PROGMEM digital_pin_to_port_PGM[]; +// extern const uint8_t PROGMEM digital_pin_to_bit_PGM[]; +extern const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[]; +extern const uint8_t PROGMEM digital_pin_to_timer_PGM[]; + +// Get the bit location within the hardware port of the given virtual pin. +// This comes from the pins_*.c file for the active board configuration. +// +// These perform slightly better as macros compared to inline functions +// +#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) ) +#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) ) +#define digitalPinToTimer(P) ( pgm_read_byte( digital_pin_to_timer_PGM + (P) ) ) +#define analogInPinToBit(P) (P) +#define portOutputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + (P))) ) +#define portInputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + (P))) ) +#define portModeRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + (P))) ) + +#define NOT_A_PIN 0 +#define NOT_A_PORT 0 + +#ifdef ARDUINO_MAIN +#define PA 1 +#define PB 2 +#define PC 3 +#define PD 4 +#define PE 5 +#define PF 6 +#define PG 7 +#define PH 8 +#define PJ 10 +#define PK 11 +#define PL 12 +#endif + +#define NOT_ON_TIMER 0 +#define TIMER0A 1 +#define TIMER0B 2 +#define TIMER1A 3 +#define TIMER1B 4 +#define TIMER2 5 +#define TIMER2A 6 +#define TIMER2B 7 + +#define TIMER3A 8 +#define TIMER3B 9 +#define TIMER3C 10 +#define TIMER4A 11 +#define TIMER4B 12 +#define TIMER4C 13 +#define TIMER4D 14 +#define TIMER5A 15 +#define TIMER5B 16 +#define TIMER5C 17 + +#ifdef __cplusplus +} // extern "C" +#endif + +#ifdef __cplusplus +#include "WCharacter.h" +#include "WString.h" +#include "HardwareSerial.h" + +uint16_t makeWord(uint16_t w); +uint16_t makeWord(byte h, byte l); + +#define word(...) makeWord(__VA_ARGS__) + +unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout = 1000000L); + +void tone(uint8_t _pin, unsigned int frequency, unsigned long duration = 0); +void noTone(uint8_t _pin); + +// WMath prototypes +long random(long); +long random(long, long); +void randomSeed(unsigned int); +long map(long, long, long, long, long); + +#endif + +#include "pins_arduino.h" + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/CDC.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/CDC.cpp new file mode 100644 index 0000000..701e483 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/CDC.cpp @@ -0,0 +1,239 @@ + + +/* Copyright (c) 2011, Peter Barrett +** +** Permission to use, copy, modify, and/or distribute this software for +** any purpose with or without fee is hereby granted, provided that the +** above copyright notice and this permission notice appear in all copies. +** +** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL +** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED +** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR +** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES +** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, +** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, +** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS +** SOFTWARE. +*/ + +#include "Platform.h" +#include "USBAPI.h" +#include + +#if defined(USBCON) +#ifdef CDC_ENABLED + +#if (RAMEND < 1000) +#define SERIAL_BUFFER_SIZE 16 +#else +#define SERIAL_BUFFER_SIZE 64 +#endif + +struct ring_buffer +{ + unsigned char buffer[SERIAL_BUFFER_SIZE]; + volatile int head; + volatile int tail; +}; + +ring_buffer cdc_rx_buffer = { { 0 }, 0, 0}; + +typedef struct +{ + u32 dwDTERate; + u8 bCharFormat; + u8 bParityType; + u8 bDataBits; + u8 lineState; +} LineInfo; + +static volatile LineInfo _usbLineInfo = { 57600, 0x00, 0x00, 0x00, 0x00 }; + +#define WEAK __attribute__ ((weak)) + +extern const CDCDescriptor _cdcInterface PROGMEM; +const CDCDescriptor _cdcInterface = +{ + D_IAD(0,2,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,1), + + // CDC communication interface + D_INTERFACE(CDC_ACM_INTERFACE,1,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,0), + D_CDCCS(CDC_HEADER,0x10,0x01), // Header (1.10 bcd) + D_CDCCS(CDC_CALL_MANAGEMENT,1,1), // Device handles call management (not) + D_CDCCS4(CDC_ABSTRACT_CONTROL_MANAGEMENT,6), // SET_LINE_CODING, GET_LINE_CODING, SET_CONTROL_LINE_STATE supported + D_CDCCS(CDC_UNION,CDC_ACM_INTERFACE,CDC_DATA_INTERFACE), // Communication interface is master, data interface is slave 0 + D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_ACM),USB_ENDPOINT_TYPE_INTERRUPT,0x10,0x40), + + // CDC data interface + D_INTERFACE(CDC_DATA_INTERFACE,2,CDC_DATA_INTERFACE_CLASS,0,0), + D_ENDPOINT(USB_ENDPOINT_OUT(CDC_ENDPOINT_OUT),USB_ENDPOINT_TYPE_BULK,0x40,0), + D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_IN ),USB_ENDPOINT_TYPE_BULK,0x40,0) +}; + +int WEAK CDC_GetInterface(u8* interfaceNum) +{ + interfaceNum[0] += 2; // uses 2 + return USB_SendControl(TRANSFER_PGM,&_cdcInterface,sizeof(_cdcInterface)); +} + +bool WEAK CDC_Setup(Setup& setup) +{ + u8 r = setup.bRequest; + u8 requestType = setup.bmRequestType; + + if (REQUEST_DEVICETOHOST_CLASS_INTERFACE == requestType) + { + if (CDC_GET_LINE_CODING == r) + { + USB_SendControl(0,(void*)&_usbLineInfo,7); + return true; + } + } + + if (REQUEST_HOSTTODEVICE_CLASS_INTERFACE == requestType) + { + if (CDC_SET_LINE_CODING == r) + { + USB_RecvControl((void*)&_usbLineInfo,7); + return true; + } + + if (CDC_SET_CONTROL_LINE_STATE == r) + { + _usbLineInfo.lineState = setup.wValueL; + + // auto-reset into the bootloader is triggered when the port, already + // open at 1200 bps, is closed. this is the signal to start the watchdog + // with a relatively long period so it can finish housekeeping tasks + // like servicing endpoints before the sketch ends + if (1200 == _usbLineInfo.dwDTERate) { + // We check DTR state to determine if host port is open (bit 0 of lineState). + if ((_usbLineInfo.lineState & 0x01) == 0) { + *(uint16_t *)0x0800 = 0x7777; + wdt_enable(WDTO_120MS); + } else { + // Most OSs do some intermediate steps when configuring ports and DTR can + // twiggle more than once before stabilizing. + // To avoid spurious resets we set the watchdog to 250ms and eventually + // cancel if DTR goes back high. + + wdt_disable(); + wdt_reset(); + *(uint16_t *)0x0800 = 0x0; + } + } + return true; + } + } + return false; +} + + +int _serialPeek = -1; +void Serial_::begin(uint16_t baud_count) +{ +} + +void Serial_::end(void) +{ +} + +void Serial_::accept(void) +{ + ring_buffer *buffer = &cdc_rx_buffer; + int i = (unsigned int)(buffer->head+1) % SERIAL_BUFFER_SIZE; + + // if we should be storing the received character into the location + // just before the tail (meaning that the head would advance to the + // current location of the tail), we're about to overflow the buffer + // and so we don't write the character or advance the head. + + // while we have room to store a byte + while (i != buffer->tail) { + int c = USB_Recv(CDC_RX); + if (c == -1) + break; // no more data + buffer->buffer[buffer->head] = c; + buffer->head = i; + + i = (unsigned int)(buffer->head+1) % SERIAL_BUFFER_SIZE; + } +} + +int Serial_::available(void) +{ + ring_buffer *buffer = &cdc_rx_buffer; + return (unsigned int)(SERIAL_BUFFER_SIZE + buffer->head - buffer->tail) % SERIAL_BUFFER_SIZE; +} + +int Serial_::peek(void) +{ + ring_buffer *buffer = &cdc_rx_buffer; + if (buffer->head == buffer->tail) { + return -1; + } else { + return buffer->buffer[buffer->tail]; + } +} + +int Serial_::read(void) +{ + ring_buffer *buffer = &cdc_rx_buffer; + // if the head isn't ahead of the tail, we don't have any characters + if (buffer->head == buffer->tail) { + return -1; + } else { + unsigned char c = buffer->buffer[buffer->tail]; + buffer->tail = (unsigned int)(buffer->tail + 1) % SERIAL_BUFFER_SIZE; + return c; + } +} + +void Serial_::flush(void) +{ + USB_Flush(CDC_TX); +} + +size_t Serial_::write(uint8_t c) +{ + /* only try to send bytes if the high-level CDC connection itself + is open (not just the pipe) - the OS should set lineState when the port + is opened and clear lineState when the port is closed. + bytes sent before the user opens the connection or after + the connection is closed are lost - just like with a UART. */ + + // TODO - ZE - check behavior on different OSes and test what happens if an + // open connection isn't broken cleanly (cable is yanked out, host dies + // or locks up, or host virtual serial port hangs) + if (_usbLineInfo.lineState > 0) { + int r = USB_Send(CDC_TX,&c,1); + if (r > 0) { + return r; + } else { + setWriteError(); + return 0; + } + } + setWriteError(); + return 0; +} + +// This operator is a convenient way for a sketch to check whether the +// port has actually been configured and opened by the host (as opposed +// to just being connected to the host). It can be used, for example, in +// setup() before printing to ensure that an application on the host is +// actually ready to receive and display the data. +// We add a short delay before returning to fix a bug observed by Federico +// where the port is configured (lineState != 0) but not quite opened. +Serial_::operator bool() { + bool result = false; + if (_usbLineInfo.lineState > 0) + result = true; + delay(10); + return result; +} + +Serial_ Serial; + +#endif +#endif /* if defined(USBCON) */ diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Client.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Client.h new file mode 100644 index 0000000..ea13483 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Client.h @@ -0,0 +1,26 @@ +#ifndef client_h +#define client_h +#include "Print.h" +#include "Stream.h" +#include "IPAddress.h" + +class Client : public Stream { + +public: + virtual int connect(IPAddress ip, uint16_t port) =0; + virtual int connect(const char *host, uint16_t port) =0; + virtual size_t write(uint8_t) =0; + virtual size_t write(const uint8_t *buf, size_t size) =0; + virtual int available() = 0; + virtual int read() = 0; + virtual int read(uint8_t *buf, size_t size) = 0; + virtual int peek() = 0; + virtual void flush() = 0; + virtual void stop() = 0; + virtual uint8_t connected() = 0; + virtual operator bool() = 0; +protected: + uint8_t* rawIPAddress(IPAddress& addr) { return addr.raw_address(); }; +}; + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/HID.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/HID.cpp new file mode 100644 index 0000000..ac63608 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/HID.cpp @@ -0,0 +1,520 @@ + + +/* Copyright (c) 2011, Peter Barrett +** +** Permission to use, copy, modify, and/or distribute this software for +** any purpose with or without fee is hereby granted, provided that the +** above copyright notice and this permission notice appear in all copies. +** +** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL +** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED +** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR +** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES +** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, +** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, +** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS +** SOFTWARE. +*/ + +#include "Platform.h" +#include "USBAPI.h" +#include "USBDesc.h" + +#if defined(USBCON) +#ifdef HID_ENABLED + +//#define RAWHID_ENABLED + +// Singletons for mouse and keyboard + +Mouse_ Mouse; +Keyboard_ Keyboard; + +//================================================================================ +//================================================================================ + +// HID report descriptor + +#define LSB(_x) ((_x) & 0xFF) +#define MSB(_x) ((_x) >> 8) + +#define RAWHID_USAGE_PAGE 0xFFC0 +#define RAWHID_USAGE 0x0C00 +#define RAWHID_TX_SIZE 64 +#define RAWHID_RX_SIZE 64 + +extern const u8 _hidReportDescriptor[] PROGMEM; +const u8 _hidReportDescriptor[] = { + + // Mouse + 0x05, 0x01, // USAGE_PAGE (Generic Desktop) // 54 + 0x09, 0x02, // USAGE (Mouse) + 0xa1, 0x01, // COLLECTION (Application) + 0x09, 0x01, // USAGE (Pointer) + 0xa1, 0x00, // COLLECTION (Physical) + 0x85, 0x01, // REPORT_ID (1) + 0x05, 0x09, // USAGE_PAGE (Button) + 0x19, 0x01, // USAGE_MINIMUM (Button 1) + 0x29, 0x03, // USAGE_MAXIMUM (Button 3) + 0x15, 0x00, // LOGICAL_MINIMUM (0) + 0x25, 0x01, // LOGICAL_MAXIMUM (1) + 0x95, 0x03, // REPORT_COUNT (3) + 0x75, 0x01, // REPORT_SIZE (1) + 0x81, 0x02, // INPUT (Data,Var,Abs) + 0x95, 0x01, // REPORT_COUNT (1) + 0x75, 0x05, // REPORT_SIZE (5) + 0x81, 0x03, // INPUT (Cnst,Var,Abs) + 0x05, 0x01, // USAGE_PAGE (Generic Desktop) + 0x09, 0x30, // USAGE (X) + 0x09, 0x31, // USAGE (Y) + 0x09, 0x38, // USAGE (Wheel) + 0x15, 0x81, // LOGICAL_MINIMUM (-127) + 0x25, 0x7f, // LOGICAL_MAXIMUM (127) + 0x75, 0x08, // REPORT_SIZE (8) + 0x95, 0x03, // REPORT_COUNT (3) + 0x81, 0x06, // INPUT (Data,Var,Rel) + 0xc0, // END_COLLECTION + 0xc0, // END_COLLECTION + + // Keyboard + 0x05, 0x01, // USAGE_PAGE (Generic Desktop) // 47 + 0x09, 0x06, // USAGE (Keyboard) + 0xa1, 0x01, // COLLECTION (Application) + 0x85, 0x02, // REPORT_ID (2) + 0x05, 0x07, // USAGE_PAGE (Keyboard) + + 0x19, 0xe0, // USAGE_MINIMUM (Keyboard LeftControl) + 0x29, 0xe7, // USAGE_MAXIMUM (Keyboard Right GUI) + 0x15, 0x00, // LOGICAL_MINIMUM (0) + 0x25, 0x01, // LOGICAL_MAXIMUM (1) + 0x75, 0x01, // REPORT_SIZE (1) + + 0x95, 0x08, // REPORT_COUNT (8) + 0x81, 0x02, // INPUT (Data,Var,Abs) + 0x95, 0x01, // REPORT_COUNT (1) + 0x75, 0x08, // REPORT_SIZE (8) + 0x81, 0x03, // INPUT (Cnst,Var,Abs) + + 0x95, 0x06, // REPORT_COUNT (6) + 0x75, 0x08, // REPORT_SIZE (8) + 0x15, 0x00, // LOGICAL_MINIMUM (0) + 0x25, 0x65, // LOGICAL_MAXIMUM (101) + 0x05, 0x07, // USAGE_PAGE (Keyboard) + + 0x19, 0x00, // USAGE_MINIMUM (Reserved (no event indicated)) + 0x29, 0x65, // USAGE_MAXIMUM (Keyboard Application) + 0x81, 0x00, // INPUT (Data,Ary,Abs) + 0xc0, // END_COLLECTION + +#if RAWHID_ENABLED + // RAW HID + 0x06, LSB(RAWHID_USAGE_PAGE), MSB(RAWHID_USAGE_PAGE), // 30 + 0x0A, LSB(RAWHID_USAGE), MSB(RAWHID_USAGE), + + 0xA1, 0x01, // Collection 0x01 + 0x85, 0x03, // REPORT_ID (3) + 0x75, 0x08, // report size = 8 bits + 0x15, 0x00, // logical minimum = 0 + 0x26, 0xFF, 0x00, // logical maximum = 255 + + 0x95, 64, // report count TX + 0x09, 0x01, // usage + 0x81, 0x02, // Input (array) + + 0x95, 64, // report count RX + 0x09, 0x02, // usage + 0x91, 0x02, // Output (array) + 0xC0 // end collection +#endif +}; + +extern const HIDDescriptor _hidInterface PROGMEM; +const HIDDescriptor _hidInterface = +{ + D_INTERFACE(HID_INTERFACE,1,3,0,0), + D_HIDREPORT(sizeof(_hidReportDescriptor)), + D_ENDPOINT(USB_ENDPOINT_IN (HID_ENDPOINT_INT),USB_ENDPOINT_TYPE_INTERRUPT,0x40,0x01) +}; + +//================================================================================ +//================================================================================ +// Driver + +u8 _hid_protocol = 1; +u8 _hid_idle = 1; + +#define WEAK __attribute__ ((weak)) + +int WEAK HID_GetInterface(u8* interfaceNum) +{ + interfaceNum[0] += 1; // uses 1 + return USB_SendControl(TRANSFER_PGM,&_hidInterface,sizeof(_hidInterface)); +} + +int WEAK HID_GetDescriptor(int i) +{ + return USB_SendControl(TRANSFER_PGM,_hidReportDescriptor,sizeof(_hidReportDescriptor)); +} + +void WEAK HID_SendReport(u8 id, const void* data, int len) +{ + USB_Send(HID_TX, &id, 1); + USB_Send(HID_TX | TRANSFER_RELEASE,data,len); +} + +bool WEAK HID_Setup(Setup& setup) +{ + u8 r = setup.bRequest; + u8 requestType = setup.bmRequestType; + if (REQUEST_DEVICETOHOST_CLASS_INTERFACE == requestType) + { + if (HID_GET_REPORT == r) + { + //HID_GetReport(); + return true; + } + if (HID_GET_PROTOCOL == r) + { + //Send8(_hid_protocol); // TODO + return true; + } + } + + if (REQUEST_HOSTTODEVICE_CLASS_INTERFACE == requestType) + { + if (HID_SET_PROTOCOL == r) + { + _hid_protocol = setup.wValueL; + return true; + } + + if (HID_SET_IDLE == r) + { + _hid_idle = setup.wValueL; + return true; + } + } + return false; +} + +//================================================================================ +//================================================================================ +// Mouse + +Mouse_::Mouse_(void) : _buttons(0) +{ +} + +void Mouse_::begin(void) +{ +} + +void Mouse_::end(void) +{ +} + +void Mouse_::click(uint8_t b) +{ + _buttons = b; + move(0,0,0); + _buttons = 0; + move(0,0,0); +} + +void Mouse_::move(signed char x, signed char y, signed char wheel) +{ + u8 m[4]; + m[0] = _buttons; + m[1] = x; + m[2] = y; + m[3] = wheel; + HID_SendReport(1,m,4); +} + +void Mouse_::buttons(uint8_t b) +{ + if (b != _buttons) + { + _buttons = b; + move(0,0,0); + } +} + +void Mouse_::press(uint8_t b) +{ + buttons(_buttons | b); +} + +void Mouse_::release(uint8_t b) +{ + buttons(_buttons & ~b); +} + +bool Mouse_::isPressed(uint8_t b) +{ + if ((b & _buttons) > 0) + return true; + return false; +} + +//================================================================================ +//================================================================================ +// Keyboard + +Keyboard_::Keyboard_(void) +{ +} + +void Keyboard_::begin(void) +{ +} + +void Keyboard_::end(void) +{ +} + +void Keyboard_::sendReport(KeyReport* keys) +{ + HID_SendReport(2,keys,sizeof(KeyReport)); +} + +extern +const uint8_t _asciimap[128] PROGMEM; + +#define SHIFT 0x80 +const uint8_t _asciimap[128] = +{ + 0x00, // NUL + 0x00, // SOH + 0x00, // STX + 0x00, // ETX + 0x00, // EOT + 0x00, // ENQ + 0x00, // ACK + 0x00, // BEL + 0x2a, // BS Backspace + 0x2b, // TAB Tab + 0x28, // LF Enter + 0x00, // VT + 0x00, // FF + 0x00, // CR + 0x00, // SO + 0x00, // SI + 0x00, // DEL + 0x00, // DC1 + 0x00, // DC2 + 0x00, // DC3 + 0x00, // DC4 + 0x00, // NAK + 0x00, // SYN + 0x00, // ETB + 0x00, // CAN + 0x00, // EM + 0x00, // SUB + 0x00, // ESC + 0x00, // FS + 0x00, // GS + 0x00, // RS + 0x00, // US + + 0x2c, // ' ' + 0x1e|SHIFT, // ! + 0x34|SHIFT, // " + 0x20|SHIFT, // # + 0x21|SHIFT, // $ + 0x22|SHIFT, // % + 0x24|SHIFT, // & + 0x34, // ' + 0x26|SHIFT, // ( + 0x27|SHIFT, // ) + 0x25|SHIFT, // * + 0x2e|SHIFT, // + + 0x36, // , + 0x2d, // - + 0x37, // . + 0x38, // / + 0x27, // 0 + 0x1e, // 1 + 0x1f, // 2 + 0x20, // 3 + 0x21, // 4 + 0x22, // 5 + 0x23, // 6 + 0x24, // 7 + 0x25, // 8 + 0x26, // 9 + 0x33|SHIFT, // : + 0x33, // ; + 0x36|SHIFT, // < + 0x2e, // = + 0x37|SHIFT, // > + 0x38|SHIFT, // ? + 0x1f|SHIFT, // @ + 0x04|SHIFT, // A + 0x05|SHIFT, // B + 0x06|SHIFT, // C + 0x07|SHIFT, // D + 0x08|SHIFT, // E + 0x09|SHIFT, // F + 0x0a|SHIFT, // G + 0x0b|SHIFT, // H + 0x0c|SHIFT, // I + 0x0d|SHIFT, // J + 0x0e|SHIFT, // K + 0x0f|SHIFT, // L + 0x10|SHIFT, // M + 0x11|SHIFT, // N + 0x12|SHIFT, // O + 0x13|SHIFT, // P + 0x14|SHIFT, // Q + 0x15|SHIFT, // R + 0x16|SHIFT, // S + 0x17|SHIFT, // T + 0x18|SHIFT, // U + 0x19|SHIFT, // V + 0x1a|SHIFT, // W + 0x1b|SHIFT, // X + 0x1c|SHIFT, // Y + 0x1d|SHIFT, // Z + 0x2f, // [ + 0x31, // bslash + 0x30, // ] + 0x23|SHIFT, // ^ + 0x2d|SHIFT, // _ + 0x35, // ` + 0x04, // a + 0x05, // b + 0x06, // c + 0x07, // d + 0x08, // e + 0x09, // f + 0x0a, // g + 0x0b, // h + 0x0c, // i + 0x0d, // j + 0x0e, // k + 0x0f, // l + 0x10, // m + 0x11, // n + 0x12, // o + 0x13, // p + 0x14, // q + 0x15, // r + 0x16, // s + 0x17, // t + 0x18, // u + 0x19, // v + 0x1a, // w + 0x1b, // x + 0x1c, // y + 0x1d, // z + 0x2f|SHIFT, // + 0x31|SHIFT, // | + 0x30|SHIFT, // } + 0x35|SHIFT, // ~ + 0 // DEL +}; + +uint8_t USBPutChar(uint8_t c); + +// press() adds the specified key (printing, non-printing, or modifier) +// to the persistent key report and sends the report. Because of the way +// USB HID works, the host acts like the key remains pressed until we +// call release(), releaseAll(), or otherwise clear the report and resend. +size_t Keyboard_::press(uint8_t k) +{ + uint8_t i; + if (k >= 136) { // it's a non-printing key (not a modifier) + k = k - 136; + } else if (k >= 128) { // it's a modifier key + _keyReport.modifiers |= (1<<(k-128)); + k = 0; + } else { // it's a printing key + k = pgm_read_byte(_asciimap + k); + if (!k) { + setWriteError(); + return 0; + } + if (k & 0x80) { // it's a capital letter or other character reached with shift + _keyReport.modifiers |= 0x02; // the left shift modifier + k &= 0x7F; + } + } + + // Add k to the key report only if it's not already present + // and if there is an empty slot. + if (_keyReport.keys[0] != k && _keyReport.keys[1] != k && + _keyReport.keys[2] != k && _keyReport.keys[3] != k && + _keyReport.keys[4] != k && _keyReport.keys[5] != k) { + + for (i=0; i<6; i++) { + if (_keyReport.keys[i] == 0x00) { + _keyReport.keys[i] = k; + break; + } + } + if (i == 6) { + setWriteError(); + return 0; + } + } + sendReport(&_keyReport); + return 1; +} + +// release() takes the specified key out of the persistent key report and +// sends the report. This tells the OS the key is no longer pressed and that +// it shouldn't be repeated any more. +size_t Keyboard_::release(uint8_t k) +{ + uint8_t i; + if (k >= 136) { // it's a non-printing key (not a modifier) + k = k - 136; + } else if (k >= 128) { // it's a modifier key + _keyReport.modifiers &= ~(1<<(k-128)); + k = 0; + } else { // it's a printing key + k = pgm_read_byte(_asciimap + k); + if (!k) { + return 0; + } + if (k & 0x80) { // it's a capital letter or other character reached with shift + _keyReport.modifiers &= ~(0x02); // the left shift modifier + k &= 0x7F; + } + } + + // Test the key report to see if k is present. Clear it if it exists. + // Check all positions in case the key is present more than once (which it shouldn't be) + for (i=0; i<6; i++) { + if (0 != k && _keyReport.keys[i] == k) { + _keyReport.keys[i] = 0x00; + } + } + + sendReport(&_keyReport); + return 1; +} + +void Keyboard_::releaseAll(void) +{ + _keyReport.keys[0] = 0; + _keyReport.keys[1] = 0; + _keyReport.keys[2] = 0; + _keyReport.keys[3] = 0; + _keyReport.keys[4] = 0; + _keyReport.keys[5] = 0; + _keyReport.modifiers = 0; + sendReport(&_keyReport); +} + +size_t Keyboard_::write(uint8_t c) +{ + uint8_t p = press(c); // Keydown + uint8_t r = release(c); // Keyup + return (p); // just return the result of press() since release() almost always returns 1 +} + +#endif + +#endif /* if defined(USBCON) */ \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.cpp new file mode 100644 index 0000000..2ca30f8 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.cpp @@ -0,0 +1,521 @@ +#define __AVR_LIBC_DEPRECATED_ENABLE__ + +/* + HardwareSerial.cpp - Hardware serial library for Wiring + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 23 November 2006 by David A. Mellis + Modified 28 September 2010 by Mark Sproul + Modified 14 August 2012 by Alarus +*/ + +#include +#include +#include +#include +#include "Arduino.h" +#include "wiring_private.h" + +// this next line disables the entire HardwareSerial.cpp, +// this is so I can support Attiny series and any other chip without a uart +#if defined(UBRRH) || defined(UBRR0H) || defined(UBRR1H) || defined(UBRR2H) || defined(UBRR3H) + +#include "HardwareSerial.h" + +/* + * on ATmega8, the uart and its bits are not numbered, so there is no "TXC0" + * definition. + */ +#if !defined(TXC0) +#if defined(TXC) +#define TXC0 TXC +#elif defined(TXC1) +// Some devices have uart1 but no uart0 +#define TXC0 TXC1 +#else +#error TXC0 not definable in HardwareSerial.h +#endif +#endif + +// Define constants and variables for buffering incoming serial data. We're +// using a ring buffer (I think), in which head is the index of the location +// to which to write the next incoming character and tail is the index of the +// location from which to read. +#if (RAMEND < 1000) + #define SERIAL_BUFFER_SIZE 16 +#else + #define SERIAL_BUFFER_SIZE 64 +#endif + +struct ring_buffer +{ + unsigned char buffer[SERIAL_BUFFER_SIZE]; + volatile unsigned int head; + volatile unsigned int tail; +}; + +#if defined(USBCON) + ring_buffer rx_buffer = { { 0 }, 0, 0}; + ring_buffer tx_buffer = { { 0 }, 0, 0}; +#endif +#if defined(UBRRH) || defined(UBRR0H) + ring_buffer rx_buffer = { { 0 }, 0, 0 }; + ring_buffer tx_buffer = { { 0 }, 0, 0 }; +#endif +#if defined(UBRR1H) + ring_buffer rx_buffer1 = { { 0 }, 0, 0 }; + ring_buffer tx_buffer1 = { { 0 }, 0, 0 }; +#endif +#if defined(UBRR2H) + ring_buffer rx_buffer2 = { { 0 }, 0, 0 }; + ring_buffer tx_buffer2 = { { 0 }, 0, 0 }; +#endif +#if defined(UBRR3H) + ring_buffer rx_buffer3 = { { 0 }, 0, 0 }; + ring_buffer tx_buffer3 = { { 0 }, 0, 0 }; +#endif + +inline void store_char(unsigned char c, ring_buffer *buffer) +{ + int i = (unsigned int)(buffer->head + 1) % SERIAL_BUFFER_SIZE; + + // if we should be storing the received character into the location + // just before the tail (meaning that the head would advance to the + // current location of the tail), we're about to overflow the buffer + // and so we don't write the character or advance the head. + if ((unsigned int)i != buffer->tail) { + buffer->buffer[buffer->head] = c; + buffer->head = i; + } +} + +#if !defined(USART0_RX_vect) && defined(USART1_RX_vect) +// do nothing - on the 32u4 the first USART is USART1 +#else +#if !defined(USART_RX_vect) && !defined(SIG_USART0_RECV) && \ + !defined(SIG_UART0_RECV) && !defined(USART0_RX_vect) && \ + !defined(SIG_UART_RECV) + #error "Don't know what the Data Received vector is called for the first UART" +#else + void serialEvent() __attribute__((weak)); + void serialEvent() {} + #define serialEvent_implemented +#if defined(USART_RX_vect) + SIGNAL(USART_RX_vect) +#elif defined(SIG_USART0_RECV) + SIGNAL(SIG_USART0_RECV) +#elif defined(SIG_UART0_RECV) + SIGNAL(SIG_UART0_RECV) +#elif defined(USART0_RX_vect) + SIGNAL(USART0_RX_vect) +#elif defined(SIG_UART_RECV) + SIGNAL(SIG_UART_RECV) +#endif + { + #if defined(UDR0) + if (bit_is_clear(UCSR0A, UPE0)) { + unsigned char c = UDR0; + store_char(c, &rx_buffer); + } else { + //unsigned char c = UDR0; + }; + #elif defined(UDR) + if (bit_is_clear(UCSRA, PE)) { + unsigned char c = UDR; + store_char(c, &rx_buffer); + } else { + //unsigned char c = UDR; + }; + #else + #error UDR not defined + #endif + } +#endif +#endif + +#if defined(USART1_RX_vect) + void serialEvent1() __attribute__((weak)); + void serialEvent1() {} + #define serialEvent1_implemented + SIGNAL(USART1_RX_vect) + { + if (bit_is_clear(UCSR1A, UPE1)) { + unsigned char c = UDR1; + store_char(c, &rx_buffer1); + } else { + //unsigned char c = UDR1; + }; + } +#elif defined(SIG_USART1_RECV) + #error SIG_USART1_RECV +#endif + +#if defined(USART2_RX_vect) && defined(UDR2) + void serialEvent2() __attribute__((weak)); + void serialEvent2() {} + #define serialEvent2_implemented + SIGNAL(USART2_RX_vect) + { + if (bit_is_clear(UCSR2A, UPE2)) { + unsigned char c = UDR2; + store_char(c, &rx_buffer2); + } else { + //unsigned char c = UDR2; + }; + } +#elif defined(SIG_USART2_RECV) + #error SIG_USART2_RECV +#endif + +#if defined(USART3_RX_vect) && defined(UDR3) + void serialEvent3() __attribute__((weak)); + void serialEvent3() {} + #define serialEvent3_implemented + SIGNAL(USART3_RX_vect) + { + if (bit_is_clear(UCSR3A, UPE3)) { + unsigned char c = UDR3; + store_char(c, &rx_buffer3); + } else { + //unsigned char c = UDR3; + }; + } +#elif defined(SIG_USART3_RECV) + #error SIG_USART3_RECV +#endif + +void serialEventRun(void) +{ +#ifdef serialEvent_implemented + if (Serial.available()) serialEvent(); +#endif +#ifdef serialEvent1_implemented + if (Serial1.available()) serialEvent1(); +#endif +#ifdef serialEvent2_implemented + if (Serial2.available()) serialEvent2(); +#endif +#ifdef serialEvent3_implemented + if (Serial3.available()) serialEvent3(); +#endif +} + + +#if !defined(USART0_UDRE_vect) && defined(USART1_UDRE_vect) +// do nothing - on the 32u4 the first USART is USART1 +#else +#if !defined(UART0_UDRE_vect) && !defined(UART_UDRE_vect) && !defined(USART0_UDRE_vect) && !defined(USART_UDRE_vect) + #error "Don't know what the Data Register Empty vector is called for the first UART" +#else +#if defined(UART0_UDRE_vect) +ISR(UART0_UDRE_vect) +#elif defined(UART_UDRE_vect) +ISR(UART_UDRE_vect) +#elif defined(USART0_UDRE_vect) +ISR(USART0_UDRE_vect) +#elif defined(USART_UDRE_vect) +ISR(USART_UDRE_vect) +#endif +{ + if (tx_buffer.head == tx_buffer.tail) { + // Buffer empty, so disable interrupts +#if defined(UCSR0B) + cbi(UCSR0B, UDRIE0); +#else + cbi(UCSRB, UDRIE); +#endif + } + else { + // There is more data in the output buffer. Send the next byte + unsigned char c = tx_buffer.buffer[tx_buffer.tail]; + tx_buffer.tail = (tx_buffer.tail + 1) % SERIAL_BUFFER_SIZE; + + #if defined(UDR0) + UDR0 = c; + #elif defined(UDR) + UDR = c; + #else + #error UDR not defined + #endif + } +} +#endif +#endif + +#ifdef USART1_UDRE_vect +ISR(USART1_UDRE_vect) +{ + if (tx_buffer1.head == tx_buffer1.tail) { + // Buffer empty, so disable interrupts + cbi(UCSR1B, UDRIE1); + } + else { + // There is more data in the output buffer. Send the next byte + unsigned char c = tx_buffer1.buffer[tx_buffer1.tail]; + tx_buffer1.tail = (tx_buffer1.tail + 1) % SERIAL_BUFFER_SIZE; + + UDR1 = c; + } +} +#endif + +#ifdef USART2_UDRE_vect +ISR(USART2_UDRE_vect) +{ + if (tx_buffer2.head == tx_buffer2.tail) { + // Buffer empty, so disable interrupts + cbi(UCSR2B, UDRIE2); + } + else { + // There is more data in the output buffer. Send the next byte + unsigned char c = tx_buffer2.buffer[tx_buffer2.tail]; + tx_buffer2.tail = (tx_buffer2.tail + 1) % SERIAL_BUFFER_SIZE; + + UDR2 = c; + } +} +#endif + +#ifdef USART3_UDRE_vect +ISR(USART3_UDRE_vect) +{ + if (tx_buffer3.head == tx_buffer3.tail) { + // Buffer empty, so disable interrupts + cbi(UCSR3B, UDRIE3); + } + else { + // There is more data in the output buffer. Send the next byte + unsigned char c = tx_buffer3.buffer[tx_buffer3.tail]; + tx_buffer3.tail = (tx_buffer3.tail + 1) % SERIAL_BUFFER_SIZE; + + UDR3 = c; + } +} +#endif + + +// Constructors //////////////////////////////////////////////////////////////// + +HardwareSerial::HardwareSerial(ring_buffer *rx_buffer, ring_buffer *tx_buffer, + volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, + volatile uint8_t *ucsra, volatile uint8_t *ucsrb, + volatile uint8_t *ucsrc, volatile uint8_t *udr, + uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udrie, uint8_t u2x) +{ + _rx_buffer = rx_buffer; + _tx_buffer = tx_buffer; + _ubrrh = ubrrh; + _ubrrl = ubrrl; + _ucsra = ucsra; + _ucsrb = ucsrb; + _ucsrc = ucsrc; + _udr = udr; + _rxen = rxen; + _txen = txen; + _rxcie = rxcie; + _udrie = udrie; + _u2x = u2x; +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void HardwareSerial::begin(unsigned long baud) +{ + uint16_t baud_setting; + bool use_u2x = true; + +#if F_CPU == 16000000UL + // hardcoded exception for compatibility with the bootloader shipped + // with the Duemilanove and previous boards and the firmware on the 8U2 + // on the Uno and Mega 2560. + if (baud == 57600) { + use_u2x = false; + } +#endif + +try_again: + + if (use_u2x) { + *_ucsra = 1 << _u2x; + baud_setting = (F_CPU / 4 / baud - 1) / 2; + } else { + *_ucsra = 0; + baud_setting = (F_CPU / 8 / baud - 1) / 2; + } + + if ((baud_setting > 4095) && use_u2x) + { + use_u2x = false; + goto try_again; + } + + // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) + *_ubrrh = baud_setting >> 8; + *_ubrrl = baud_setting; + + transmitting = false; + + sbi(*_ucsrb, _rxen); + sbi(*_ucsrb, _txen); + sbi(*_ucsrb, _rxcie); + cbi(*_ucsrb, _udrie); +} + +void HardwareSerial::begin(unsigned long baud, byte config) +{ + uint16_t baud_setting; + //uint8_t current_config; + bool use_u2x = true; + +#if F_CPU == 16000000UL + // hardcoded exception for compatibility with the bootloader shipped + // with the Duemilanove and previous boards and the firmware on the 8U2 + // on the Uno and Mega 2560. + if (baud == 57600) { + use_u2x = false; + } +#endif + +try_again: + + if (use_u2x) { + *_ucsra = 1 << _u2x; + baud_setting = (F_CPU / 4 / baud - 1) / 2; + } else { + *_ucsra = 0; + baud_setting = (F_CPU / 8 / baud - 1) / 2; + } + + if ((baud_setting > 4095) && use_u2x) + { + use_u2x = false; + goto try_again; + } + + // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) + *_ubrrh = baud_setting >> 8; + *_ubrrl = baud_setting; + + //set the data bits, parity, and stop bits +#if defined(__AVR_ATmega8__) + config |= 0x80; // select UCSRC register (shared with UBRRH) +#endif + *_ucsrc = config; + + sbi(*_ucsrb, _rxen); + sbi(*_ucsrb, _txen); + sbi(*_ucsrb, _rxcie); + cbi(*_ucsrb, _udrie); +} + +void HardwareSerial::end() +{ + // wait for transmission of outgoing data + while (_tx_buffer->head != _tx_buffer->tail) + ; + + cbi(*_ucsrb, _rxen); + cbi(*_ucsrb, _txen); + cbi(*_ucsrb, _rxcie); + cbi(*_ucsrb, _udrie); + + // clear any received data + _rx_buffer->head = _rx_buffer->tail; +} + +int HardwareSerial::available(void) +{ + return (unsigned int)(SERIAL_BUFFER_SIZE + _rx_buffer->head - _rx_buffer->tail) % SERIAL_BUFFER_SIZE; +} + +int HardwareSerial::peek(void) +{ + if (_rx_buffer->head == _rx_buffer->tail) { + return -1; + } else { + return _rx_buffer->buffer[_rx_buffer->tail]; + } +} + +int HardwareSerial::read(void) +{ + // if the head isn't ahead of the tail, we don't have any characters + if (_rx_buffer->head == _rx_buffer->tail) { + return -1; + } else { + unsigned char c = _rx_buffer->buffer[_rx_buffer->tail]; + _rx_buffer->tail = (unsigned int)(_rx_buffer->tail + 1) % SERIAL_BUFFER_SIZE; + return c; + } +} + +void HardwareSerial::flush() +{ + // UDR is kept full while the buffer is not empty, so TXC triggers when EMPTY && SENT + while (transmitting && ! (*_ucsra & _BV(TXC0))); + transmitting = false; +} + +size_t HardwareSerial::write(uint8_t c) +{ + int i = (_tx_buffer->head + 1) % SERIAL_BUFFER_SIZE; + + // If the output buffer is full, there's nothing for it other than to + // wait for the interrupt handler to empty it a bit + // ???: return 0 here instead? + while ((unsigned int)i == _tx_buffer->tail) + ; + + _tx_buffer->buffer[_tx_buffer->head] = c; + _tx_buffer->head = i; + + sbi(*_ucsrb, _udrie); + // clear the TXC bit -- "can be cleared by writing a one to its bit location" + transmitting = true; + sbi(*_ucsra, TXC0); + + return 1; +} + +HardwareSerial::operator bool() { + return true; +} + +// Preinstantiate Objects ////////////////////////////////////////////////////// + +#if defined(UBRRH) && defined(UBRRL) + HardwareSerial Serial(&rx_buffer, &tx_buffer, &UBRRH, &UBRRL, &UCSRA, &UCSRB, &UCSRC, &UDR, RXEN, TXEN, RXCIE, UDRIE, U2X); +#elif defined(UBRR0H) && defined(UBRR0L) + HardwareSerial Serial(&rx_buffer, &tx_buffer, &UBRR0H, &UBRR0L, &UCSR0A, &UCSR0B, &UCSR0C, &UDR0, RXEN0, TXEN0, RXCIE0, UDRIE0, U2X0); +#elif defined(USBCON) + // do nothing - Serial object and buffers are initialized in CDC code +#else + #error no serial port defined (port 0) +#endif + +#if defined(UBRR1H) + HardwareSerial Serial1(&rx_buffer1, &tx_buffer1, &UBRR1H, &UBRR1L, &UCSR1A, &UCSR1B, &UCSR1C, &UDR1, RXEN1, TXEN1, RXCIE1, UDRIE1, U2X1); +#endif +#if defined(UBRR2H) + HardwareSerial Serial2(&rx_buffer2, &tx_buffer2, &UBRR2H, &UBRR2L, &UCSR2A, &UCSR2B, &UCSR2C, &UDR2, RXEN2, TXEN2, RXCIE2, UDRIE2, U2X2); +#endif +#if defined(UBRR3H) + HardwareSerial Serial3(&rx_buffer3, &tx_buffer3, &UBRR3H, &UBRR3L, &UCSR3A, &UCSR3B, &UCSR3C, &UDR3, RXEN3, TXEN3, RXCIE3, UDRIE3, U2X3); +#endif + +#endif // whole file + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.h new file mode 100644 index 0000000..a73117f --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/HardwareSerial.h @@ -0,0 +1,115 @@ +/* + HardwareSerial.h - Hardware serial library for Wiring + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 28 September 2010 by Mark Sproul + Modified 14 August 2012 by Alarus +*/ + +#ifndef HardwareSerial_h +#define HardwareSerial_h + +#include + +#include "Stream.h" + +struct ring_buffer; + +class HardwareSerial : public Stream +{ + private: + ring_buffer *_rx_buffer; + ring_buffer *_tx_buffer; + volatile uint8_t *_ubrrh; + volatile uint8_t *_ubrrl; + volatile uint8_t *_ucsra; + volatile uint8_t *_ucsrb; + volatile uint8_t *_ucsrc; + volatile uint8_t *_udr; + uint8_t _rxen; + uint8_t _txen; + uint8_t _rxcie; + uint8_t _udrie; + uint8_t _u2x; + bool transmitting; + public: + HardwareSerial(ring_buffer *rx_buffer, ring_buffer *tx_buffer, + volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, + volatile uint8_t *ucsra, volatile uint8_t *ucsrb, + volatile uint8_t *ucsrc, volatile uint8_t *udr, + uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udrie, uint8_t u2x); + void begin(unsigned long); + void begin(unsigned long, uint8_t); + void end(); + virtual int available(void); + virtual int peek(void); + virtual int read(void); + virtual void flush(void); + virtual size_t write(uint8_t); + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + using Print::write; // pull in write(str) and write(buf, size) from Print + operator bool(); +}; + +// Define config for Serial.begin(baud, config); +#define SERIAL_5N1 0x00 +#define SERIAL_6N1 0x02 +#define SERIAL_7N1 0x04 +#define SERIAL_8N1 0x06 +#define SERIAL_5N2 0x08 +#define SERIAL_6N2 0x0A +#define SERIAL_7N2 0x0C +#define SERIAL_8N2 0x0E +#define SERIAL_5E1 0x20 +#define SERIAL_6E1 0x22 +#define SERIAL_7E1 0x24 +#define SERIAL_8E1 0x26 +#define SERIAL_5E2 0x28 +#define SERIAL_6E2 0x2A +#define SERIAL_7E2 0x2C +#define SERIAL_8E2 0x2E +#define SERIAL_5O1 0x30 +#define SERIAL_6O1 0x32 +#define SERIAL_7O1 0x34 +#define SERIAL_8O1 0x36 +#define SERIAL_5O2 0x38 +#define SERIAL_6O2 0x3A +#define SERIAL_7O2 0x3C +#define SERIAL_8O2 0x3E + +#if defined(UBRRH) || defined(UBRR0H) + extern HardwareSerial Serial; +#elif defined(USBCON) + #include "USBAPI.h" +// extern HardwareSerial Serial_; +#endif +#if defined(UBRR1H) + extern HardwareSerial Serial1; +#endif +#if defined(UBRR2H) + extern HardwareSerial Serial2; +#endif +#if defined(UBRR3H) + extern HardwareSerial Serial3; +#endif + +extern void serialEventRun(void) __attribute__((weak)); + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.cpp new file mode 100644 index 0000000..67c85ed --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.cpp @@ -0,0 +1,56 @@ + +#include +#include + +IPAddress::IPAddress() +{ + memset(_address.a8, 0, sizeof(_address)); +} + +IPAddress::IPAddress(uint8_t first_octet, uint8_t second_octet, uint8_t third_octet, uint8_t fourth_octet) +{ + _address.a8[0] = first_octet; + _address.a8[1] = second_octet; + _address.a8[2] = third_octet; + _address.a8[3] = fourth_octet; +} + +IPAddress::IPAddress(uint32_t address) +{ + _address.a32=address; +} + +IPAddress::IPAddress(const uint8_t *address) +{ + memcpy(_address.a8, address, sizeof(_address)); +} + +IPAddress& IPAddress::operator=(const uint8_t *address) +{ + memcpy(_address.a8, address, sizeof(_address)); + return *this; +} + +IPAddress& IPAddress::operator=(uint32_t address) +{ + _address.a32=address; + return *this; +} + +bool IPAddress::operator==(const uint8_t* addr) +{ + return memcmp(addr, _address.a8, sizeof(_address)) == 0; +} + +size_t IPAddress::printTo(Print& p) const +{ + size_t n = 0; + for (int i =0; i < 3; i++) + { + n += p.print(_address.a8[i], DEC); + n += p.print('.'); + } + n += p.print(_address.a8[3], DEC); + return n; +} + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.h new file mode 100644 index 0000000..c298abc --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/IPAddress.h @@ -0,0 +1,80 @@ +/* + * + * MIT License: + * Copyright (c) 2011 Adrian McEwen + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * adrianm@mcqn.com 1/1/2011 + */ + +#ifndef IPAddress_h +#define IPAddress_h + +#include + +// A class to make it easier to handle and pass around IP addresses + +class IPAddress : public Printable { +private: + union { + uint8_t a8[4]; // IPv4 address + uint32_t a32; + } _address; + + // Access the raw byte array containing the address. Because this returns a pointer + // to the internal structure rather than a copy of the address this function should only + // be used when you know that the usage of the returned uint8_t* will be transient and not + // stored. + uint8_t* raw_address() { return _address.a8; } + +public: + // Constructors + IPAddress(); + IPAddress(uint8_t first_octet, uint8_t second_octet, uint8_t third_octet, uint8_t fourth_octet); + IPAddress(uint32_t address); + IPAddress(const uint8_t *address); + + // Overloaded cast operator to allow IPAddress objects to be used where a pointer + // to a four-byte uint8_t array is expected + operator uint32_t() { return _address.a32; } + bool operator==(const IPAddress& addr) { return _address.a32 == addr._address.a32; }; + bool operator==(const uint8_t* addr); + + // Overloaded index operator to allow getting and setting individual octets of the address + uint8_t operator[](int index) const { return _address.a8[index]; }; + uint8_t& operator[](int index) { return _address.a8[index]; }; + + // Overloaded copy operators to allow initialisation of IPAddress objects from other types + IPAddress& operator=(const uint8_t *address); + IPAddress& operator=(uint32_t address); + + virtual size_t printTo(Print& p) const; + + friend class EthernetClass; + friend class UDP; + friend class Client; + friend class Server; + friend class DhcpClass; + friend class DNSClient; +}; + +const IPAddress INADDR_NONE(0,0,0,0); + + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Platform.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Platform.h new file mode 100644 index 0000000..8b8f742 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Platform.h @@ -0,0 +1,23 @@ + +#ifndef __PLATFORM_H__ +#define __PLATFORM_H__ + +#include +#include +#include +#include +#include + +typedef unsigned char u8; +typedef unsigned short u16; +typedef unsigned long u32; + +#include "Arduino.h" + +#if defined(USBCON) + #include "USBDesc.h" + #include "USBCore.h" + #include "USBAPI.h" +#endif /* if defined(USBCON) */ + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.cpp new file mode 100644 index 0000000..c7a4d76 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.cpp @@ -0,0 +1,268 @@ +/* + Print.cpp - Base class that provides print() and println() + Copyright (c) 2008 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 23 November 2006 by David A. Mellis + */ + +#include +#include +#include +#include +#include "Arduino.h" + +#include "Print.h" + +// Public Methods ////////////////////////////////////////////////////////////// + +/* default implementation: may be overridden */ +size_t Print::write(const uint8_t *buffer, size_t size) +{ + size_t n = 0; + while (size--) { + n += write(*buffer++); + } + return n; +} + +size_t Print::print(const __FlashStringHelper *ifsh) +{ + const char * __attribute__((progmem)) p = (const char * ) ifsh; + size_t n = 0; + while (1) { + unsigned char c = pgm_read_byte(p++); + if (c == 0) break; + n += write(c); + } + return n; +} + +size_t Print::print(const String &s) +{ + size_t n = 0; + for (uint16_t i = 0; i < s.length(); i++) { + n += write(s[i]); + } + return n; +} + +size_t Print::print(const char str[]) +{ + return write(str); +} + +size_t Print::print(char c) +{ + return write(c); +} + +size_t Print::print(unsigned char b, int base) +{ + return print((unsigned long) b, base); +} + +size_t Print::print(int n, int base) +{ + return print((long) n, base); +} + +size_t Print::print(unsigned int n, int base) +{ + return print((unsigned long) n, base); +} + +size_t Print::print(long n, int base) +{ + if (base == 0) { + return write(n); + } else if (base == 10) { + if (n < 0) { + int t = print('-'); + n = -n; + return printNumber(n, 10) + t; + } + return printNumber(n, 10); + } else { + return printNumber(n, base); + } +} + +size_t Print::print(unsigned long n, int base) +{ + if (base == 0) return write(n); + else return printNumber(n, base); +} + +size_t Print::print(double n, int digits) +{ + return printFloat(n, digits); +} + +size_t Print::println(const __FlashStringHelper *ifsh) +{ + size_t n = print(ifsh); + n += println(); + return n; +} + +size_t Print::print(const Printable& x) +{ + return x.printTo(*this); +} + +size_t Print::println(void) +{ + size_t n = print('\r'); + n += print('\n'); + return n; +} + +size_t Print::println(const String &s) +{ + size_t n = print(s); + n += println(); + return n; +} + +size_t Print::println(const char c[]) +{ + size_t n = print(c); + n += println(); + return n; +} + +size_t Print::println(char c) +{ + size_t n = print(c); + n += println(); + return n; +} + +size_t Print::println(unsigned char b, int base) +{ + size_t n = print(b, base); + n += println(); + return n; +} + +size_t Print::println(int num, int base) +{ + size_t n = print(num, base); + n += println(); + return n; +} + +size_t Print::println(unsigned int num, int base) +{ + size_t n = print(num, base); + n += println(); + return n; +} + +size_t Print::println(long num, int base) +{ + size_t n = print(num, base); + n += println(); + return n; +} + +size_t Print::println(unsigned long num, int base) +{ + size_t n = print(num, base); + n += println(); + return n; +} + +size_t Print::println(double num, int digits) +{ + size_t n = print(num, digits); + n += println(); + return n; +} + +size_t Print::println(const Printable& x) +{ + size_t n = print(x); + n += println(); + return n; +} + +// Private Methods ///////////////////////////////////////////////////////////// + +size_t Print::printNumber(unsigned long n, uint8_t base) { + char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte. + char *str = &buf[sizeof(buf) - 1]; + + *str = '\0'; + + // prevent crash if called with base == 1 + if (base < 2) base = 10; + + do { + unsigned long m = n; + n /= base; + char c = m - base * n; + *--str = c < 10 ? c + '0' : c + 'A' - 10; + } while(n); + + return write(str); +} + +size_t Print::printFloat(double number, uint8_t digits) +{ + size_t n = 0; + + if (isnan(number)) return print("nan"); + if (isinf(number)) return print("inf"); + if (number > 4294967040.0) return print ("ovf"); // constant determined empirically + if (number <-4294967040.0) return print ("ovf"); // constant determined empirically + + // Handle negative numbers + if (number < 0.0) + { + n += print('-'); + number = -number; + } + + // Round correctly so that print(1.999, 2) prints as "2.00" + double rounding = 0.5; + for (uint8_t i=0; i 0) { + n += print("."); + } + + // Extract digits from the remainder one at a time + while (digits-- > 0) + { + remainder *= 10.0; + int toPrint = int(remainder); + n += print(toPrint); + remainder -= toPrint; + } + + return n; +} diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.h new file mode 100644 index 0000000..dc76150 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Print.h @@ -0,0 +1,81 @@ +/* + Print.h - Base class that provides print() and println() + Copyright (c) 2008 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef Print_h +#define Print_h + +#include +#include // for size_t + +#include "WString.h" +#include "Printable.h" + +#define DEC 10 +#define HEX 16 +#define OCT 8 +#define BIN 2 + +class Print +{ + private: + int write_error; + size_t printNumber(unsigned long, uint8_t); + size_t printFloat(double, uint8_t); + protected: + void setWriteError(int err = 1) { write_error = err; } + public: + Print() : write_error(0) {} + + int getWriteError() { return write_error; } + void clearWriteError() { setWriteError(0); } + + virtual size_t write(uint8_t) = 0; + size_t write(const char *str) { + if (str == NULL) return 0; + return write((const uint8_t *)str, strlen(str)); + } + virtual size_t write(const uint8_t *buffer, size_t size); + + size_t print(const __FlashStringHelper *); + size_t print(const String &); + size_t print(const char[]); + size_t print(char); + size_t print(unsigned char, int = DEC); + size_t print(int, int = DEC); + size_t print(unsigned int, int = DEC); + size_t print(long, int = DEC); + size_t print(unsigned long, int = DEC); + size_t print(double, int = 2); + size_t print(const Printable&); + + size_t println(const __FlashStringHelper *); + size_t println(const String &s); + size_t println(const char[]); + size_t println(char); + size_t println(unsigned char, int = DEC); + size_t println(int, int = DEC); + size_t println(unsigned int, int = DEC); + size_t println(long, int = DEC); + size_t println(unsigned long, int = DEC); + size_t println(double, int = 2); + size_t println(const Printable&); + size_t println(void); +}; + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Printable.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Printable.h new file mode 100644 index 0000000..d03c9af --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Printable.h @@ -0,0 +1,40 @@ +/* + Printable.h - Interface class that allows printing of complex types + Copyright (c) 2011 Adrian McEwen. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef Printable_h +#define Printable_h + +#include + +class Print; + +/** The Printable class provides a way for new classes to allow themselves to be printed. + By deriving from Printable and implementing the printTo method, it will then be possible + for users to print out instances of this class by passing them into the usual + Print::print and Print::println methods. +*/ + +class Printable +{ + public: + virtual size_t printTo(Print& p) const = 0; +}; + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Server.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Server.h new file mode 100644 index 0000000..9674c76 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Server.h @@ -0,0 +1,9 @@ +#ifndef server_h +#define server_h + +class Server : public Print { +public: + virtual void begin() =0; +}; + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.cpp new file mode 100644 index 0000000..aafb7fc --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.cpp @@ -0,0 +1,270 @@ +/* + Stream.cpp - adds parsing methods to Stream class + Copyright (c) 2008 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Created July 2011 + parsing functions based on TextFinder library by Michael Margolis + */ + +#include "Arduino.h" +#include "Stream.h" + +#define PARSE_TIMEOUT 1000 // default number of milli-seconds to wait +#define NO_SKIP_CHAR 1 // a magic char not found in a valid ASCII numeric field + +// private method to read stream with timeout +int Stream::timedRead() +{ + int c; + _startMillis = millis(); + do { + c = read(); + if (c >= 0) return c; + } while(millis() - _startMillis < _timeout); + return -1; // -1 indicates timeout +} + +// private method to peek stream with timeout +int Stream::timedPeek() +{ + int c; + _startMillis = millis(); + do { + c = peek(); + if (c >= 0) return c; + } while(millis() - _startMillis < _timeout); + return -1; // -1 indicates timeout +} + +// returns peek of the next digit in the stream or -1 if timeout +// discards non-numeric characters +int Stream::peekNextDigit() +{ + int c; + while (1) { + c = timedPeek(); + if (c < 0) return c; // timeout + if (c == '-') return c; + if (c >= '0' && c <= '9') return c; + read(); // discard non-numeric + } +} + +// Public Methods +////////////////////////////////////////////////////////////// + +void Stream::setTimeout(unsigned long timeout) // sets the maximum number of milliseconds to wait +{ + _timeout = timeout; +} + + // find returns true if the target string is found +bool Stream::find(char *target) +{ + return findUntil(target, NULL); +} + +// reads data from the stream until the target string of given length is found +// returns true if target string is found, false if timed out +bool Stream::find(char *target, size_t length) +{ + return findUntil(target, length, NULL, 0); +} + +// as find but search ends if the terminator string is found +bool Stream::findUntil(char *target, char *terminator) +{ + return findUntil(target, strlen(target), terminator, strlen(terminator)); +} + +// reads data from the stream until the target string of the given length is found +// search terminated if the terminator string is found +// returns true if target string is found, false if terminated or timed out +bool Stream::findUntil(char *target, size_t targetLen, char *terminator, size_t termLen) +{ + size_t index = 0; // maximum target string length is 64k bytes! + size_t termIndex = 0; + int c; + + if( *target == 0) + return true; // return true if target is a null string + while( (c = timedRead()) > 0){ + + if(c != target[index]) + index = 0; // reset index if any char does not match + + if( c == target[index]){ + //////Serial.print("found "); Serial.write(c); Serial.print("index now"); Serial.println(index+1); + if(++index >= targetLen){ // return true if all chars in the target match + return true; + } + } + + if(termLen > 0 && c == terminator[termIndex]){ + if(++termIndex >= termLen) + return false; // return false if terminate string found before target string + } + else + termIndex = 0; + } + return false; +} + + +// returns the first valid (long) integer value from the current position. +// initial characters that are not digits (or the minus sign) are skipped +// function is terminated by the first character that is not a digit. +long Stream::parseInt() +{ + return parseInt(NO_SKIP_CHAR); // terminate on first non-digit character (or timeout) +} + +// as above but a given skipChar is ignored +// this allows format characters (typically commas) in values to be ignored +long Stream::parseInt(char skipChar) +{ + boolean isNegative = false; + long value = 0; + int c; + + c = peekNextDigit(); + // ignore non numeric leading characters + if(c < 0) + return 0; // zero returned if timeout + + do{ + if(c == skipChar) + ; // ignore this charactor + else if(c == '-') + isNegative = true; + else if(c >= '0' && c <= '9') // is c a digit? + value = value * 10 + c - '0'; + read(); // consume the character we got with peek + c = timedPeek(); + } + while( (c >= '0' && c <= '9') || c == skipChar ); + + if(isNegative) + value = -value; + return value; +} + + +// as parseInt but returns a floating point value +float Stream::parseFloat() +{ + return parseFloat(NO_SKIP_CHAR); +} + +// as above but the given skipChar is ignored +// this allows format characters (typically commas) in values to be ignored +float Stream::parseFloat(char skipChar){ + boolean isNegative = false; + boolean isFraction = false; + long value = 0; + char c; + float fraction = 1.0; + + c = peekNextDigit(); + // ignore non numeric leading characters + if(c < 0) + return 0; // zero returned if timeout + + do{ + if(c == skipChar) + ; // ignore + else if(c == '-') + isNegative = true; + else if (c == '.') + isFraction = true; + else if(c >= '0' && c <= '9') { // is c a digit? + value = value * 10 + c - '0'; + if(isFraction) + fraction *= 0.1; + } + read(); // consume the character we got with peek + c = timedPeek(); + } + while( (c >= '0' && c <= '9') || c == '.' || c == skipChar ); + + if(isNegative) + value = -value; + if(isFraction) + return value * fraction; + else + return value; +} + +// read characters from stream into buffer +// terminates if length characters have been read, or timeout (see setTimeout) +// returns the number of characters placed in the buffer +// the buffer is NOT null terminated. +// +size_t Stream::readBytes(char *buffer, size_t length) +{ + size_t count = 0; + while (count < length) { + int c = timedRead(); + if (c < 0) break; + *buffer++ = (char)c; + count++; + } + return count; +} + + +// as readBytes with terminator character +// terminates if length characters have been read, timeout, or if the terminator character detected +// returns the number of characters placed in the buffer (0 means no valid data found) + +size_t Stream::readBytesUntil(char terminator, char *buffer, size_t length) +{ + if (length < 1) return 0; + size_t index = 0; + while (index < length) { + int c = timedRead(); + if (c < 0 || c == terminator) break; + *buffer++ = (char)c; + index++; + } + return index; // return number of characters, not including null terminator +} + +String Stream::readString() +{ + String ret; + int c = timedRead(); + while (c >= 0) + { + ret += (char)c; + c = timedRead(); + } + return ret; +} + +String Stream::readStringUntil(char terminator) +{ + String ret; + int c = timedRead(); + while (c >= 0 && c != terminator) + { + ret += (char)c; + c = timedRead(); + } + return ret; +} + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.h new file mode 100644 index 0000000..58bbf75 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Stream.h @@ -0,0 +1,96 @@ +/* + Stream.h - base class for character-based streams. + Copyright (c) 2010 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + parsing functions based on TextFinder library by Michael Margolis +*/ + +#ifndef Stream_h +#define Stream_h + +#include +#include "Print.h" + +// compatability macros for testing +/* +#define getInt() parseInt() +#define getInt(skipChar) parseInt(skipchar) +#define getFloat() parseFloat() +#define getFloat(skipChar) parseFloat(skipChar) +#define getString( pre_string, post_string, buffer, length) +readBytesBetween( pre_string, terminator, buffer, length) +*/ + +class Stream : public Print +{ + private: + unsigned long _timeout; // number of milliseconds to wait for the next char before aborting timed read + unsigned long _startMillis; // used for timeout measurement + int timedRead(); // private method to read stream with timeout + int timedPeek(); // private method to peek stream with timeout + int peekNextDigit(); // returns the next numeric digit in the stream or -1 if timeout + + public: + virtual int available() = 0; + virtual int read() = 0; + virtual int peek() = 0; + virtual void flush() = 0; + + Stream() {_timeout=1000;} + +// parsing methods + + void setTimeout(unsigned long timeout); // sets maximum milliseconds to wait for stream data, default is 1 second + + bool find(char *target); // reads data from the stream until the target string is found + // returns true if target string is found, false if timed out (see setTimeout) + + bool find(char *target, size_t length); // reads data from the stream until the target string of given length is found + // returns true if target string is found, false if timed out + + bool findUntil(char *target, char *terminator); // as find but search ends if the terminator string is found + + bool findUntil(char *target, size_t targetLen, char *terminate, size_t termLen); // as above but search ends if the terminate string is found + + + long parseInt(); // returns the first valid (long) integer value from the current position. + // initial characters that are not digits (or the minus sign) are skipped + // integer is terminated by the first character that is not a digit. + + float parseFloat(); // float version of parseInt + + size_t readBytes( char *buffer, size_t length); // read chars from stream into buffer + // terminates if length characters have been read or timeout (see setTimeout) + // returns the number of characters placed in the buffer (0 means no valid data found) + + size_t readBytesUntil( char terminator, char *buffer, size_t length); // as readBytes with terminator character + // terminates if length characters have been read, timeout, or if the terminator character detected + // returns the number of characters placed in the buffer (0 means no valid data found) + + // Arduino String functions to be added here + String readString(); + String readStringUntil(char terminator); + + protected: + long parseInt(char skipChar); // as above but the given skipChar is ignored + // as above but the given skipChar is ignored + // this allows format characters (typically commas) in values to be ignored + + float parseFloat(char skipChar); // as above but the given skipChar is ignored +}; + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Tone.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/Tone.cpp new file mode 100644 index 0000000..9bb6fe7 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Tone.cpp @@ -0,0 +1,616 @@ +/* Tone.cpp + + A Tone Generator Library + + Written by Brett Hagman + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +Version Modified By Date Comments +------- ----------- -------- -------- +0001 B Hagman 09/08/02 Initial coding +0002 B Hagman 09/08/18 Multiple pins +0003 B Hagman 09/08/18 Moved initialization from constructor to begin() +0004 B Hagman 09/09/26 Fixed problems with ATmega8 +0005 B Hagman 09/11/23 Scanned prescalars for best fit on 8 bit timers + 09/11/25 Changed pin toggle method to XOR + 09/11/25 Fixed timer0 from being excluded +0006 D Mellis 09/12/29 Replaced objects with functions +0007 M Sproul 10/08/29 Changed #ifdefs from cpu to register +0008 S Kanemoto 12/06/22 Fixed for Leonardo by @maris_HY +*************************************************/ + +#include +#include +#include "Arduino.h" +#include "pins_arduino.h" + +#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) +#define TCCR2A TCCR2 +#define TCCR2B TCCR2 +#define COM2A1 COM21 +#define COM2A0 COM20 +#define OCR2A OCR2 +#define TIMSK2 TIMSK +#define OCIE2A OCIE2 +#define TIMER2_COMPA_vect TIMER2_COMP_vect +#define TIMSK1 TIMSK +#endif + +// timerx_toggle_count: +// > 0 - duration specified +// = 0 - stopped +// < 0 - infinitely (until stop() method called, or new play() called) + +#if !defined(__AVR_ATmega8__) +volatile long timer0_toggle_count; +volatile uint8_t *timer0_pin_port; +volatile uint8_t timer0_pin_mask; +#endif + +volatile long timer1_toggle_count; +volatile uint8_t *timer1_pin_port; +volatile uint8_t timer1_pin_mask; +volatile long timer2_toggle_count; +volatile uint8_t *timer2_pin_port; +volatile uint8_t timer2_pin_mask; + +#if defined(TIMSK3) +volatile long timer3_toggle_count; +volatile uint8_t *timer3_pin_port; +volatile uint8_t timer3_pin_mask; +#endif + +#if defined(TIMSK4) +volatile long timer4_toggle_count; +volatile uint8_t *timer4_pin_port; +volatile uint8_t timer4_pin_mask; +#endif + +#if defined(TIMSK5) +volatile long timer5_toggle_count; +volatile uint8_t *timer5_pin_port; +volatile uint8_t timer5_pin_mask; +#endif + + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + +#define AVAILABLE_TONE_PINS 1 +#define USE_TIMER2 + +const uint8_t PROGMEM tone_pin_to_timer_PGM[] = { 2 /*, 3, 4, 5, 1, 0 */ }; +static uint8_t tone_pins[AVAILABLE_TONE_PINS] = { 255 /*, 255, 255, 255, 255, 255 */ }; + +#elif defined(__AVR_ATmega8__) + +#define AVAILABLE_TONE_PINS 1 +#define USE_TIMER2 + +const uint8_t PROGMEM tone_pin_to_timer_PGM[] = { 2 /*, 1 */ }; +static uint8_t tone_pins[AVAILABLE_TONE_PINS] = { 255 /*, 255 */ }; + +#elif defined(__AVR_ATmega32U4__) + +#define AVAILABLE_TONE_PINS 1 +#define USE_TIMER3 + +const uint8_t PROGMEM tone_pin_to_timer_PGM[] = { 3 /*, 1 */ }; +static uint8_t tone_pins[AVAILABLE_TONE_PINS] = { 255 /*, 255 */ }; + +#else + +#define AVAILABLE_TONE_PINS 1 +#define USE_TIMER2 + +// Leave timer 0 to last. +const uint8_t PROGMEM tone_pin_to_timer_PGM[] = { 2 /*, 1, 0 */ }; +static uint8_t tone_pins[AVAILABLE_TONE_PINS] = { 255 /*, 255, 255 */ }; + +#endif + + + +static int8_t toneBegin(uint8_t _pin) +{ + int8_t _timer = -1; + + // if we're already using the pin, the timer should be configured. + for (int i = 0; i < AVAILABLE_TONE_PINS; i++) { + if (tone_pins[i] == _pin) { + return pgm_read_byte(tone_pin_to_timer_PGM + i); + } + } + + // search for an unused timer. + for (int i = 0; i < AVAILABLE_TONE_PINS; i++) { + if (tone_pins[i] == 255) { + tone_pins[i] = _pin; + _timer = pgm_read_byte(tone_pin_to_timer_PGM + i); + break; + } + } + + if (_timer != -1) + { + // Set timer specific stuff + // All timers in CTC mode + // 8 bit timers will require changing prescalar values, + // whereas 16 bit timers are set to either ck/1 or ck/64 prescalar + switch (_timer) + { + #if defined(TCCR0A) && defined(TCCR0B) + case 0: + // 8 bit timer + TCCR0A = 0; + TCCR0B = 0; + bitWrite(TCCR0A, WGM01, 1); + bitWrite(TCCR0B, CS00, 1); + timer0_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer0_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + + #if defined(TCCR1A) && defined(TCCR1B) && defined(WGM12) + case 1: + // 16 bit timer + TCCR1A = 0; + TCCR1B = 0; + bitWrite(TCCR1B, WGM12, 1); + bitWrite(TCCR1B, CS10, 1); + timer1_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer1_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + + #if defined(TCCR2A) && defined(TCCR2B) + case 2: + // 8 bit timer + TCCR2A = 0; + TCCR2B = 0; + bitWrite(TCCR2A, WGM21, 1); + bitWrite(TCCR2B, CS20, 1); + timer2_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer2_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + + #if defined(TCCR3A) && defined(TCCR3B) && defined(TIMSK3) + case 3: + // 16 bit timer + TCCR3A = 0; + TCCR3B = 0; + bitWrite(TCCR3B, WGM32, 1); + bitWrite(TCCR3B, CS30, 1); + timer3_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer3_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + + #if defined(TCCR4A) && defined(TCCR4B) && defined(TIMSK4) + case 4: + // 16 bit timer + TCCR4A = 0; + TCCR4B = 0; + #if defined(WGM42) + bitWrite(TCCR4B, WGM42, 1); + #elif defined(CS43) + #warning this may not be correct + // atmega32u4 + bitWrite(TCCR4B, CS43, 1); + #endif + bitWrite(TCCR4B, CS40, 1); + timer4_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer4_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + + #if defined(TCCR5A) && defined(TCCR5B) && defined(TIMSK5) + case 5: + // 16 bit timer + TCCR5A = 0; + TCCR5B = 0; + bitWrite(TCCR5B, WGM52, 1); + bitWrite(TCCR5B, CS50, 1); + timer5_pin_port = portOutputRegister(digitalPinToPort(_pin)); + timer5_pin_mask = digitalPinToBitMask(_pin); + break; + #endif + } + } + + return _timer; +} + + + +// frequency (in hertz) and duration (in milliseconds). + +void tone(uint8_t _pin, unsigned int frequency, unsigned long duration) +{ + uint8_t prescalarbits = 0b001; + long toggle_count = 0; + uint32_t ocr = 0; + int8_t _timer; + + _timer = toneBegin(_pin); + + if (_timer >= 0) + { + // Set the pinMode as OUTPUT + pinMode(_pin, OUTPUT); + + // if we are using an 8 bit timer, scan through prescalars to find the best fit + if (_timer == 0 || _timer == 2) + { + ocr = F_CPU / frequency / 2 - 1; + prescalarbits = 0b001; // ck/1: same for both timers + if (ocr > 255) + { + ocr = F_CPU / frequency / 2 / 8 - 1; + prescalarbits = 0b010; // ck/8: same for both timers + + if (_timer == 2 && ocr > 255) + { + ocr = F_CPU / frequency / 2 / 32 - 1; + prescalarbits = 0b011; + } + + if (ocr > 255) + { + ocr = F_CPU / frequency / 2 / 64 - 1; + prescalarbits = _timer == 0 ? 0b011 : 0b100; + + if (_timer == 2 && ocr > 255) + { + ocr = F_CPU / frequency / 2 / 128 - 1; + prescalarbits = 0b101; + } + + if (ocr > 255) + { + ocr = F_CPU / frequency / 2 / 256 - 1; + prescalarbits = _timer == 0 ? 0b100 : 0b110; + if (ocr > 255) + { + // can't do any better than /1024 + ocr = F_CPU / frequency / 2 / 1024 - 1; + prescalarbits = _timer == 0 ? 0b101 : 0b111; + } + } + } + } + +#if defined(TCCR0B) + if (_timer == 0) + { + TCCR0B = prescalarbits; + } + else +#endif +#if defined(TCCR2B) + { + TCCR2B = prescalarbits; + } +#else + { + // dummy place holder to make the above ifdefs work + } +#endif + } + else + { + // two choices for the 16 bit timers: ck/1 or ck/64 + ocr = F_CPU / frequency / 2 - 1; + + prescalarbits = 0b001; + if (ocr > 0xffff) + { + ocr = F_CPU / frequency / 2 / 64 - 1; + prescalarbits = 0b011; + } + + if (_timer == 1) + { +#if defined(TCCR1B) + TCCR1B = (TCCR1B & 0b11111000) | prescalarbits; +#endif + } +#if defined(TCCR3B) + else if (_timer == 3) + TCCR3B = (TCCR3B & 0b11111000) | prescalarbits; +#endif +#if defined(TCCR4B) + else if (_timer == 4) + TCCR4B = (TCCR4B & 0b11111000) | prescalarbits; +#endif +#if defined(TCCR5B) + else if (_timer == 5) + TCCR5B = (TCCR5B & 0b11111000) | prescalarbits; +#endif + + } + + + // Calculate the toggle count + if (duration > 0) + { + toggle_count = 2 * frequency * duration / 1000; + } + else + { + toggle_count = -1; + } + + // Set the OCR for the given timer, + // set the toggle count, + // then turn on the interrupts + switch (_timer) + { + +#if defined(OCR0A) && defined(TIMSK0) && defined(OCIE0A) + case 0: + OCR0A = ocr; + timer0_toggle_count = toggle_count; + bitWrite(TIMSK0, OCIE0A, 1); + break; +#endif + + case 1: +#if defined(OCR1A) && defined(TIMSK1) && defined(OCIE1A) + OCR1A = ocr; + timer1_toggle_count = toggle_count; + bitWrite(TIMSK1, OCIE1A, 1); +#elif defined(OCR1A) && defined(TIMSK) && defined(OCIE1A) + // this combination is for at least the ATmega32 + OCR1A = ocr; + timer1_toggle_count = toggle_count; + bitWrite(TIMSK, OCIE1A, 1); +#endif + break; + +#if defined(OCR2A) && defined(TIMSK2) && defined(OCIE2A) + case 2: + OCR2A = ocr; + timer2_toggle_count = toggle_count; + bitWrite(TIMSK2, OCIE2A, 1); + break; +#endif + +#if defined(TIMSK3) + case 3: + OCR3A = ocr; + timer3_toggle_count = toggle_count; + bitWrite(TIMSK3, OCIE3A, 1); + break; +#endif + +#if defined(TIMSK4) + case 4: + OCR4A = ocr; + timer4_toggle_count = toggle_count; + bitWrite(TIMSK4, OCIE4A, 1); + break; +#endif + +#if defined(OCR5A) && defined(TIMSK5) && defined(OCIE5A) + case 5: + OCR5A = ocr; + timer5_toggle_count = toggle_count; + bitWrite(TIMSK5, OCIE5A, 1); + break; +#endif + + } + } +} + + +// XXX: this function only works properly for timer 2 (the only one we use +// currently). for the others, it should end the tone, but won't restore +// proper PWM functionality for the timer. +void disableTimer(uint8_t _timer) +{ + switch (_timer) + { + case 0: + #if defined(TIMSK0) + TIMSK0 = 0; + #elif defined(TIMSK) + TIMSK = 0; // atmega32 + #endif + break; + +#if defined(TIMSK1) && defined(OCIE1A) + case 1: + bitWrite(TIMSK1, OCIE1A, 0); + break; +#endif + + case 2: + #if defined(TIMSK2) && defined(OCIE2A) + bitWrite(TIMSK2, OCIE2A, 0); // disable interrupt + #endif + #if defined(TCCR2A) && defined(WGM20) + TCCR2A = (1 << WGM20); + #endif + #if defined(TCCR2B) && defined(CS22) + TCCR2B = (TCCR2B & 0b11111000) | (1 << CS22); + #endif + #if defined(OCR2A) + OCR2A = 0; + #endif + break; + +#if defined(TIMSK3) + case 3: + TIMSK3 = 0; + break; +#endif + +#if defined(TIMSK4) + case 4: + TIMSK4 = 0; + break; +#endif + +#if defined(TIMSK5) + case 5: + TIMSK5 = 0; + break; +#endif + } +} + + +void noTone(uint8_t _pin) +{ + int8_t _timer = -1; + + for (int i = 0; i < AVAILABLE_TONE_PINS; i++) { + if (tone_pins[i] == _pin) { + _timer = pgm_read_byte(tone_pin_to_timer_PGM + i); + tone_pins[i] = 255; + } + } + + disableTimer(_timer); + + digitalWrite(_pin, 0); +} + +#ifdef USE_TIMER0 +ISR(TIMER0_COMPA_vect) +{ + if (timer0_toggle_count != 0) + { + // toggle the pin + *timer0_pin_port ^= timer0_pin_mask; + + if (timer0_toggle_count > 0) + timer0_toggle_count--; + } + else + { + disableTimer(0); + *timer0_pin_port &= ~(timer0_pin_mask); // keep pin low after stop + } +} +#endif + + +#ifdef USE_TIMER1 +ISR(TIMER1_COMPA_vect) +{ + if (timer1_toggle_count != 0) + { + // toggle the pin + *timer1_pin_port ^= timer1_pin_mask; + + if (timer1_toggle_count > 0) + timer1_toggle_count--; + } + else + { + disableTimer(1); + *timer1_pin_port &= ~(timer1_pin_mask); // keep pin low after stop + } +} +#endif + + +#ifdef USE_TIMER2 +ISR(TIMER2_COMPA_vect) +{ + + if (timer2_toggle_count != 0) + { + // toggle the pin + *timer2_pin_port ^= timer2_pin_mask; + + if (timer2_toggle_count > 0) + timer2_toggle_count--; + } + else + { + // need to call noTone() so that the tone_pins[] entry is reset, so the + // timer gets initialized next time we call tone(). + // XXX: this assumes timer 2 is always the first one used. + noTone(tone_pins[0]); +// disableTimer(2); +// *timer2_pin_port &= ~(timer2_pin_mask); // keep pin low after stop + } +} +#endif + + +#ifdef USE_TIMER3 +ISR(TIMER3_COMPA_vect) +{ + if (timer3_toggle_count != 0) + { + // toggle the pin + *timer3_pin_port ^= timer3_pin_mask; + + if (timer3_toggle_count > 0) + timer3_toggle_count--; + } + else + { + disableTimer(3); + *timer3_pin_port &= ~(timer3_pin_mask); // keep pin low after stop + } +} +#endif + + +#ifdef USE_TIMER4 +ISR(TIMER4_COMPA_vect) +{ + if (timer4_toggle_count != 0) + { + // toggle the pin + *timer4_pin_port ^= timer4_pin_mask; + + if (timer4_toggle_count > 0) + timer4_toggle_count--; + } + else + { + disableTimer(4); + *timer4_pin_port &= ~(timer4_pin_mask); // keep pin low after stop + } +} +#endif + + +#ifdef USE_TIMER5 +ISR(TIMER5_COMPA_vect) +{ + if (timer5_toggle_count != 0) + { + // toggle the pin + *timer5_pin_port ^= timer5_pin_mask; + + if (timer5_toggle_count > 0) + timer5_toggle_count--; + } + else + { + disableTimer(5); + *timer5_pin_port &= ~(timer5_pin_mask); // keep pin low after stop + } +} +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/USBAPI.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBAPI.h new file mode 100644 index 0000000..eb2e593 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBAPI.h @@ -0,0 +1,196 @@ + + +#ifndef __USBAPI__ +#define __USBAPI__ + +#if defined(USBCON) + +//================================================================================ +//================================================================================ +// USB + +class USBDevice_ +{ +public: + USBDevice_(); + bool configured(); + + void attach(); + void detach(); // Serial port goes down too... + void poll(); +}; +extern USBDevice_ USBDevice; + +//================================================================================ +//================================================================================ +// Serial over CDC (Serial1 is the physical port) + +class Serial_ : public Stream +{ +private: + ring_buffer *_cdc_rx_buffer; +public: + void begin(uint16_t baud_count); + void end(void); + + virtual int available(void); + virtual void accept(void); + virtual int peek(void); + virtual int read(void); + virtual void flush(void); + virtual size_t write(uint8_t); + using Print::write; // pull in write(str) and write(buf, size) from Print + operator bool(); +}; +extern Serial_ Serial; + +//================================================================================ +//================================================================================ +// Mouse + +#define MOUSE_LEFT 1 +#define MOUSE_RIGHT 2 +#define MOUSE_MIDDLE 4 +#define MOUSE_ALL (MOUSE_LEFT | MOUSE_RIGHT | MOUSE_MIDDLE) + +class Mouse_ +{ +private: + uint8_t _buttons; + void buttons(uint8_t b); +public: + Mouse_(void); + void begin(void); + void end(void); + void click(uint8_t b = MOUSE_LEFT); + void move(signed char x, signed char y, signed char wheel = 0); + void press(uint8_t b = MOUSE_LEFT); // press LEFT by default + void release(uint8_t b = MOUSE_LEFT); // release LEFT by default + bool isPressed(uint8_t b = MOUSE_LEFT); // check LEFT by default +}; +extern Mouse_ Mouse; + +//================================================================================ +//================================================================================ +// Keyboard + +#define KEY_LEFT_CTRL 0x80 +#define KEY_LEFT_SHIFT 0x81 +#define KEY_LEFT_ALT 0x82 +#define KEY_LEFT_GUI 0x83 +#define KEY_RIGHT_CTRL 0x84 +#define KEY_RIGHT_SHIFT 0x85 +#define KEY_RIGHT_ALT 0x86 +#define KEY_RIGHT_GUI 0x87 + +#define KEY_UP_ARROW 0xDA +#define KEY_DOWN_ARROW 0xD9 +#define KEY_LEFT_ARROW 0xD8 +#define KEY_RIGHT_ARROW 0xD7 +#define KEY_BACKSPACE 0xB2 +#define KEY_TAB 0xB3 +#define KEY_RETURN 0xB0 +#define KEY_ESC 0xB1 +#define KEY_INSERT 0xD1 +#define KEY_DELETE 0xD4 +#define KEY_PAGE_UP 0xD3 +#define KEY_PAGE_DOWN 0xD6 +#define KEY_HOME 0xD2 +#define KEY_END 0xD5 +#define KEY_CAPS_LOCK 0xC1 +#define KEY_F1 0xC2 +#define KEY_F2 0xC3 +#define KEY_F3 0xC4 +#define KEY_F4 0xC5 +#define KEY_F5 0xC6 +#define KEY_F6 0xC7 +#define KEY_F7 0xC8 +#define KEY_F8 0xC9 +#define KEY_F9 0xCA +#define KEY_F10 0xCB +#define KEY_F11 0xCC +#define KEY_F12 0xCD + +// Low level key report: up to 6 keys and shift, ctrl etc at once +typedef struct +{ + uint8_t modifiers; + uint8_t reserved; + uint8_t keys[6]; +} KeyReport; + +class Keyboard_ : public Print +{ +private: + KeyReport _keyReport; + void sendReport(KeyReport* keys); +public: + Keyboard_(void); + void begin(void); + void end(void); + virtual size_t write(uint8_t k); + virtual size_t press(uint8_t k); + virtual size_t release(uint8_t k); + virtual void releaseAll(void); +}; +extern Keyboard_ Keyboard; + +//================================================================================ +//================================================================================ +// Low level API + +typedef struct +{ + uint8_t bmRequestType; + uint8_t bRequest; + uint8_t wValueL; + uint8_t wValueH; + uint16_t wIndex; + uint16_t wLength; +} Setup; + +//================================================================================ +//================================================================================ +// HID 'Driver' + +int HID_GetInterface(uint8_t* interfaceNum); +int HID_GetDescriptor(int i); +bool HID_Setup(Setup& setup); +void HID_SendReport(uint8_t id, const void* data, int len); + +//================================================================================ +//================================================================================ +// MSC 'Driver' + +int MSC_GetInterface(uint8_t* interfaceNum); +int MSC_GetDescriptor(int i); +bool MSC_Setup(Setup& setup); +bool MSC_Data(uint8_t rx,uint8_t tx); + +//================================================================================ +//================================================================================ +// CSC 'Driver' + +int CDC_GetInterface(uint8_t* interfaceNum); +int CDC_GetDescriptor(int i); +bool CDC_Setup(Setup& setup); + +//================================================================================ +//================================================================================ + +#define TRANSFER_PGM 0x80 +#define TRANSFER_RELEASE 0x40 +#define TRANSFER_ZERO 0x20 + +int USB_SendControl(uint8_t flags, const void* d, int len); +int USB_RecvControl(void* d, int len); + +uint8_t USB_Available(uint8_t ep); +int USB_Send(uint8_t ep, const void* data, int len); // blocking +int USB_Recv(uint8_t ep, void* data, int len); // non-blocking +int USB_Recv(uint8_t ep); // non-blocking +void USB_Flush(uint8_t ep); + +#endif + +#endif /* if defined(USBCON) */ \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/USBCore.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBCore.cpp new file mode 100644 index 0000000..d3e0170 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBCore.cpp @@ -0,0 +1,684 @@ + + +/* Copyright (c) 2010, Peter Barrett +** +** Permission to use, copy, modify, and/or distribute this software for +** any purpose with or without fee is hereby granted, provided that the +** above copyright notice and this permission notice appear in all copies. +** +** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL +** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED +** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR +** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES +** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, +** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, +** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS +** SOFTWARE. +*/ + +#include "Platform.h" +#include "USBAPI.h" +#include "USBDesc.h" + +#if defined(USBCON) + +#define EP_TYPE_CONTROL 0x00 +#define EP_TYPE_BULK_IN 0x81 +#define EP_TYPE_BULK_OUT 0x80 +#define EP_TYPE_INTERRUPT_IN 0xC1 +#define EP_TYPE_INTERRUPT_OUT 0xC0 +#define EP_TYPE_ISOCHRONOUS_IN 0x41 +#define EP_TYPE_ISOCHRONOUS_OUT 0x40 + +/** Pulse generation counters to keep track of the number of milliseconds remaining for each pulse type */ +#define TX_RX_LED_PULSE_MS 100 +volatile u8 TxLEDPulse; /**< Milliseconds remaining for data Tx LED pulse */ +volatile u8 RxLEDPulse; /**< Milliseconds remaining for data Rx LED pulse */ + +//================================================================== +//================================================================== + +extern const u16 STRING_LANGUAGE[] PROGMEM; +extern const u16 STRING_IPRODUCT[] PROGMEM; +extern const u16 STRING_IMANUFACTURER[] PROGMEM; +extern const DeviceDescriptor USB_DeviceDescriptor PROGMEM; +extern const DeviceDescriptor USB_DeviceDescriptorA PROGMEM; + +const u16 STRING_LANGUAGE[2] = { + (3<<8) | (2+2), + 0x0409 // English +}; + +const u16 STRING_IPRODUCT[17] = { + (3<<8) | (2+2*16), +#if USB_PID == 0x8036 + 'A','r','d','u','i','n','o',' ','L','e','o','n','a','r','d','o' +#elif USB_PID == 0x8037 + 'A','r','d','u','i','n','o',' ','M','i','c','r','o',' ',' ',' ' +#elif USB_PID == 0x803C + 'A','r','d','u','i','n','o',' ','E','s','p','l','o','r','a',' ' +#elif USB_PID == 0x9208 + 'L','i','l','y','P','a','d','U','S','B',' ',' ',' ',' ',' ',' ' +#else + 'U','S','B',' ','I','O',' ','B','o','a','r','d',' ',' ',' ',' ' +#endif +}; + +const u16 STRING_IMANUFACTURER[12] = { + (3<<8) | (2+2*11), +#if USB_VID == 0x2341 + 'A','r','d','u','i','n','o',' ','L','L','C' +#elif USB_VID == 0x1b4f + 'S','p','a','r','k','F','u','n',' ',' ',' ' +#else + 'U','n','k','n','o','w','n',' ',' ',' ',' ' +#endif +}; + +#ifdef CDC_ENABLED +#define DEVICE_CLASS 0x02 +#else +#define DEVICE_CLASS 0x00 +#endif + +// DEVICE DESCRIPTOR +const DeviceDescriptor USB_DeviceDescriptor = + D_DEVICE(0x00,0x00,0x00,64,USB_VID,USB_PID,0x100,IMANUFACTURER,IPRODUCT,0,1); + +const DeviceDescriptor USB_DeviceDescriptorA = + D_DEVICE(DEVICE_CLASS,0x00,0x00,64,USB_VID,USB_PID,0x100,IMANUFACTURER,IPRODUCT,0,1); + +//================================================================== +//================================================================== + +volatile u8 _usbConfiguration = 0; + +static inline void WaitIN(void) +{ + while (!(UEINTX & (1< len) + n = len; + len -= n; + { + LockEP lock(ep); + if (ep & TRANSFER_ZERO) + { + while (n--) + Send8(0); + } + else if (ep & TRANSFER_PGM) + { + while (n--) + Send8(pgm_read_byte(data++)); + } + else + { + while (n--) + Send8(*data++); + } + if (!ReadWriteAllowed() || ((len == 0) && (ep & TRANSFER_RELEASE))) // Release full buffer + ReleaseTX(); + } + } + TXLED1; // light the TX LED + TxLEDPulse = TX_RX_LED_PULSE_MS; + return r; +} + +extern const u8 _initEndpoints[] PROGMEM; +const u8 _initEndpoints[] = +{ + 0, + +#ifdef CDC_ENABLED + EP_TYPE_INTERRUPT_IN, // CDC_ENDPOINT_ACM + EP_TYPE_BULK_OUT, // CDC_ENDPOINT_OUT + EP_TYPE_BULK_IN, // CDC_ENDPOINT_IN +#endif + +#ifdef HID_ENABLED + EP_TYPE_INTERRUPT_IN // HID_ENDPOINT_INT +#endif +}; + +#define EP_SINGLE_64 0x32 // EP0 +#define EP_DOUBLE_64 0x36 // Other endpoints + +static +void InitEP(u8 index, u8 type, u8 size) +{ + UENUM = index; + UECONX = 1; + UECFG0X = type; + UECFG1X = size; +} + +static +void InitEndpoints() +{ + for (u8 i = 1; i < sizeof(_initEndpoints); i++) + { + UENUM = i; + UECONX = 1; + UECFG0X = pgm_read_byte(_initEndpoints+i); + UECFG1X = EP_DOUBLE_64; + } + UERST = 0x7E; // And reset them + UERST = 0; +} + +// Handle CLASS_INTERFACE requests +static +bool ClassInterfaceRequest(Setup& setup) +{ + u8 i = setup.wIndex; + +#ifdef CDC_ENABLED + if (CDC_ACM_INTERFACE == i) + return CDC_Setup(setup); +#endif + +#ifdef HID_ENABLED + if (HID_INTERFACE == i) + return HID_Setup(setup); +#endif + return false; +} + +int _cmark; +int _cend; +void InitControl(int end) +{ + SetEP(0); + _cmark = 0; + _cend = end; +} + +static +bool SendControl(u8 d) +{ + if (_cmark < _cend) + { + if (!WaitForINOrOUT()) + return false; + Send8(d); + if (!((_cmark + 1) & 0x3F)) + ClearIN(); // Fifo is full, release this packet + } + _cmark++; + return true; +}; + +// Clipped by _cmark/_cend +int USB_SendControl(u8 flags, const void* d, int len) +{ + int sent = len; + const u8* data = (const u8*)d; + bool pgm = flags & TRANSFER_PGM; + while (len--) + { + u8 c = pgm ? pgm_read_byte(data++) : *data++; + if (!SendControl(c)) + return -1; + } + return sent; +} + +// Does not timeout or cross fifo boundaries +// Will only work for transfers <= 64 bytes +// TODO +int USB_RecvControl(void* d, int len) +{ + WaitOUT(); + Recv((u8*)d,len); + ClearOUT(); + return len; +} + +int SendInterfaces() +{ + int total = 0; + u8 interfaces = 0; + +#ifdef CDC_ENABLED + total = CDC_GetInterface(&interfaces); +#endif + +#ifdef HID_ENABLED + total += HID_GetInterface(&interfaces); +#endif + + return interfaces; +} + +// Construct a dynamic configuration descriptor +// This really needs dynamic endpoint allocation etc +// TODO +static +bool SendConfiguration(int maxlen) +{ + // Count and measure interfaces + InitControl(0); + int interfaces = SendInterfaces(); + ConfigDescriptor config = D_CONFIG(_cmark + sizeof(ConfigDescriptor),interfaces); + + // Now send them + InitControl(maxlen); + USB_SendControl(0,&config,sizeof(ConfigDescriptor)); + SendInterfaces(); + return true; +} + +u8 _cdcComposite = 0; + +static +bool SendDescriptor(Setup& setup) +{ + u8 t = setup.wValueH; + if (USB_CONFIGURATION_DESCRIPTOR_TYPE == t) + return SendConfiguration(setup.wLength); + + InitControl(setup.wLength); +#ifdef HID_ENABLED + if (HID_REPORT_DESCRIPTOR_TYPE == t) + return HID_GetDescriptor(t); +#endif + + u8 desc_length = 0; + const u8* desc_addr = 0; + if (USB_DEVICE_DESCRIPTOR_TYPE == t) + { + if (setup.wLength == 8) + _cdcComposite = 1; + desc_addr = _cdcComposite ? (const u8*)&USB_DeviceDescriptorA : (const u8*)&USB_DeviceDescriptor; + } + else if (USB_STRING_DESCRIPTOR_TYPE == t) + { + if (setup.wValueL == 0) + desc_addr = (const u8*)&STRING_LANGUAGE; + else if (setup.wValueL == IPRODUCT) + desc_addr = (const u8*)&STRING_IPRODUCT; + else if (setup.wValueL == IMANUFACTURER) + desc_addr = (const u8*)&STRING_IMANUFACTURER; + else + return false; + } + + if (desc_addr == 0) + return false; + if (desc_length == 0) + desc_length = pgm_read_byte(desc_addr); + + USB_SendControl(TRANSFER_PGM,desc_addr,desc_length); + return true; +} + +// Endpoint 0 interrupt +ISR(USB_COM_vect) +{ + SetEP(0); + if (!ReceivedSetupInt()) + return; + + Setup setup; + Recv((u8*)&setup,8); + ClearSetupInt(); + + u8 requestType = setup.bmRequestType; + if (requestType & REQUEST_DEVICETOHOST) + WaitIN(); + else + ClearIN(); + + bool ok = true; + if (REQUEST_STANDARD == (requestType & REQUEST_TYPE)) + { + // Standard Requests + u8 r = setup.bRequest; + if (GET_STATUS == r) + { + Send8(0); // TODO + Send8(0); + } + else if (CLEAR_FEATURE == r) + { + } + else if (SET_FEATURE == r) + { + } + else if (SET_ADDRESS == r) + { + WaitIN(); + UDADDR = setup.wValueL | (1<> 8) & 0xFF) + +#define CDC_V1_10 0x0110 +#define CDC_COMMUNICATION_INTERFACE_CLASS 0x02 + +#define CDC_CALL_MANAGEMENT 0x01 +#define CDC_ABSTRACT_CONTROL_MODEL 0x02 +#define CDC_HEADER 0x00 +#define CDC_ABSTRACT_CONTROL_MANAGEMENT 0x02 +#define CDC_UNION 0x06 +#define CDC_CS_INTERFACE 0x24 +#define CDC_CS_ENDPOINT 0x25 +#define CDC_DATA_INTERFACE_CLASS 0x0A + +#define MSC_SUBCLASS_SCSI 0x06 +#define MSC_PROTOCOL_BULK_ONLY 0x50 + +#define HID_HID_DESCRIPTOR_TYPE 0x21 +#define HID_REPORT_DESCRIPTOR_TYPE 0x22 +#define HID_PHYSICAL_DESCRIPTOR_TYPE 0x23 + + +// Device +typedef struct { + u8 len; // 18 + u8 dtype; // 1 USB_DEVICE_DESCRIPTOR_TYPE + u16 usbVersion; // 0x200 + u8 deviceClass; + u8 deviceSubClass; + u8 deviceProtocol; + u8 packetSize0; // Packet 0 + u16 idVendor; + u16 idProduct; + u16 deviceVersion; // 0x100 + u8 iManufacturer; + u8 iProduct; + u8 iSerialNumber; + u8 bNumConfigurations; +} DeviceDescriptor; + +// Config +typedef struct { + u8 len; // 9 + u8 dtype; // 2 + u16 clen; // total length + u8 numInterfaces; + u8 config; + u8 iconfig; + u8 attributes; + u8 maxPower; +} ConfigDescriptor; + +// String + +// Interface +typedef struct +{ + u8 len; // 9 + u8 dtype; // 4 + u8 number; + u8 alternate; + u8 numEndpoints; + u8 interfaceClass; + u8 interfaceSubClass; + u8 protocol; + u8 iInterface; +} InterfaceDescriptor; + +// Endpoint +typedef struct +{ + u8 len; // 7 + u8 dtype; // 5 + u8 addr; + u8 attr; + u16 packetSize; + u8 interval; +} EndpointDescriptor; + +// Interface Association Descriptor +// Used to bind 2 interfaces together in CDC compostite device +typedef struct +{ + u8 len; // 8 + u8 dtype; // 11 + u8 firstInterface; + u8 interfaceCount; + u8 functionClass; + u8 funtionSubClass; + u8 functionProtocol; + u8 iInterface; +} IADDescriptor; + +// CDC CS interface descriptor +typedef struct +{ + u8 len; // 5 + u8 dtype; // 0x24 + u8 subtype; + u8 d0; + u8 d1; +} CDCCSInterfaceDescriptor; + +typedef struct +{ + u8 len; // 4 + u8 dtype; // 0x24 + u8 subtype; + u8 d0; +} CDCCSInterfaceDescriptor4; + +typedef struct +{ + u8 len; + u8 dtype; // 0x24 + u8 subtype; // 1 + u8 bmCapabilities; + u8 bDataInterface; +} CMFunctionalDescriptor; + +typedef struct +{ + u8 len; + u8 dtype; // 0x24 + u8 subtype; // 1 + u8 bmCapabilities; +} ACMFunctionalDescriptor; + +typedef struct +{ + // IAD + IADDescriptor iad; // Only needed on compound device + + // Control + InterfaceDescriptor cif; // + CDCCSInterfaceDescriptor header; + CMFunctionalDescriptor callManagement; // Call Management + ACMFunctionalDescriptor controlManagement; // ACM + CDCCSInterfaceDescriptor functionalDescriptor; // CDC_UNION + EndpointDescriptor cifin; + + // Data + InterfaceDescriptor dif; + EndpointDescriptor in; + EndpointDescriptor out; +} CDCDescriptor; + +typedef struct +{ + InterfaceDescriptor msc; + EndpointDescriptor in; + EndpointDescriptor out; +} MSCDescriptor; + +typedef struct +{ + u8 len; // 9 + u8 dtype; // 0x21 + u8 addr; + u8 versionL; // 0x101 + u8 versionH; // 0x101 + u8 country; + u8 desctype; // 0x22 report + u8 descLenL; + u8 descLenH; +} HIDDescDescriptor; + +typedef struct +{ + InterfaceDescriptor hid; + HIDDescDescriptor desc; + EndpointDescriptor in; +} HIDDescriptor; + + +#define D_DEVICE(_class,_subClass,_proto,_packetSize0,_vid,_pid,_version,_im,_ip,_is,_configs) \ + { 18, 1, 0x200, _class,_subClass,_proto,_packetSize0,_vid,_pid,_version,_im,_ip,_is,_configs } + +#define D_CONFIG(_totalLength,_interfaces) \ + { 9, 2, _totalLength,_interfaces, 1, 0, USB_CONFIG_BUS_POWERED, USB_CONFIG_POWER_MA(500) } + +#define D_INTERFACE(_n,_numEndpoints,_class,_subClass,_protocol) \ + { 9, 4, _n, 0, _numEndpoints, _class,_subClass, _protocol, 0 } + +#define D_ENDPOINT(_addr,_attr,_packetSize, _interval) \ + { 7, 5, _addr,_attr,_packetSize, _interval } + +#define D_IAD(_firstInterface, _count, _class, _subClass, _protocol) \ + { 8, 11, _firstInterface, _count, _class, _subClass, _protocol, 0 } + +#define D_HIDREPORT(_descriptorLength) \ + { 9, 0x21, 0x1, 0x1, 0, 1, 0x22, _descriptorLength, 0 } + +#define D_CDCCS(_subtype,_d0,_d1) { 5, 0x24, _subtype, _d0, _d1 } +#define D_CDCCS4(_subtype,_d0) { 4, 0x24, _subtype, _d0 } + + +#endif \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/USBDesc.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBDesc.h new file mode 100644 index 0000000..900713e --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/USBDesc.h @@ -0,0 +1,63 @@ + + +/* Copyright (c) 2011, Peter Barrett +** +** Permission to use, copy, modify, and/or distribute this software for +** any purpose with or without fee is hereby granted, provided that the +** above copyright notice and this permission notice appear in all copies. +** +** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL +** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED +** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR +** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES +** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, +** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, +** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS +** SOFTWARE. +*/ + +#define CDC_ENABLED +#define HID_ENABLED + + +#ifdef CDC_ENABLED +#define CDC_INTERFACE_COUNT 2 +#define CDC_ENPOINT_COUNT 3 +#else +#define CDC_INTERFACE_COUNT 0 +#define CDC_ENPOINT_COUNT 0 +#endif + +#ifdef HID_ENABLED +#define HID_INTERFACE_COUNT 1 +#define HID_ENPOINT_COUNT 1 +#else +#define HID_INTERFACE_COUNT 0 +#define HID_ENPOINT_COUNT 0 +#endif + +#define CDC_ACM_INTERFACE 0 // CDC ACM +#define CDC_DATA_INTERFACE 1 // CDC Data +#define CDC_FIRST_ENDPOINT 1 +#define CDC_ENDPOINT_ACM (CDC_FIRST_ENDPOINT) // CDC First +#define CDC_ENDPOINT_OUT (CDC_FIRST_ENDPOINT+1) +#define CDC_ENDPOINT_IN (CDC_FIRST_ENDPOINT+2) + +#define HID_INTERFACE (CDC_ACM_INTERFACE + CDC_INTERFACE_COUNT) // HID Interface +#define HID_FIRST_ENDPOINT (CDC_FIRST_ENDPOINT + CDC_ENPOINT_COUNT) +#define HID_ENDPOINT_INT (HID_FIRST_ENDPOINT) + +#define INTERFACE_COUNT (MSC_INTERFACE + MSC_INTERFACE_COUNT) + +#ifdef CDC_ENABLED +#define CDC_RX CDC_ENDPOINT_OUT +#define CDC_TX CDC_ENDPOINT_IN +#endif + +#ifdef HID_ENABLED +#define HID_TX HID_ENDPOINT_INT +#endif + +#define IMANUFACTURER 1 +#define IPRODUCT 2 + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/Udp.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/Udp.h new file mode 100644 index 0000000..dc5644b --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/Udp.h @@ -0,0 +1,88 @@ +/* + * Udp.cpp: Library to send/receive UDP packets. + * + * NOTE: UDP is fast, but has some important limitations (thanks to Warren Gray for mentioning these) + * 1) UDP does not guarantee the order in which assembled UDP packets are received. This + * might not happen often in practice, but in larger network topologies, a UDP + * packet can be received out of sequence. + * 2) UDP does not guard against lost packets - so packets *can* disappear without the sender being + * aware of it. Again, this may not be a concern in practice on small local networks. + * For more information, see http://www.cafeaulait.org/course/week12/35.html + * + * MIT License: + * Copyright (c) 2008 Bjoern Hartmann + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * bjoern@cs.stanford.edu 12/30/2008 + */ + +#ifndef udp_h +#define udp_h + +#include +#include + +class UDP : public Stream { + +public: + virtual uint8_t begin(uint16_t) =0; // initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use + virtual void stop() =0; // Finish with the UDP socket + + // Sending UDP packets + + // Start building up a packet to send to the remote host specific in ip and port + // Returns 1 if successful, 0 if there was a problem with the supplied IP address or port + virtual int beginPacket(IPAddress ip, uint16_t port) =0; + // Start building up a packet to send to the remote host specific in host and port + // Returns 1 if successful, 0 if there was a problem resolving the hostname or port + virtual int beginPacket(const char *host, uint16_t port) =0; + // Finish off this packet and send it + // Returns 1 if the packet was sent successfully, 0 if there was an error + virtual int endPacket() =0; + // Write a single byte into the packet + virtual size_t write(uint8_t) =0; + // Write size bytes from buffer into the packet + virtual size_t write(const uint8_t *buffer, size_t size) =0; + + // Start processing the next available incoming packet + // Returns the size of the packet in bytes, or 0 if no packets are available + virtual int parsePacket() =0; + // Number of bytes remaining in the current packet + virtual int available() =0; + // Read a single byte from the current packet + virtual int read() =0; + // Read up to len bytes from the current packet and place them into buffer + // Returns the number of bytes read, or 0 if none are available + virtual int read(unsigned char* buffer, size_t len) =0; + // Read up to len characters from the current packet and place them into buffer + // Returns the number of characters read, or 0 if none are available + virtual int read(char* buffer, size_t len) =0; + // Return the next byte from the current packet without moving on to the next byte + virtual int peek() =0; + virtual void flush() =0; // Finish reading the current packet + + // Return the IP address of the host who sent the current incoming packet + virtual IPAddress remoteIP() =0; + // Return the port of the host who sent the current incoming packet + virtual uint16_t remotePort() =0; +protected: + uint8_t* rawIPAddress(IPAddress& addr) { return addr.raw_address(); }; +}; + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/WCharacter.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/WCharacter.h new file mode 100644 index 0000000..79733b5 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/WCharacter.h @@ -0,0 +1,168 @@ +/* + WCharacter.h - Character utility functions for Wiring & Arduino + Copyright (c) 2010 Hernando Barragan. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#ifndef Character_h +#define Character_h + +#include + +// WCharacter.h prototypes +inline boolean isAlphaNumeric(int c) __attribute__((always_inline)); +inline boolean isAlpha(int c) __attribute__((always_inline)); +inline boolean isAscii(int c) __attribute__((always_inline)); +inline boolean isWhitespace(int c) __attribute__((always_inline)); +inline boolean isControl(int c) __attribute__((always_inline)); +inline boolean isDigit(int c) __attribute__((always_inline)); +inline boolean isGraph(int c) __attribute__((always_inline)); +inline boolean isLowerCase(int c) __attribute__((always_inline)); +inline boolean isPrintable(int c) __attribute__((always_inline)); +inline boolean isPunct(int c) __attribute__((always_inline)); +inline boolean isSpace(int c) __attribute__((always_inline)); +inline boolean isUpperCase(int c) __attribute__((always_inline)); +inline boolean isHexadecimalDigit(int c) __attribute__((always_inline)); +inline int toAscii(int c) __attribute__((always_inline)); +inline int toLowerCase(int c) __attribute__((always_inline)); +inline int toUpperCase(int c)__attribute__((always_inline)); + + +// Checks for an alphanumeric character. +// It is equivalent to (isalpha(c) || isdigit(c)). +inline boolean isAlphaNumeric(int c) +{ + return ( isalnum(c) == 0 ? false : true); +} + + +// Checks for an alphabetic character. +// It is equivalent to (isupper(c) || islower(c)). +inline boolean isAlpha(int c) +{ + return ( isalpha(c) == 0 ? false : true); +} + + +// Checks whether c is a 7-bit unsigned char value +// that fits into the ASCII character set. +inline boolean isAscii(int c) +{ + return ( isascii (c) == 0 ? false : true); +} + + +// Checks for a blank character, that is, a space or a tab. +inline boolean isWhitespace(int c) +{ + return ( isblank (c) == 0 ? false : true); +} + + +// Checks for a control character. +inline boolean isControl(int c) +{ + return ( iscntrl (c) == 0 ? false : true); +} + + +// Checks for a digit (0 through 9). +inline boolean isDigit(int c) +{ + return ( isdigit (c) == 0 ? false : true); +} + + +// Checks for any printable character except space. +inline boolean isGraph(int c) +{ + return ( isgraph (c) == 0 ? false : true); +} + + +// Checks for a lower-case character. +inline boolean isLowerCase(int c) +{ + return (islower (c) == 0 ? false : true); +} + + +// Checks for any printable character including space. +inline boolean isPrintable(int c) +{ + return ( isprint (c) == 0 ? false : true); +} + + +// Checks for any printable character which is not a space +// or an alphanumeric character. +inline boolean isPunct(int c) +{ + return ( ispunct (c) == 0 ? false : true); +} + + +// Checks for white-space characters. For the avr-libc library, +// these are: space, formfeed ('\f'), newline ('\n'), carriage +// return ('\r'), horizontal tab ('\t'), and vertical tab ('\v'). +inline boolean isSpace(int c) +{ + return ( isspace (c) == 0 ? false : true); +} + + +// Checks for an uppercase letter. +inline boolean isUpperCase(int c) +{ + return ( isupper (c) == 0 ? false : true); +} + + +// Checks for a hexadecimal digits, i.e. one of 0 1 2 3 4 5 6 7 +// 8 9 a b c d e f A B C D E F. +inline boolean isHexadecimalDigit(int c) +{ + return ( isxdigit (c) == 0 ? false : true); +} + + +// Converts c to a 7-bit unsigned char value that fits into the +// ASCII character set, by clearing the high-order bits. +inline int toAscii(int c) +{ + return toascii (c); +} + + +// Warning: +// Many people will be unhappy if you use this function. +// This function will convert accented letters into random +// characters. + +// Converts the letter c to lower case, if possible. +inline int toLowerCase(int c) +{ + return tolower (c); +} + + +// Converts the letter c to upper case, if possible. +inline int toUpperCase(int c) +{ + return toupper (c); +} + +#endif \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/WInterrupts.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/WInterrupts.c new file mode 100644 index 0000000..62efc9c --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/WInterrupts.c @@ -0,0 +1,322 @@ +/* -*- mode: jde; c-basic-offset: 2; indent-tabs-mode: nil -*- */ + +/* + Part of the Wiring project - http://wiring.uniandes.edu.co + + Copyright (c) 2004-05 Hernando Barragan + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + Modified 24 November 2006 by David A. Mellis + Modified 1 August 2010 by Mark Sproul +*/ + +#include +#include +#include +#include +#include + +#include "wiring_private.h" + +static volatile voidFuncPtr intFunc[EXTERNAL_NUM_INTERRUPTS]; +// volatile static voidFuncPtr twiIntFunc; + +void attachInterrupt(uint8_t interruptNum, void (*userFunc)(void), int mode) { + if(interruptNum < EXTERNAL_NUM_INTERRUPTS) { + intFunc[interruptNum] = userFunc; + + // Configure the interrupt mode (trigger on low input, any change, rising + // edge, or falling edge). The mode constants were chosen to correspond + // to the configuration bits in the hardware register, so we simply shift + // the mode into place. + + // Enable the interrupt. + + switch (interruptNum) { +#if defined(__AVR_ATmega32U4__) + // I hate doing this, but the register assignment differs between the 1280/2560 + // and the 32U4. Since avrlib defines registers PCMSK1 and PCMSK2 that aren't + // even present on the 32U4 this is the only way to distinguish between them. + case 0: + EICRA = (EICRA & ~((1<= howbig) { + return howsmall; + } + long diff = howbig - howsmall; + return random(diff) + howsmall; +} + +long map(long x, long in_min, long in_max, long out_min, long out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +unsigned int makeWord(unsigned int w) { return w; } +unsigned int makeWord(unsigned char h, unsigned char l) { return (h << 8) | l; } \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.cpp new file mode 100644 index 0000000..c6839fc --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.cpp @@ -0,0 +1,645 @@ +/* + WString.cpp - String library for Wiring & Arduino + ...mostly rewritten by Paul Stoffregen... + Copyright (c) 2009-10 Hernando Barragan. All rights reserved. + Copyright 2011, Paul Stoffregen, paul@pjrc.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include "WString.h" + + +/*********************************************/ +/* Constructors */ +/*********************************************/ + +String::String(const char *cstr) +{ + init(); + if (cstr) copy(cstr, strlen(cstr)); +} + +String::String(const String &value) +{ + init(); + *this = value; +} + +#ifdef __GXX_EXPERIMENTAL_CXX0X__ +String::String(String &&rval) +{ + init(); + move(rval); +} +String::String(StringSumHelper &&rval) +{ + init(); + move(rval); +} +#endif + +String::String(char c) +{ + init(); + char buf[2]; + buf[0] = c; + buf[1] = 0; + *this = buf; +} + +String::String(unsigned char value, unsigned char base) +{ + init(); + char buf[9]; + utoa(value, buf, base); + *this = buf; +} + +String::String(int value, unsigned char base) +{ + init(); + char buf[18]; + itoa(value, buf, base); + *this = buf; +} + +String::String(unsigned int value, unsigned char base) +{ + init(); + char buf[17]; + utoa(value, buf, base); + *this = buf; +} + +String::String(long value, unsigned char base) +{ + init(); + char buf[34]; + ltoa(value, buf, base); + *this = buf; +} + +String::String(unsigned long value, unsigned char base) +{ + init(); + char buf[33]; + ultoa(value, buf, base); + *this = buf; +} + +String::~String() +{ + free(buffer); +} + +/*********************************************/ +/* Memory Management */ +/*********************************************/ + +inline void String::init(void) +{ + buffer = NULL; + capacity = 0; + len = 0; + flags = 0; +} + +void String::invalidate(void) +{ + if (buffer) free(buffer); + buffer = NULL; + capacity = len = 0; +} + +unsigned char String::reserve(unsigned int size) +{ + if (buffer && capacity >= size) return 1; + if (changeBuffer(size)) { + if (len == 0) buffer[0] = 0; + return 1; + } + return 0; +} + +unsigned char String::changeBuffer(unsigned int maxStrLen) +{ + char *newbuffer = (char *)realloc(buffer, maxStrLen + 1); + if (newbuffer) { + buffer = newbuffer; + capacity = maxStrLen; + return 1; + } + return 0; +} + +/*********************************************/ +/* Copy and Move */ +/*********************************************/ + +String & String::copy(const char *cstr, unsigned int length) +{ + if (!reserve(length)) { + invalidate(); + return *this; + } + len = length; + strcpy(buffer, cstr); + return *this; +} + +#ifdef __GXX_EXPERIMENTAL_CXX0X__ +void String::move(String &rhs) +{ + if (buffer) { + if (capacity >= rhs.len) { + strcpy(buffer, rhs.buffer); + len = rhs.len; + rhs.len = 0; + return; + } else { + free(buffer); + } + } + buffer = rhs.buffer; + capacity = rhs.capacity; + len = rhs.len; + rhs.buffer = NULL; + rhs.capacity = 0; + rhs.len = 0; +} +#endif + +String & String::operator = (const String &rhs) +{ + if (this == &rhs) return *this; + + if (rhs.buffer) copy(rhs.buffer, rhs.len); + else invalidate(); + + return *this; +} + +#ifdef __GXX_EXPERIMENTAL_CXX0X__ +String & String::operator = (String &&rval) +{ + if (this != &rval) move(rval); + return *this; +} + +String & String::operator = (StringSumHelper &&rval) +{ + if (this != &rval) move(rval); + return *this; +} +#endif + +String & String::operator = (const char *cstr) +{ + if (cstr) copy(cstr, strlen(cstr)); + else invalidate(); + + return *this; +} + +/*********************************************/ +/* concat */ +/*********************************************/ + +unsigned char String::concat(const String &s) +{ + return concat(s.buffer, s.len); +} + +unsigned char String::concat(const char *cstr, unsigned int length) +{ + unsigned int newlen = len + length; + if (!cstr) return 0; + if (length == 0) return 1; + if (!reserve(newlen)) return 0; + strcpy(buffer + len, cstr); + len = newlen; + return 1; +} + +unsigned char String::concat(const char *cstr) +{ + if (!cstr) return 0; + return concat(cstr, strlen(cstr)); +} + +unsigned char String::concat(char c) +{ + char buf[2]; + buf[0] = c; + buf[1] = 0; + return concat(buf, 1); +} + +unsigned char String::concat(unsigned char num) +{ + char buf[4]; + itoa(num, buf, 10); + return concat(buf, strlen(buf)); +} + +unsigned char String::concat(int num) +{ + char buf[7]; + itoa(num, buf, 10); + return concat(buf, strlen(buf)); +} + +unsigned char String::concat(unsigned int num) +{ + char buf[6]; + utoa(num, buf, 10); + return concat(buf, strlen(buf)); +} + +unsigned char String::concat(long num) +{ + char buf[12]; + ltoa(num, buf, 10); + return concat(buf, strlen(buf)); +} + +unsigned char String::concat(unsigned long num) +{ + char buf[11]; + ultoa(num, buf, 10); + return concat(buf, strlen(buf)); +} + +/*********************************************/ +/* Concatenate */ +/*********************************************/ + +StringSumHelper & operator + (const StringSumHelper &lhs, const String &rhs) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(rhs.buffer, rhs.len)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, const char *cstr) +{ + StringSumHelper &a = const_cast(lhs); + if (!cstr || !a.concat(cstr, strlen(cstr))) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, char c) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(c)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, unsigned char num) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(num)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, int num) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(num)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, unsigned int num) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(num)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, long num) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(num)) a.invalidate(); + return a; +} + +StringSumHelper & operator + (const StringSumHelper &lhs, unsigned long num) +{ + StringSumHelper &a = const_cast(lhs); + if (!a.concat(num)) a.invalidate(); + return a; +} + +/*********************************************/ +/* Comparison */ +/*********************************************/ + +int String::compareTo(const String &s) const +{ + if (!buffer || !s.buffer) { + if (s.buffer && s.len > 0) return 0 - *(unsigned char *)s.buffer; + if (buffer && len > 0) return *(unsigned char *)buffer; + return 0; + } + return strcmp(buffer, s.buffer); +} + +unsigned char String::equals(const String &s2) const +{ + return (len == s2.len && compareTo(s2) == 0); +} + +unsigned char String::equals(const char *cstr) const +{ + if (len == 0) return (cstr == NULL || *cstr == 0); + if (cstr == NULL) return buffer[0] == 0; + return strcmp(buffer, cstr) == 0; +} + +unsigned char String::operator<(const String &rhs) const +{ + return compareTo(rhs) < 0; +} + +unsigned char String::operator>(const String &rhs) const +{ + return compareTo(rhs) > 0; +} + +unsigned char String::operator<=(const String &rhs) const +{ + return compareTo(rhs) <= 0; +} + +unsigned char String::operator>=(const String &rhs) const +{ + return compareTo(rhs) >= 0; +} + +unsigned char String::equalsIgnoreCase( const String &s2 ) const +{ + if (this == &s2) return 1; + if (len != s2.len) return 0; + if (len == 0) return 1; + const char *p1 = buffer; + const char *p2 = s2.buffer; + while (*p1) { + if (tolower(*p1++) != tolower(*p2++)) return 0; + } + return 1; +} + +unsigned char String::startsWith( const String &s2 ) const +{ + if (len < s2.len) return 0; + return startsWith(s2, 0); +} + +unsigned char String::startsWith( const String &s2, unsigned int offset ) const +{ + if (offset > len - s2.len || !buffer || !s2.buffer) return 0; + return strncmp( &buffer[offset], s2.buffer, s2.len ) == 0; +} + +unsigned char String::endsWith( const String &s2 ) const +{ + if ( len < s2.len || !buffer || !s2.buffer) return 0; + return strcmp(&buffer[len - s2.len], s2.buffer) == 0; +} + +/*********************************************/ +/* Character Access */ +/*********************************************/ + +char String::charAt(unsigned int loc) const +{ + return operator[](loc); +} + +void String::setCharAt(unsigned int loc, char c) +{ + if (loc < len) buffer[loc] = c; +} + +char & String::operator[](unsigned int index) +{ + static char dummy_writable_char; + if (index >= len || !buffer) { + dummy_writable_char = 0; + return dummy_writable_char; + } + return buffer[index]; +} + +char String::operator[]( unsigned int index ) const +{ + if (index >= len || !buffer) return 0; + return buffer[index]; +} + +void String::getBytes(unsigned char *buf, unsigned int bufsize, unsigned int index) const +{ + if (!bufsize || !buf) return; + if (index >= len) { + buf[0] = 0; + return; + } + unsigned int n = bufsize - 1; + if (n > len - index) n = len - index; + strncpy((char *)buf, buffer + index, n); + buf[n] = 0; +} + +/*********************************************/ +/* Search */ +/*********************************************/ + +int String::indexOf(char c) const +{ + return indexOf(c, 0); +} + +int String::indexOf( char ch, unsigned int fromIndex ) const +{ + if (fromIndex >= len) return -1; + const char* temp = strchr(buffer + fromIndex, ch); + if (temp == NULL) return -1; + return temp - buffer; +} + +int String::indexOf(const String &s2) const +{ + return indexOf(s2, 0); +} + +int String::indexOf(const String &s2, unsigned int fromIndex) const +{ + if (fromIndex >= len) return -1; + const char *found = strstr(buffer + fromIndex, s2.buffer); + if (found == NULL) return -1; + return found - buffer; +} + +int String::lastIndexOf( char theChar ) const +{ + return lastIndexOf(theChar, len - 1); +} + +int String::lastIndexOf(char ch, unsigned int fromIndex) const +{ + if (fromIndex >= len) return -1; + char tempchar = buffer[fromIndex + 1]; + buffer[fromIndex + 1] = '\0'; + char* temp = strrchr( buffer, ch ); + buffer[fromIndex + 1] = tempchar; + if (temp == NULL) return -1; + return temp - buffer; +} + +int String::lastIndexOf(const String &s2) const +{ + return lastIndexOf(s2, len - s2.len); +} + +int String::lastIndexOf(const String &s2, unsigned int fromIndex) const +{ + if (s2.len == 0 || len == 0 || s2.len > len) return -1; + if (fromIndex >= len) fromIndex = len - 1; + int found = -1; + for (char *p = buffer; p <= buffer + fromIndex; p++) { + p = strstr(p, s2.buffer); + if (!p) break; + if ((unsigned int)(p - buffer) <= fromIndex) found = p - buffer; + } + return found; +} + +String String::substring( unsigned int left ) const +{ + return substring(left, len); +} + +String String::substring(unsigned int left, unsigned int right) const +{ + if (left > right) { + unsigned int temp = right; + right = left; + left = temp; + } + String out; + if (left > len) return out; + if (right > len) right = len; + char temp = buffer[right]; // save the replaced character + buffer[right] = '\0'; + out = buffer + left; // pointer arithmetic + buffer[right] = temp; //restore character + return out; +} + +/*********************************************/ +/* Modification */ +/*********************************************/ + +void String::replace(char find, char replace) +{ + if (!buffer) return; + for (char *p = buffer; *p; p++) { + if (*p == find) *p = replace; + } +} + +void String::replace(const String& find, const String& replace) +{ + if (len == 0 || find.len == 0) return; + int diff = replace.len - find.len; + char *readFrom = buffer; + char *foundAt; + if (diff == 0) { + while ((foundAt = strstr(readFrom, find.buffer)) != NULL) { + memcpy(foundAt, replace.buffer, replace.len); + readFrom = foundAt + replace.len; + } + } else if (diff < 0) { + char *writeTo = buffer; + while ((foundAt = strstr(readFrom, find.buffer)) != NULL) { + unsigned int n = foundAt - readFrom; + memcpy(writeTo, readFrom, n); + writeTo += n; + memcpy(writeTo, replace.buffer, replace.len); + writeTo += replace.len; + readFrom = foundAt + find.len; + len += diff; + } + strcpy(writeTo, readFrom); + } else { + unsigned int size = len; // compute size needed for result + while ((foundAt = strstr(readFrom, find.buffer)) != NULL) { + readFrom = foundAt + find.len; + size += diff; + } + if (size == len) return; + if (size > capacity && !changeBuffer(size)) return; // XXX: tell user! + int index = len - 1; + while (index >= 0 && (index = lastIndexOf(find, index)) >= 0) { + readFrom = buffer + index + find.len; + memmove(readFrom + diff, readFrom, len - (readFrom - buffer)); + len += diff; + buffer[len] = 0; + memcpy(buffer + index, replace.buffer, replace.len); + index--; + } + } +} + +void String::toLowerCase(void) +{ + if (!buffer) return; + for (char *p = buffer; *p; p++) { + *p = tolower(*p); + } +} + +void String::toUpperCase(void) +{ + if (!buffer) return; + for (char *p = buffer; *p; p++) { + *p = toupper(*p); + } +} + +void String::trim(void) +{ + if (!buffer || len == 0) return; + char *begin = buffer; + while (isspace(*begin)) begin++; + char *end = buffer + len - 1; + while (isspace(*end) && end >= begin) end--; + len = end + 1 - begin; + if (begin > buffer) memcpy(buffer, begin, len); + buffer[len] = 0; +} + +/*********************************************/ +/* Parsing / Conversion */ +/*********************************************/ + +long String::toInt(void) const +{ + if (buffer) return atol(buffer); + return 0; +} + + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.h new file mode 100644 index 0000000..947325e --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/WString.h @@ -0,0 +1,205 @@ +/* + WString.h - String library for Wiring & Arduino + ...mostly rewritten by Paul Stoffregen... + Copyright (c) 2009-10 Hernando Barragan. All right reserved. + Copyright 2011, Paul Stoffregen, paul@pjrc.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef String_class_h +#define String_class_h +#ifdef __cplusplus + +#include +#include +#include +#include + +// When compiling programs with this class, the following gcc parameters +// dramatically increase performance and memory (RAM) efficiency, typically +// with little or no increase in code size. +// -felide-constructors +// -std=c++0x + +class __FlashStringHelper; +#define F(string_literal) (reinterpret_cast(PSTR(string_literal))) + +// An inherited class for holding the result of a concatenation. These +// result objects are assumed to be writable by subsequent concatenations. +class StringSumHelper; + +// The string class +class String +{ + // use a function pointer to allow for "if (s)" without the + // complications of an operator bool(). for more information, see: + // http://www.artima.com/cppsource/safebool.html + typedef void (String::*StringIfHelperType)() const; + void StringIfHelper() const {} + +public: + // constructors + // creates a copy of the initial value. + // if the initial value is null or invalid, or if memory allocation + // fails, the string will be marked as invalid (i.e. "if (s)" will + // be false). + String(const char *cstr = ""); + String(const String &str); + #ifdef __GXX_EXPERIMENTAL_CXX0X__ + String(String &&rval); + String(StringSumHelper &&rval); + #endif + explicit String(char c); + explicit String(unsigned char, unsigned char base=10); + explicit String(int, unsigned char base=10); + explicit String(unsigned int, unsigned char base=10); + explicit String(long, unsigned char base=10); + explicit String(unsigned long, unsigned char base=10); + ~String(void); + + // memory management + // return true on success, false on failure (in which case, the string + // is left unchanged). reserve(0), if successful, will validate an + // invalid string (i.e., "if (s)" will be true afterwards) + unsigned char reserve(unsigned int size); + inline unsigned int length(void) const {return len;} + + // creates a copy of the assigned value. if the value is null or + // invalid, or if the memory allocation fails, the string will be + // marked as invalid ("if (s)" will be false). + String & operator = (const String &rhs); + String & operator = (const char *cstr); + #ifdef __GXX_EXPERIMENTAL_CXX0X__ + String & operator = (String &&rval); + String & operator = (StringSumHelper &&rval); + #endif + + // concatenate (works w/ built-in types) + + // returns true on success, false on failure (in which case, the string + // is left unchanged). if the argument is null or invalid, the + // concatenation is considered unsucessful. + unsigned char concat(const String &str); + unsigned char concat(const char *cstr); + unsigned char concat(char c); + unsigned char concat(unsigned char c); + unsigned char concat(int num); + unsigned char concat(unsigned int num); + unsigned char concat(long num); + unsigned char concat(unsigned long num); + + // if there's not enough memory for the concatenated value, the string + // will be left unchanged (but this isn't signalled in any way) + String & operator += (const String &rhs) {concat(rhs); return (*this);} + String & operator += (const char *cstr) {concat(cstr); return (*this);} + String & operator += (char c) {concat(c); return (*this);} + String & operator += (unsigned char num) {concat(num); return (*this);} + String & operator += (int num) {concat(num); return (*this);} + String & operator += (unsigned int num) {concat(num); return (*this);} + String & operator += (long num) {concat(num); return (*this);} + String & operator += (unsigned long num) {concat(num); return (*this);} + + friend StringSumHelper & operator + (const StringSumHelper &lhs, const String &rhs); + friend StringSumHelper & operator + (const StringSumHelper &lhs, const char *cstr); + friend StringSumHelper & operator + (const StringSumHelper &lhs, char c); + friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned char num); + friend StringSumHelper & operator + (const StringSumHelper &lhs, int num); + friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned int num); + friend StringSumHelper & operator + (const StringSumHelper &lhs, long num); + friend StringSumHelper & operator + (const StringSumHelper &lhs, unsigned long num); + + // comparison (only works w/ Strings and "strings") + operator StringIfHelperType() const { return buffer ? &String::StringIfHelper : 0; } + int compareTo(const String &s) const; + unsigned char equals(const String &s) const; + unsigned char equals(const char *cstr) const; + unsigned char operator == (const String &rhs) const {return equals(rhs);} + unsigned char operator == (const char *cstr) const {return equals(cstr);} + unsigned char operator != (const String &rhs) const {return !equals(rhs);} + unsigned char operator != (const char *cstr) const {return !equals(cstr);} + unsigned char operator < (const String &rhs) const; + unsigned char operator > (const String &rhs) const; + unsigned char operator <= (const String &rhs) const; + unsigned char operator >= (const String &rhs) const; + unsigned char equalsIgnoreCase(const String &s) const; + unsigned char startsWith( const String &prefix) const; + unsigned char startsWith(const String &prefix, unsigned int offset) const; + unsigned char endsWith(const String &suffix) const; + + // character acccess + char charAt(unsigned int index) const; + void setCharAt(unsigned int index, char c); + char operator [] (unsigned int index) const; + char& operator [] (unsigned int index); + void getBytes(unsigned char *buf, unsigned int bufsize, unsigned int index=0) const; + void toCharArray(char *buf, unsigned int bufsize, unsigned int index=0) const + {getBytes((unsigned char *)buf, bufsize, index);} + + // search + int indexOf( char ch ) const; + int indexOf( char ch, unsigned int fromIndex ) const; + int indexOf( const String &str ) const; + int indexOf( const String &str, unsigned int fromIndex ) const; + int lastIndexOf( char ch ) const; + int lastIndexOf( char ch, unsigned int fromIndex ) const; + int lastIndexOf( const String &str ) const; + int lastIndexOf( const String &str, unsigned int fromIndex ) const; + String substring( unsigned int beginIndex ) const; + String substring( unsigned int beginIndex, unsigned int endIndex ) const; + + // modification + void replace(char find, char replace); + void replace(const String& find, const String& replace); + void toLowerCase(void); + void toUpperCase(void); + void trim(void); + + // parsing/conversion + long toInt(void) const; + +protected: + char *buffer; // the actual char array + unsigned int capacity; // the array length minus one (for the '\0') + unsigned int len; // the String length (not counting the '\0') + unsigned char flags; // unused, for future features +protected: + void init(void); + void invalidate(void); + unsigned char changeBuffer(unsigned int maxStrLen); + unsigned char concat(const char *cstr, unsigned int length); + + // copy and move + String & copy(const char *cstr, unsigned int length); + #ifdef __GXX_EXPERIMENTAL_CXX0X__ + void move(String &rhs); + #endif +}; + +class StringSumHelper : public String +{ +public: + StringSumHelper(const String &s) : String(s) {} + StringSumHelper(const char *p) : String(p) {} + StringSumHelper(char c) : String(c) {} + StringSumHelper(unsigned char num) : String(num) {} + StringSumHelper(int num) : String(num) {} + StringSumHelper(unsigned int num) : String(num) {} + StringSumHelper(long num) : String(num) {} + StringSumHelper(unsigned long num) : String(num) {} +}; + +#endif // __cplusplus +#endif // String_class_h diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/binary.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/binary.h new file mode 100644 index 0000000..af14980 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/binary.h @@ -0,0 +1,515 @@ +#ifndef Binary_h +#define Binary_h + +#define B0 0 +#define B00 0 +#define B000 0 +#define B0000 0 +#define B00000 0 +#define B000000 0 +#define B0000000 0 +#define B00000000 0 +#define B1 1 +#define B01 1 +#define B001 1 +#define B0001 1 +#define B00001 1 +#define B000001 1 +#define B0000001 1 +#define B00000001 1 +#define B10 2 +#define B010 2 +#define B0010 2 +#define B00010 2 +#define B000010 2 +#define B0000010 2 +#define B00000010 2 +#define B11 3 +#define B011 3 +#define B0011 3 +#define B00011 3 +#define B000011 3 +#define B0000011 3 +#define B00000011 3 +#define B100 4 +#define B0100 4 +#define B00100 4 +#define B000100 4 +#define B0000100 4 +#define B00000100 4 +#define B101 5 +#define B0101 5 +#define B00101 5 +#define B000101 5 +#define B0000101 5 +#define B00000101 5 +#define B110 6 +#define B0110 6 +#define B00110 6 +#define B000110 6 +#define B0000110 6 +#define B00000110 6 +#define B111 7 +#define B0111 7 +#define B00111 7 +#define B000111 7 +#define B0000111 7 +#define B00000111 7 +#define B1000 8 +#define B01000 8 +#define B001000 8 +#define B0001000 8 +#define B00001000 8 +#define B1001 9 +#define B01001 9 +#define B001001 9 +#define B0001001 9 +#define B00001001 9 +#define B1010 10 +#define B01010 10 +#define B001010 10 +#define B0001010 10 +#define B00001010 10 +#define B1011 11 +#define B01011 11 +#define B001011 11 +#define B0001011 11 +#define B00001011 11 +#define B1100 12 +#define B01100 12 +#define B001100 12 +#define B0001100 12 +#define B00001100 12 +#define B1101 13 +#define B01101 13 +#define B001101 13 +#define B0001101 13 +#define B00001101 13 +#define B1110 14 +#define B01110 14 +#define B001110 14 +#define B0001110 14 +#define B00001110 14 +#define B1111 15 +#define B01111 15 +#define B001111 15 +#define B0001111 15 +#define B00001111 15 +#define B10000 16 +#define B010000 16 +#define B0010000 16 +#define B00010000 16 +#define B10001 17 +#define B010001 17 +#define B0010001 17 +#define B00010001 17 +#define B10010 18 +#define B010010 18 +#define B0010010 18 +#define B00010010 18 +#define B10011 19 +#define B010011 19 +#define B0010011 19 +#define B00010011 19 +#define B10100 20 +#define B010100 20 +#define B0010100 20 +#define B00010100 20 +#define B10101 21 +#define B010101 21 +#define B0010101 21 +#define B00010101 21 +#define B10110 22 +#define B010110 22 +#define B0010110 22 +#define B00010110 22 +#define B10111 23 +#define B010111 23 +#define B0010111 23 +#define B00010111 23 +#define B11000 24 +#define B011000 24 +#define B0011000 24 +#define B00011000 24 +#define B11001 25 +#define B011001 25 +#define B0011001 25 +#define B00011001 25 +#define B11010 26 +#define B011010 26 +#define B0011010 26 +#define B00011010 26 +#define B11011 27 +#define B011011 27 +#define B0011011 27 +#define B00011011 27 +#define B11100 28 +#define B011100 28 +#define B0011100 28 +#define B00011100 28 +#define B11101 29 +#define B011101 29 +#define B0011101 29 +#define B00011101 29 +#define B11110 30 +#define B011110 30 +#define B0011110 30 +#define B00011110 30 +#define B11111 31 +#define B011111 31 +#define B0011111 31 +#define B00011111 31 +#define B100000 32 +#define B0100000 32 +#define B00100000 32 +#define B100001 33 +#define B0100001 33 +#define B00100001 33 +#define B100010 34 +#define B0100010 34 +#define B00100010 34 +#define B100011 35 +#define B0100011 35 +#define B00100011 35 +#define B100100 36 +#define B0100100 36 +#define B00100100 36 +#define B100101 37 +#define B0100101 37 +#define B00100101 37 +#define B100110 38 +#define B0100110 38 +#define B00100110 38 +#define B100111 39 +#define B0100111 39 +#define B00100111 39 +#define B101000 40 +#define B0101000 40 +#define B00101000 40 +#define B101001 41 +#define B0101001 41 +#define B00101001 41 +#define B101010 42 +#define B0101010 42 +#define B00101010 42 +#define B101011 43 +#define B0101011 43 +#define B00101011 43 +#define B101100 44 +#define B0101100 44 +#define B00101100 44 +#define B101101 45 +#define B0101101 45 +#define B00101101 45 +#define B101110 46 +#define B0101110 46 +#define B00101110 46 +#define B101111 47 +#define B0101111 47 +#define B00101111 47 +#define B110000 48 +#define B0110000 48 +#define B00110000 48 +#define B110001 49 +#define B0110001 49 +#define B00110001 49 +#define B110010 50 +#define B0110010 50 +#define B00110010 50 +#define B110011 51 +#define B0110011 51 +#define B00110011 51 +#define B110100 52 +#define B0110100 52 +#define B00110100 52 +#define B110101 53 +#define B0110101 53 +#define B00110101 53 +#define B110110 54 +#define B0110110 54 +#define B00110110 54 +#define B110111 55 +#define B0110111 55 +#define B00110111 55 +#define B111000 56 +#define B0111000 56 +#define B00111000 56 +#define B111001 57 +#define B0111001 57 +#define B00111001 57 +#define B111010 58 +#define B0111010 58 +#define B00111010 58 +#define B111011 59 +#define B0111011 59 +#define B00111011 59 +#define B111100 60 +#define B0111100 60 +#define B00111100 60 +#define B111101 61 +#define B0111101 61 +#define B00111101 61 +#define B111110 62 +#define B0111110 62 +#define B00111110 62 +#define B111111 63 +#define B0111111 63 +#define B00111111 63 +#define B1000000 64 +#define B01000000 64 +#define B1000001 65 +#define B01000001 65 +#define B1000010 66 +#define B01000010 66 +#define B1000011 67 +#define B01000011 67 +#define B1000100 68 +#define B01000100 68 +#define B1000101 69 +#define B01000101 69 +#define B1000110 70 +#define B01000110 70 +#define B1000111 71 +#define B01000111 71 +#define B1001000 72 +#define B01001000 72 +#define B1001001 73 +#define B01001001 73 +#define B1001010 74 +#define B01001010 74 +#define B1001011 75 +#define B01001011 75 +#define B1001100 76 +#define B01001100 76 +#define B1001101 77 +#define B01001101 77 +#define B1001110 78 +#define B01001110 78 +#define B1001111 79 +#define B01001111 79 +#define B1010000 80 +#define B01010000 80 +#define B1010001 81 +#define B01010001 81 +#define B1010010 82 +#define B01010010 82 +#define B1010011 83 +#define B01010011 83 +#define B1010100 84 +#define B01010100 84 +#define B1010101 85 +#define B01010101 85 +#define B1010110 86 +#define B01010110 86 +#define B1010111 87 +#define B01010111 87 +#define B1011000 88 +#define B01011000 88 +#define B1011001 89 +#define B01011001 89 +#define B1011010 90 +#define B01011010 90 +#define B1011011 91 +#define B01011011 91 +#define B1011100 92 +#define B01011100 92 +#define B1011101 93 +#define B01011101 93 +#define B1011110 94 +#define B01011110 94 +#define B1011111 95 +#define B01011111 95 +#define B1100000 96 +#define B01100000 96 +#define B1100001 97 +#define B01100001 97 +#define B1100010 98 +#define B01100010 98 +#define B1100011 99 +#define B01100011 99 +#define B1100100 100 +#define B01100100 100 +#define B1100101 101 +#define B01100101 101 +#define B1100110 102 +#define B01100110 102 +#define B1100111 103 +#define B01100111 103 +#define B1101000 104 +#define B01101000 104 +#define B1101001 105 +#define B01101001 105 +#define B1101010 106 +#define B01101010 106 +#define B1101011 107 +#define B01101011 107 +#define B1101100 108 +#define B01101100 108 +#define B1101101 109 +#define B01101101 109 +#define B1101110 110 +#define B01101110 110 +#define B1101111 111 +#define B01101111 111 +#define B1110000 112 +#define B01110000 112 +#define B1110001 113 +#define B01110001 113 +#define B1110010 114 +#define B01110010 114 +#define B1110011 115 +#define B01110011 115 +#define B1110100 116 +#define B01110100 116 +#define B1110101 117 +#define B01110101 117 +#define B1110110 118 +#define B01110110 118 +#define B1110111 119 +#define B01110111 119 +#define B1111000 120 +#define B01111000 120 +#define B1111001 121 +#define B01111001 121 +#define B1111010 122 +#define B01111010 122 +#define B1111011 123 +#define B01111011 123 +#define B1111100 124 +#define B01111100 124 +#define B1111101 125 +#define B01111101 125 +#define B1111110 126 +#define B01111110 126 +#define B1111111 127 +#define B01111111 127 +#define B10000000 128 +#define B10000001 129 +#define B10000010 130 +#define B10000011 131 +#define B10000100 132 +#define B10000101 133 +#define B10000110 134 +#define B10000111 135 +#define B10001000 136 +#define B10001001 137 +#define B10001010 138 +#define B10001011 139 +#define B10001100 140 +#define B10001101 141 +#define B10001110 142 +#define B10001111 143 +#define B10010000 144 +#define B10010001 145 +#define B10010010 146 +#define B10010011 147 +#define B10010100 148 +#define B10010101 149 +#define B10010110 150 +#define B10010111 151 +#define B10011000 152 +#define B10011001 153 +#define B10011010 154 +#define B10011011 155 +#define B10011100 156 +#define B10011101 157 +#define B10011110 158 +#define B10011111 159 +#define B10100000 160 +#define B10100001 161 +#define B10100010 162 +#define B10100011 163 +#define B10100100 164 +#define B10100101 165 +#define B10100110 166 +#define B10100111 167 +#define B10101000 168 +#define B10101001 169 +#define B10101010 170 +#define B10101011 171 +#define B10101100 172 +#define B10101101 173 +#define B10101110 174 +#define B10101111 175 +#define B10110000 176 +#define B10110001 177 +#define B10110010 178 +#define B10110011 179 +#define B10110100 180 +#define B10110101 181 +#define B10110110 182 +#define B10110111 183 +#define B10111000 184 +#define B10111001 185 +#define B10111010 186 +#define B10111011 187 +#define B10111100 188 +#define B10111101 189 +#define B10111110 190 +#define B10111111 191 +#define B11000000 192 +#define B11000001 193 +#define B11000010 194 +#define B11000011 195 +#define B11000100 196 +#define B11000101 197 +#define B11000110 198 +#define B11000111 199 +#define B11001000 200 +#define B11001001 201 +#define B11001010 202 +#define B11001011 203 +#define B11001100 204 +#define B11001101 205 +#define B11001110 206 +#define B11001111 207 +#define B11010000 208 +#define B11010001 209 +#define B11010010 210 +#define B11010011 211 +#define B11010100 212 +#define B11010101 213 +#define B11010110 214 +#define B11010111 215 +#define B11011000 216 +#define B11011001 217 +#define B11011010 218 +#define B11011011 219 +#define B11011100 220 +#define B11011101 221 +#define B11011110 222 +#define B11011111 223 +#define B11100000 224 +#define B11100001 225 +#define B11100010 226 +#define B11100011 227 +#define B11100100 228 +#define B11100101 229 +#define B11100110 230 +#define B11100111 231 +#define B11101000 232 +#define B11101001 233 +#define B11101010 234 +#define B11101011 235 +#define B11101100 236 +#define B11101101 237 +#define B11101110 238 +#define B11101111 239 +#define B11110000 240 +#define B11110001 241 +#define B11110010 242 +#define B11110011 243 +#define B11110100 244 +#define B11110101 245 +#define B11110110 246 +#define B11110111 247 +#define B11111000 248 +#define B11111001 249 +#define B11111010 250 +#define B11111011 251 +#define B11111100 252 +#define B11111101 253 +#define B11111110 254 +#define B11111111 255 + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/main.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/main.cpp new file mode 100644 index 0000000..3d4e079 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/main.cpp @@ -0,0 +1,20 @@ +#include + +int main(void) +{ + init(); + +#if defined(USBCON) + USBDevice.attach(); +#endif + + setup(); + + for (;;) { + loop(); + if (serialEventRun) serialEventRun(); + } + + return 0; +} + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/new.cpp b/libs/arduino-1.0/hardware/arduino/cores/arduino/new.cpp new file mode 100644 index 0000000..0f6d422 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/new.cpp @@ -0,0 +1,18 @@ +#include + +void * operator new(size_t size) +{ + return malloc(size); +} + +void operator delete(void * ptr) +{ + free(ptr); +} + +int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);}; +void __cxa_guard_release (__guard *g) {*(char *)g = 1;}; +void __cxa_guard_abort (__guard *) {}; + +void __cxa_pure_virtual(void) {}; + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/new.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/new.h new file mode 100644 index 0000000..cd940ce --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/new.h @@ -0,0 +1,22 @@ +/* Header to define new/delete operators as they aren't provided by avr-gcc by default + Taken from http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&t=59453 + */ + +#ifndef NEW_H +#define NEW_H + +#include + +void * operator new(size_t size); +void operator delete(void * ptr); + +__extension__ typedef int __guard __attribute__((mode (__DI__))); + +extern "C" int __cxa_guard_acquire(__guard *); +extern "C" void __cxa_guard_release (__guard *); +extern "C" void __cxa_guard_abort (__guard *); + +extern "C" void __cxa_pure_virtual(void); + +#endif + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring.c new file mode 100644 index 0000000..ac8bb6f --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring.c @@ -0,0 +1,324 @@ +/* + wiring.c - Partial implementation of the Wiring API for the ATmega8. + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id$ +*/ + +#include "wiring_private.h" + +// the prescaler is set so that timer0 ticks every 64 clock cycles, and the +// the overflow handler is called every 256 ticks. +#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(64 * 256)) + +// the whole number of milliseconds per timer0 overflow +#define MILLIS_INC (MICROSECONDS_PER_TIMER0_OVERFLOW / 1000) + +// the fractional number of milliseconds per timer0 overflow. we shift right +// by three to fit these numbers into a byte. (for the clock speeds we care +// about - 8 and 16 MHz - this doesn't lose precision.) +#define FRACT_INC ((MICROSECONDS_PER_TIMER0_OVERFLOW % 1000) >> 3) +#define FRACT_MAX (1000 >> 3) + +volatile unsigned long timer0_overflow_count = 0; +volatile unsigned long timer0_millis = 0; +static unsigned char timer0_fract = 0; + +#if defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) +SIGNAL(TIM0_OVF_vect) +#else +SIGNAL(TIMER0_OVF_vect) +#endif +{ + // copy these to local variables so they can be stored in registers + // (volatile variables must be read from memory on every access) + unsigned long m = timer0_millis; + unsigned char f = timer0_fract; + + m += MILLIS_INC; + f += FRACT_INC; + if (f >= FRACT_MAX) { + f -= FRACT_MAX; + m += 1; + } + + timer0_fract = f; + timer0_millis = m; + timer0_overflow_count++; +} + +unsigned long millis() +{ + unsigned long m; + uint8_t oldSREG = SREG; + + // disable interrupts while we read timer0_millis or we might get an + // inconsistent value (e.g. in the middle of a write to timer0_millis) + cli(); + m = timer0_millis; + SREG = oldSREG; + + return m; +} + +unsigned long micros() { + unsigned long m; + uint8_t oldSREG = SREG, t; + + cli(); + m = timer0_overflow_count; +#if defined(TCNT0) + t = TCNT0; +#elif defined(TCNT0L) + t = TCNT0L; +#else + #error TIMER 0 not defined +#endif + + +#ifdef TIFR0 + if ((TIFR0 & _BV(TOV0)) && (t < 255)) + m++; +#else + if ((TIFR & _BV(TOV0)) && (t < 255)) + m++; +#endif + + SREG = oldSREG; + + return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond()); +} + +void delay(unsigned long ms) +{ + uint16_t start = (uint16_t)micros(); + + while (ms > 0) { + if (((uint16_t)micros() - start) >= 1000) { + ms--; + start += 1000; + } + } +} + +/* Delay for the given number of microseconds. Assumes a 8 or 16 MHz clock. */ +void delayMicroseconds(unsigned int us) +{ + // calling avrlib's delay_us() function with low values (e.g. 1 or + // 2 microseconds) gives delays longer than desired. + //delay_us(us); +#if F_CPU >= 20000000L + // for the 20 MHz clock on rare Arduino boards + + // for a one-microsecond delay, simply wait 2 cycle and return. The overhead + // of the function call yields a delay of exactly a one microsecond. + __asm__ __volatile__ ( + "nop" "\n\t" + "nop"); //just waiting 2 cycle + if (--us == 0) + return; + + // the following loop takes a 1/5 of a microsecond (4 cycles) + // per iteration, so execute it five times for each microsecond of + // delay requested. + us = (us<<2) + us; // x5 us + + // account for the time taken in the preceeding commands. + us -= 2; + +#elif F_CPU >= 16000000L + // for the 16 MHz clock on most Arduino boards + + // for a one-microsecond delay, simply return. the overhead + // of the function call yields a delay of approximately 1 1/8 us. + if (--us == 0) + return; + + // the following loop takes a quarter of a microsecond (4 cycles) + // per iteration, so execute it four times for each microsecond of + // delay requested. + us <<= 2; + + // account for the time taken in the preceeding commands. + us -= 2; +#else + // for the 8 MHz internal clock on the ATmega168 + + // for a one- or two-microsecond delay, simply return. the overhead of + // the function calls takes more than two microseconds. can't just + // subtract two, since us is unsigned; we'd overflow. + if (--us == 0) + return; + if (--us == 0) + return; + + // the following loop takes half of a microsecond (4 cycles) + // per iteration, so execute it twice for each microsecond of + // delay requested. + us <<= 1; + + // partially compensate for the time taken by the preceeding commands. + // we can't subtract any more than this or we'd overflow w/ small delays. + us--; +#endif + + // busy wait + __asm__ __volatile__ ( + "1: sbiw %0,1" "\n\t" // 2 cycles + "brne 1b" : "=w" (us) : "0" (us) // 2 cycles + ); +} + +void init() +{ + // this needs to be called before setup() or some functions won't + // work there + sei(); + + // on the ATmega168, timer 0 is also used for fast hardware pwm + // (using phase-correct PWM would mean that timer 0 overflowed half as often + // resulting in different millis() behavior on the ATmega8 and ATmega168) +#if defined(TCCR0A) && defined(WGM01) + sbi(TCCR0A, WGM01); + sbi(TCCR0A, WGM00); +#endif + + // set timer 0 prescale factor to 64 +#if defined(__AVR_ATmega128__) + // CPU specific: different values for the ATmega128 + sbi(TCCR0, CS02); +#elif defined(TCCR0) && defined(CS01) && defined(CS00) + // this combination is for the standard atmega8 + sbi(TCCR0, CS01); + sbi(TCCR0, CS00); +#elif defined(TCCR0B) && defined(CS01) && defined(CS00) + // this combination is for the standard 168/328/1280/2560 + sbi(TCCR0B, CS01); + sbi(TCCR0B, CS00); +#elif defined(TCCR0A) && defined(CS01) && defined(CS00) + // this combination is for the __AVR_ATmega645__ series + sbi(TCCR0A, CS01); + sbi(TCCR0A, CS00); +#else + #error Timer 0 prescale factor 64 not set correctly +#endif + + // enable timer 0 overflow interrupt +#if defined(TIMSK) && defined(TOIE0) + sbi(TIMSK, TOIE0); +#elif defined(TIMSK0) && defined(TOIE0) + sbi(TIMSK0, TOIE0); +#else + #error Timer 0 overflow interrupt not set correctly +#endif + + // timers 1 and 2 are used for phase-correct hardware pwm + // this is better for motors as it ensures an even waveform + // note, however, that fast pwm mode can achieve a frequency of up + // 8 MHz (with a 16 MHz clock) at 50% duty cycle + +#if defined(TCCR1B) && defined(CS11) && defined(CS10) + TCCR1B = 0; + + // set timer 1 prescale factor to 64 + sbi(TCCR1B, CS11); +#if F_CPU >= 8000000L + sbi(TCCR1B, CS10); +#endif +#elif defined(TCCR1) && defined(CS11) && defined(CS10) + sbi(TCCR1, CS11); +#if F_CPU >= 8000000L + sbi(TCCR1, CS10); +#endif +#endif + // put timer 1 in 8-bit phase correct pwm mode +#if defined(TCCR1A) && defined(WGM10) + sbi(TCCR1A, WGM10); +#elif defined(TCCR1) + #warning this needs to be finished +#endif + + // set timer 2 prescale factor to 64 +#if defined(TCCR2) && defined(CS22) + sbi(TCCR2, CS22); +#elif defined(TCCR2B) && defined(CS22) + sbi(TCCR2B, CS22); +#else + #warning Timer 2 not finished (may not be present on this CPU) +#endif + + // configure timer 2 for phase correct pwm (8-bit) +#if defined(TCCR2) && defined(WGM20) + sbi(TCCR2, WGM20); +#elif defined(TCCR2A) && defined(WGM20) + sbi(TCCR2A, WGM20); +#else + #warning Timer 2 not finished (may not be present on this CPU) +#endif + +#if defined(TCCR3B) && defined(CS31) && defined(WGM30) + sbi(TCCR3B, CS31); // set timer 3 prescale factor to 64 + sbi(TCCR3B, CS30); + sbi(TCCR3A, WGM30); // put timer 3 in 8-bit phase correct pwm mode +#endif + +#if defined(TCCR4A) && defined(TCCR4B) && defined(TCCR4D) /* beginning of timer4 block for 32U4 and similar */ + sbi(TCCR4B, CS42); // set timer4 prescale factor to 64 + sbi(TCCR4B, CS41); + sbi(TCCR4B, CS40); + sbi(TCCR4D, WGM40); // put timer 4 in phase- and frequency-correct PWM mode + sbi(TCCR4A, PWM4A); // enable PWM mode for comparator OCR4A + sbi(TCCR4C, PWM4D); // enable PWM mode for comparator OCR4D +#else /* beginning of timer4 block for ATMEGA1280 and ATMEGA2560 */ +#if defined(TCCR4B) && defined(CS41) && defined(WGM40) + sbi(TCCR4B, CS41); // set timer 4 prescale factor to 64 + sbi(TCCR4B, CS40); + sbi(TCCR4A, WGM40); // put timer 4 in 8-bit phase correct pwm mode +#endif +#endif /* end timer4 block for ATMEGA1280/2560 and similar */ + +#if defined(TCCR5B) && defined(CS51) && defined(WGM50) + sbi(TCCR5B, CS51); // set timer 5 prescale factor to 64 + sbi(TCCR5B, CS50); + sbi(TCCR5A, WGM50); // put timer 5 in 8-bit phase correct pwm mode +#endif + +#if defined(ADCSRA) + // set a2d prescale factor to 128 + // 16 MHz / 128 = 125 KHz, inside the desired 50-200 KHz range. + // XXX: this will not work properly for other clock speeds, and + // this code should use F_CPU to determine the prescale factor. + sbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); + + // enable a2d conversions + sbi(ADCSRA, ADEN); +#endif + + // the bootloader connects pins 0 and 1 to the USART; disconnect them + // here so they can be used as normal digital i/o; they will be + // reconnected in Serial.begin() +#if defined(UCSRB) + UCSRB = 0; +#elif defined(UCSR0B) + UCSR0B = 0; +#endif +} diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_analog.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_analog.c new file mode 100644 index 0000000..23b01c6 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_analog.c @@ -0,0 +1,282 @@ +/* + wiring_analog.c - analog input and output + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + Modified 28 September 2010 by Mark Sproul + + $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ +*/ + +#include "wiring_private.h" +#include "pins_arduino.h" + +uint8_t analog_reference = DEFAULT; + +void analogReference(uint8_t mode) +{ + // can't actually set the register here because the default setting + // will connect AVCC and the AREF pin, which would cause a short if + // there's something connected to AREF. + analog_reference = mode; +} + +int analogRead(uint8_t pin) +{ + uint8_t low, high; + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + if (pin >= 54) pin -= 54; // allow for channel or pin numbers +#elif defined(__AVR_ATmega32U4__) + if (pin >= 18) pin -= 18; // allow for channel or pin numbers +#elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) + if (pin >= 24) pin -= 24; // allow for channel or pin numbers +#else + if (pin >= 14) pin -= 14; // allow for channel or pin numbers +#endif + +#if defined(__AVR_ATmega32U4__) + pin = analogPinToChannel(pin); + ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((pin >> 3) & 0x01) << MUX5); +#elif defined(ADCSRB) && defined(MUX5) + // the MUX5 bit of ADCSRB selects whether we're reading from channels + // 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high). + ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((pin >> 3) & 0x01) << MUX5); +#endif + + // set the analog reference (high two bits of ADMUX) and select the + // channel (low 4 bits). this also sets ADLAR (left-adjust result) + // to 0 (the default). +#if defined(ADMUX) + ADMUX = (analog_reference << 6) | (pin & 0x07); +#endif + + // without a delay, we seem to read from the wrong channel + //delay(1); + +#if defined(ADCSRA) && defined(ADCL) + // start the conversion + sbi(ADCSRA, ADSC); + + // ADSC is cleared when the conversion finishes + while (bit_is_set(ADCSRA, ADSC)); + + // we have to read ADCL first; doing so locks both ADCL + // and ADCH until ADCH is read. reading ADCL second would + // cause the results of each conversion to be discarded, + // as ADCL and ADCH would be locked when it completed. + low = ADCL; + high = ADCH; +#else + // we dont have an ADC, return 0 + low = 0; + high = 0; +#endif + + // combine the two bytes + return (high << 8) | low; +} + +// Right now, PWM output only works on the pins with +// hardware support. These are defined in the appropriate +// pins_*.c file. For the rest of the pins, we default +// to digital output. +void analogWrite(uint8_t pin, int val) +{ + // We need to make sure the PWM output is enabled for those pins + // that support it, as we turn it off when digitally reading or + // writing with them. Also, make sure the pin is in output mode + // for consistenty with Wiring, which doesn't require a pinMode + // call for the analog output pins. + pinMode(pin, OUTPUT); + if (val == 0) + { + digitalWrite(pin, LOW); + } + else if (val == 255) + { + digitalWrite(pin, HIGH); + } + else + { + switch(digitalPinToTimer(pin)) + { + // XXX fix needed for atmega8 + #if defined(TCCR0) && defined(COM00) && !defined(__AVR_ATmega8__) + case TIMER0A: + // connect pwm to pin on timer 0 + sbi(TCCR0, COM00); + OCR0 = val; // set pwm duty + break; + #endif + + #if defined(TCCR0A) && defined(COM0A1) + case TIMER0A: + // connect pwm to pin on timer 0, channel A + sbi(TCCR0A, COM0A1); + OCR0A = val; // set pwm duty + break; + #endif + + #if defined(TCCR0A) && defined(COM0B1) + case TIMER0B: + // connect pwm to pin on timer 0, channel B + sbi(TCCR0A, COM0B1); + OCR0B = val; // set pwm duty + break; + #endif + + #if defined(TCCR1A) && defined(COM1A1) + case TIMER1A: + // connect pwm to pin on timer 1, channel A + sbi(TCCR1A, COM1A1); + OCR1A = val; // set pwm duty + break; + #endif + + #if defined(TCCR1A) && defined(COM1B1) + case TIMER1B: + // connect pwm to pin on timer 1, channel B + sbi(TCCR1A, COM1B1); + OCR1B = val; // set pwm duty + break; + #endif + + #if defined(TCCR2) && defined(COM21) + case TIMER2: + // connect pwm to pin on timer 2 + sbi(TCCR2, COM21); + OCR2 = val; // set pwm duty + break; + #endif + + #if defined(TCCR2A) && defined(COM2A1) + case TIMER2A: + // connect pwm to pin on timer 2, channel A + sbi(TCCR2A, COM2A1); + OCR2A = val; // set pwm duty + break; + #endif + + #if defined(TCCR2A) && defined(COM2B1) + case TIMER2B: + // connect pwm to pin on timer 2, channel B + sbi(TCCR2A, COM2B1); + OCR2B = val; // set pwm duty + break; + #endif + + #if defined(TCCR3A) && defined(COM3A1) + case TIMER3A: + // connect pwm to pin on timer 3, channel A + sbi(TCCR3A, COM3A1); + OCR3A = val; // set pwm duty + break; + #endif + + #if defined(TCCR3A) && defined(COM3B1) + case TIMER3B: + // connect pwm to pin on timer 3, channel B + sbi(TCCR3A, COM3B1); + OCR3B = val; // set pwm duty + break; + #endif + + #if defined(TCCR3A) && defined(COM3C1) + case TIMER3C: + // connect pwm to pin on timer 3, channel C + sbi(TCCR3A, COM3C1); + OCR3C = val; // set pwm duty + break; + #endif + + #if defined(TCCR4A) + case TIMER4A: + //connect pwm to pin on timer 4, channel A + sbi(TCCR4A, COM4A1); + #if defined(COM4A0) // only used on 32U4 + cbi(TCCR4A, COM4A0); + #endif + OCR4A = val; // set pwm duty + break; + #endif + + #if defined(TCCR4A) && defined(COM4B1) + case TIMER4B: + // connect pwm to pin on timer 4, channel B + sbi(TCCR4A, COM4B1); + OCR4B = val; // set pwm duty + break; + #endif + + #if defined(TCCR4A) && defined(COM4C1) + case TIMER4C: + // connect pwm to pin on timer 4, channel C + sbi(TCCR4A, COM4C1); + OCR4C = val; // set pwm duty + break; + #endif + + #if defined(TCCR4C) && defined(COM4D1) + case TIMER4D: + // connect pwm to pin on timer 4, channel D + sbi(TCCR4C, COM4D1); + #if defined(COM4D0) // only used on 32U4 + cbi(TCCR4C, COM4D0); + #endif + OCR4D = val; // set pwm duty + break; + #endif + + + #if defined(TCCR5A) && defined(COM5A1) + case TIMER5A: + // connect pwm to pin on timer 5, channel A + sbi(TCCR5A, COM5A1); + OCR5A = val; // set pwm duty + break; + #endif + + #if defined(TCCR5A) && defined(COM5B1) + case TIMER5B: + // connect pwm to pin on timer 5, channel B + sbi(TCCR5A, COM5B1); + OCR5B = val; // set pwm duty + break; + #endif + + #if defined(TCCR5A) && defined(COM5C1) + case TIMER5C: + // connect pwm to pin on timer 5, channel C + sbi(TCCR5A, COM5C1); + OCR5C = val; // set pwm duty + break; + #endif + + case NOT_ON_TIMER: + default: + if (val < 128) { + digitalWrite(pin, LOW); + } else { + digitalWrite(pin, HIGH); + } + } + } +} + diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_digital.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_digital.c new file mode 100644 index 0000000..be323b1 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_digital.c @@ -0,0 +1,178 @@ +/* + wiring_digital.c - digital input and output functions + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + Modified 28 September 2010 by Mark Sproul + + $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ +*/ + +#define ARDUINO_MAIN +#include "wiring_private.h" +#include "pins_arduino.h" + +void pinMode(uint8_t pin, uint8_t mode) +{ + uint8_t bit = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + volatile uint8_t *reg, *out; + + if (port == NOT_A_PIN) return; + + // JWS: can I let the optimizer do this? + reg = portModeRegister(port); + out = portOutputRegister(port); + + if (mode == INPUT) { + uint8_t oldSREG = SREG; + cli(); + *reg &= ~bit; + *out &= ~bit; + SREG = oldSREG; + } else if (mode == INPUT_PULLUP) { + uint8_t oldSREG = SREG; + cli(); + *reg &= ~bit; + *out |= bit; + SREG = oldSREG; + } else { + uint8_t oldSREG = SREG; + cli(); + *reg |= bit; + SREG = oldSREG; + } +} + +// Forcing this inline keeps the callers from having to push their own stuff +// on the stack. It is a good performance win and only takes 1 more byte per +// user than calling. (It will take more bytes on the 168.) +// +// But shouldn't this be moved into pinMode? Seems silly to check and do on +// each digitalread or write. +// +// Mark Sproul: +// - Removed inline. Save 170 bytes on atmega1280 +// - changed to a switch statment; added 32 bytes but much easier to read and maintain. +// - Added more #ifdefs, now compiles for atmega645 +// +//static inline void turnOffPWM(uint8_t timer) __attribute__ ((always_inline)); +//static inline void turnOffPWM(uint8_t timer) +static void turnOffPWM(uint8_t timer) +{ + switch (timer) + { + #if defined(TCCR1A) && defined(COM1A1) + case TIMER1A: cbi(TCCR1A, COM1A1); break; + #endif + #if defined(TCCR1A) && defined(COM1B1) + case TIMER1B: cbi(TCCR1A, COM1B1); break; + #endif + + #if defined(TCCR2) && defined(COM21) + case TIMER2: cbi(TCCR2, COM21); break; + #endif + + #if defined(TCCR0A) && defined(COM0A1) + case TIMER0A: cbi(TCCR0A, COM0A1); break; + #endif + + #if defined(TIMER0B) && defined(COM0B1) + case TIMER0B: cbi(TCCR0A, COM0B1); break; + #endif + #if defined(TCCR2A) && defined(COM2A1) + case TIMER2A: cbi(TCCR2A, COM2A1); break; + #endif + #if defined(TCCR2A) && defined(COM2B1) + case TIMER2B: cbi(TCCR2A, COM2B1); break; + #endif + + #if defined(TCCR3A) && defined(COM3A1) + case TIMER3A: cbi(TCCR3A, COM3A1); break; + #endif + #if defined(TCCR3A) && defined(COM3B1) + case TIMER3B: cbi(TCCR3A, COM3B1); break; + #endif + #if defined(TCCR3A) && defined(COM3C1) + case TIMER3C: cbi(TCCR3A, COM3C1); break; + #endif + + #if defined(TCCR4A) && defined(COM4A1) + case TIMER4A: cbi(TCCR4A, COM4A1); break; + #endif + #if defined(TCCR4A) && defined(COM4B1) + case TIMER4B: cbi(TCCR4A, COM4B1); break; + #endif + #if defined(TCCR4A) && defined(COM4C1) + case TIMER4C: cbi(TCCR4A, COM4C1); break; + #endif + #if defined(TCCR4C) && defined(COM4D1) + case TIMER4D: cbi(TCCR4C, COM4D1); break; + #endif + + #if defined(TCCR5A) + case TIMER5A: cbi(TCCR5A, COM5A1); break; + case TIMER5B: cbi(TCCR5A, COM5B1); break; + case TIMER5C: cbi(TCCR5A, COM5C1); break; + #endif + } +} + +void digitalWrite(uint8_t pin, uint8_t val) +{ + uint8_t timer = digitalPinToTimer(pin); + uint8_t bit = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + volatile uint8_t *out; + + if (port == NOT_A_PIN) return; + + // If the pin that support PWM output, we need to turn it off + // before doing a digital write. + if (timer != NOT_ON_TIMER) turnOffPWM(timer); + + out = portOutputRegister(port); + + uint8_t oldSREG = SREG; + cli(); + + if (val == LOW) { + *out &= ~bit; + } else { + *out |= bit; + } + + SREG = oldSREG; +} + +int digitalRead(uint8_t pin) +{ + uint8_t timer = digitalPinToTimer(pin); + uint8_t bit = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + + if (port == NOT_A_PIN) return LOW; + + // If the pin that support PWM output, we need to turn it off + // before getting a digital reading. + if (timer != NOT_ON_TIMER) turnOffPWM(timer); + + if (*portInputRegister(port) & bit) return HIGH; + return LOW; +} diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_private.h b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_private.h new file mode 100644 index 0000000..f678265 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_private.h @@ -0,0 +1,71 @@ +/* + wiring_private.h - Internal header file. + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 239 2007-01-12 17:58:39Z mellis $ +*/ + +#ifndef WiringPrivate_h +#define WiringPrivate_h + +#include +#include +#include +#include + +#include "Arduino.h" + +#ifdef __cplusplus +extern "C"{ +#endif + +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +#define EXTERNAL_INT_0 0 +#define EXTERNAL_INT_1 1 +#define EXTERNAL_INT_2 2 +#define EXTERNAL_INT_3 3 +#define EXTERNAL_INT_4 4 +#define EXTERNAL_INT_5 5 +#define EXTERNAL_INT_6 6 +#define EXTERNAL_INT_7 7 + +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define EXTERNAL_NUM_INTERRUPTS 8 +#elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) +#define EXTERNAL_NUM_INTERRUPTS 3 +#elif defined(__AVR_ATmega32U4__) +#define EXTERNAL_NUM_INTERRUPTS 4 +#else +#define EXTERNAL_NUM_INTERRUPTS 2 +#endif + +typedef void (*voidFuncPtr)(void); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_pulse.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_pulse.c new file mode 100644 index 0000000..0d96886 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_pulse.c @@ -0,0 +1,69 @@ +/* + wiring_pulse.c - pulseIn() function + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ +*/ + +#include "wiring_private.h" +#include "pins_arduino.h" + +/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH + * or LOW, the type of pulse to measure. Works on pulses from 2-3 microseconds + * to 3 minutes in length, but must be called at least a few dozen microseconds + * before the start of the pulse. */ +unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout) +{ + // cache the port and bit of the pin in order to speed up the + // pulse width measuring loop and achieve finer resolution. calling + // digitalRead() instead yields much coarser resolution. + uint8_t bit = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + uint8_t stateMask = (state ? bit : 0); + unsigned long width = 0; // keep initialization out of time critical area + + // convert the timeout from microseconds to a number of times through + // the initial loop; it takes 16 clock cycles per iteration. + unsigned long numloops = 0; + unsigned long maxloops = microsecondsToClockCycles(timeout) / 16; + + // wait for any previous pulse to end + while ((*portInputRegister(port) & bit) == stateMask) + if (numloops++ == maxloops) + return 0; + + // wait for the pulse to start + while ((*portInputRegister(port) & bit) != stateMask) + if (numloops++ == maxloops) + return 0; + + // wait for the pulse to stop + while ((*portInputRegister(port) & bit) == stateMask) { + if (numloops++ == maxloops) + return 0; + width++; + } + + // convert the reading to microseconds. The loop has been determined + // to be 20 clock cycles long and have about 16 clocks between the edge + // and the start of the loop. There will be some error introduced by + // the interrupt handlers. + return clockCyclesToMicroseconds(width * 21 + 16); +} diff --git a/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_shift.c b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_shift.c new file mode 100644 index 0000000..cfe7867 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/cores/arduino/wiring_shift.c @@ -0,0 +1,55 @@ +/* + wiring_shift.c - shiftOut() function + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ +*/ + +#include "wiring_private.h" + +uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder) { + uint8_t value = 0; + uint8_t i; + + for (i = 0; i < 8; ++i) { + digitalWrite(clockPin, HIGH); + if (bitOrder == LSBFIRST) + value |= digitalRead(dataPin) << i; + else + value |= digitalRead(dataPin) << (7 - i); + digitalWrite(clockPin, LOW); + } + return value; +} + +void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val) +{ + uint8_t i; + + for (i = 0; i < 8; i++) { + if (bitOrder == LSBFIRST) + digitalWrite(dataPin, !!(val & (1 << i))); + else + digitalWrite(dataPin, !!(val & (1 << (7 - i)))); + + digitalWrite(clockPin, HIGH); + digitalWrite(clockPin, LOW); + } +} diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/README.txt b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/README.txt new file mode 100644 index 0000000..386dcf0 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/README.txt @@ -0,0 +1,33 @@ +Arduino Uno and Mega 2560 Firmwares for the ATmega8U2 + +This directory contains the firmwares used on the ATmega8U2 on the Arduino +Uno and Arduino Mega 2560. The arduino-usbdfu directory contains the DFU +bootloader on the 8U2; the arduino-usbserial directory contains the actual +usb to serial firmware. Both should be compiled against LUFA 100807. The +two .hex files in this directory combine the dfu and serial firmwares into +a single file to burn onto the 8U2. + +To burn (Uno): +avrdude -p at90usb82 -F -P usb -c avrispmkii -U flash:w:UNO-dfu_and_usbserial_combined.hex -U lfuse:w:0xFF:m -U hfuse:w:0xD9:m -U efuse:w:0xF4:m -U lock:w:0x0F:m + +To burn (Mega 2560): +avrdude -p at90usb82 -F -P usb -c avrispmkii -U flash:w:MEGA-dfu_and_usbserial_combined.hex -U lfuse:w:0xFF:m -U hfuse:w:0xD9:m -U efuse:w:0xF4:m -U lock:w:0x0F:m + + +Note on USB Vendor IDs (VID) and Product IDs (PID): The arduino-usbdfu +project uses Atmel's VID and MCU-specific PIDs to maintain compatibility +with their FLIP software. The source code to the arduino-usbserial +project includes Atmel's VID and a PID donated by them to LUFA. This +PID is used in LUFA's USBtoSerial project, which forms the basis for +arduino-usbserial. According to the LUFA documentation, this VID/PID +combination is: + + "For use in testing of LUFA powered devices during development only, + by non-commercial entities. All devices must accept collisions on this + VID/PID range (from other in-development LUFA devices) to be resolved + by using a unique release number in the Device Descriptor. No devices + using this VID/PID combination may be released to the general public." + +The production version of the arduino-usbserial firmware uses the +Arduino VID. This is only for use with official Arduino hardware and +should not be used on other products. \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.c b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.c new file mode 100644 index 0000000..18c761f --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.c @@ -0,0 +1,728 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Main source file for the DFU class bootloader. This file contains the complete bootloader logic. + */ + +#define INCLUDE_FROM_BOOTLOADER_C +#include "Arduino-usbdfu.h" + +/** Flag to indicate if the bootloader should be running, or should exit and allow the application code to run + * via a soft reset. When cleared, the bootloader will abort, the USB interface will shut down and the application + * jumped to via an indirect jump to location 0x0000 (or other location specified by the host). + */ +bool RunBootloader = true; + +/** Flag to indicate if the bootloader is waiting to exit. When the host requests the bootloader to exit and + * jump to the application address it specifies, it sends two sequential commands which must be properly + * acknowledged. Upon reception of the first the RunBootloader flag is cleared and the WaitForExit flag is set, + * causing the bootloader to wait for the final exit command before shutting down. + */ +bool WaitForExit = false; + +/** Current DFU state machine state, one of the values in the DFU_State_t enum. */ +uint8_t DFU_State = dfuIDLE; + +/** Status code of the last executed DFU command. This is set to one of the values in the DFU_Status_t enum after + * each operation, and returned to the host when a Get Status DFU request is issued. + */ +uint8_t DFU_Status = OK; + +/** Data containing the DFU command sent from the host. */ +DFU_Command_t SentCommand; + +/** Response to the last issued Read Data DFU command. Unlike other DFU commands, the read command + * requires a single byte response from the bootloader containing the read data when the next DFU_UPLOAD command + * is issued by the host. + */ +uint8_t ResponseByte; + +/** Pointer to the start of the user application. By default this is 0x0000 (the reset vector), however the host + * may specify an alternate address when issuing the application soft-start command. + */ +AppPtr_t AppStartPtr = (AppPtr_t)0x0000; + +/** 64-bit flash page number. This is concatenated with the current 16-bit address on USB AVRs containing more than + * 64KB of flash memory. + */ +uint8_t Flash64KBPage = 0; + +/** Memory start address, indicating the current address in the memory being addressed (either FLASH or EEPROM + * depending on the issued command from the host). + */ +uint16_t StartAddr = 0x0000; + +/** Memory end address, indicating the end address to read to/write from in the memory being addressed (either FLASH + * of EEPROM depending on the issued command from the host). + */ +uint16_t EndAddr = 0x0000; + + +/** Pulse generation counters to keep track of the number of milliseconds remaining for each pulse type */ +volatile struct +{ + uint8_t TxLEDPulse; /**< Milliseconds remaining for data Tx LED pulse */ + uint8_t RxLEDPulse; /**< Milliseconds remaining for data Rx LED pulse */ + uint8_t PingPongLEDPulse; /**< Milliseconds remaining for enumeration Tx/Rx ping-pong LED pulse */ +} PulseMSRemaining; + +/** Main program entry point. This routine configures the hardware required by the bootloader, then continuously + * runs the bootloader processing routine until instructed to soft-exit, or hard-reset via the watchdog to start + * the loaded application code. + */ +int main(void) +{ + /* Configure hardware required by the bootloader */ + SetupHardware(); + + /* Enable global interrupts so that the USB stack can function */ + sei(); + + /* Run the USB management task while the bootloader is supposed to be running */ + while (RunBootloader || WaitForExit) + USB_USBTask(); + + /* Reset configured hardware back to their original states for the user application */ + ResetHardware(); + + /* Start the user application */ + AppStartPtr(); +} + +/** Configures all hardware required for the bootloader. */ +void SetupHardware(void) +{ + /* Disable watchdog if enabled by bootloader/fuses */ + MCUSR &= ~(1 << WDRF); + wdt_disable(); + + /* Disable clock division */ +// clock_prescale_set(clock_div_1); + + /* Relocate the interrupt vector table to the bootloader section */ + MCUCR = (1 << IVCE); + MCUCR = (1 << IVSEL); + + LEDs_Init(); + + /* Initialize the USB subsystem */ + USB_Init(); +} + +/** Resets all configured hardware required for the bootloader back to their original states. */ +void ResetHardware(void) +{ + /* Shut down the USB subsystem */ + USB_ShutDown(); + + /* Relocate the interrupt vector table back to the application section */ + MCUCR = (1 << IVCE); + MCUCR = 0; +} + +/** Event handler for the USB_UnhandledControlRequest event. This is used to catch standard and class specific + * control requests that are not handled internally by the USB library (including the DFU commands, which are + * all issued via the control endpoint), so that they can be handled appropriately for the application. + */ +void EVENT_USB_Device_UnhandledControlRequest(void) +{ + /* Get the size of the command and data from the wLength value */ + SentCommand.DataSize = USB_ControlRequest.wLength; + + /* Turn off TX LED(s) once the TX pulse period has elapsed */ + if (PulseMSRemaining.TxLEDPulse && !(--PulseMSRemaining.TxLEDPulse)) + LEDs_TurnOffLEDs(LEDMASK_TX); + + /* Turn off RX LED(s) once the RX pulse period has elapsed */ + if (PulseMSRemaining.RxLEDPulse && !(--PulseMSRemaining.RxLEDPulse)) + LEDs_TurnOffLEDs(LEDMASK_RX); + + switch (USB_ControlRequest.bRequest) + { + case DFU_DNLOAD: + LEDs_TurnOnLEDs(LEDMASK_RX); + PulseMSRemaining.RxLEDPulse = TX_RX_LED_PULSE_MS; + + Endpoint_ClearSETUP(); + + /* Check if bootloader is waiting to terminate */ + if (WaitForExit) + { + /* Bootloader is terminating - process last received command */ + ProcessBootloaderCommand(); + + /* Turn off TX/RX status LEDs so that they're not left on when application starts */ + LEDs_TurnOffLEDs(LEDMASK_TX); + LEDs_TurnOffLEDs(LEDMASK_RX); + + /* Indicate that the last command has now been processed - free to exit bootloader */ + WaitForExit = false; + } + + /* If the request has a data stage, load it into the command struct */ + if (SentCommand.DataSize) + { + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + /* First byte of the data stage is the DNLOAD request's command */ + SentCommand.Command = Endpoint_Read_Byte(); + + /* One byte of the data stage is the command, so subtract it from the total data bytes */ + SentCommand.DataSize--; + + /* Load in the rest of the data stage as command parameters */ + for (uint8_t DataByte = 0; (DataByte < sizeof(SentCommand.Data)) && + Endpoint_BytesInEndpoint(); DataByte++) + { + SentCommand.Data[DataByte] = Endpoint_Read_Byte(); + SentCommand.DataSize--; + } + + /* Process the command */ + ProcessBootloaderCommand(); + } + + /* Check if currently downloading firmware */ + if (DFU_State == dfuDNLOAD_IDLE) + { + if (!(SentCommand.DataSize)) + { + DFU_State = dfuIDLE; + } + else + { + /* Throw away the filler bytes before the start of the firmware */ + DiscardFillerBytes(DFU_FILLER_BYTES_SIZE); + + /* Throw away the packet alignment filler bytes before the start of the firmware */ + DiscardFillerBytes(StartAddr % FIXED_CONTROL_ENDPOINT_SIZE); + + /* Calculate the number of bytes remaining to be written */ + uint16_t BytesRemaining = ((EndAddr - StartAddr) + 1); + + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00)) // Write flash + { + /* Calculate the number of words to be written from the number of bytes to be written */ + uint16_t WordsRemaining = (BytesRemaining >> 1); + + union + { + uint16_t Words[2]; + uint32_t Long; + } CurrFlashAddress = {.Words = {StartAddr, Flash64KBPage}}; + + uint32_t CurrFlashPageStartAddress = CurrFlashAddress.Long; + uint8_t WordsInFlashPage = 0; + + while (WordsRemaining--) + { + /* Check if endpoint is empty - if so clear it and wait until ready for next packet */ + if (!(Endpoint_BytesInEndpoint())) + { + Endpoint_ClearOUT(); + + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Write the next word into the current flash page */ + boot_page_fill(CurrFlashAddress.Long, Endpoint_Read_Word_LE()); + + /* Adjust counters */ + WordsInFlashPage += 1; + CurrFlashAddress.Long += 2; + + /* See if an entire page has been written to the flash page buffer */ + if ((WordsInFlashPage == (SPM_PAGESIZE >> 1)) || !(WordsRemaining)) + { + /* Commit the flash page to memory */ + boot_page_write(CurrFlashPageStartAddress); + boot_spm_busy_wait(); + + /* Check if programming incomplete */ + if (WordsRemaining) + { + CurrFlashPageStartAddress = CurrFlashAddress.Long; + WordsInFlashPage = 0; + + /* Erase next page's temp buffer */ + boot_page_erase(CurrFlashAddress.Long); + boot_spm_busy_wait(); + } + } + } + + /* Once programming complete, start address equals the end address */ + StartAddr = EndAddr; + + /* Re-enable the RWW section of flash */ + boot_rww_enable(); + } + else // Write EEPROM + { + while (BytesRemaining--) + { + /* Check if endpoint is empty - if so clear it and wait until ready for next packet */ + if (!(Endpoint_BytesInEndpoint())) + { + Endpoint_ClearOUT(); + + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Read the byte from the USB interface and write to to the EEPROM */ + eeprom_write_byte((uint8_t*)StartAddr, Endpoint_Read_Byte()); + + /* Adjust counters */ + StartAddr++; + } + } + + /* Throw away the currently unused DFU file suffix */ + DiscardFillerBytes(DFU_FILE_SUFFIX_SIZE); + } + } + + Endpoint_ClearOUT(); + + Endpoint_ClearStatusStage(); + + break; + case DFU_UPLOAD: + Endpoint_ClearSETUP(); + + LEDs_TurnOnLEDs(LEDMASK_TX); + PulseMSRemaining.TxLEDPulse = TX_RX_LED_PULSE_MS; + + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + + if (DFU_State != dfuUPLOAD_IDLE) + { + if ((DFU_State == dfuERROR) && IS_ONEBYTE_COMMAND(SentCommand.Data, 0x01)) // Blank Check + { + /* Blank checking is performed in the DFU_DNLOAD request - if we get here we've told the host + that the memory isn't blank, and the host is requesting the first non-blank address */ + Endpoint_Write_Word_LE(StartAddr); + } + else + { + /* Idle state upload - send response to last issued command */ + Endpoint_Write_Byte(ResponseByte); + } + } + else + { + /* Determine the number of bytes remaining in the current block */ + uint16_t BytesRemaining = ((EndAddr - StartAddr) + 1); + + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00)) // Read FLASH + { + /* Calculate the number of words to be written from the number of bytes to be written */ + uint16_t WordsRemaining = (BytesRemaining >> 1); + + union + { + uint16_t Words[2]; + uint32_t Long; + } CurrFlashAddress = {.Words = {StartAddr, Flash64KBPage}}; + + while (WordsRemaining--) + { + /* Check if endpoint is full - if so clear it and wait until ready for next packet */ + if (Endpoint_BytesInEndpoint() == FIXED_CONTROL_ENDPOINT_SIZE) + { + Endpoint_ClearIN(); + + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Read the flash word and send it via USB to the host */ + #if (FLASHEND > 0xFFFF) + Endpoint_Write_Word_LE(pgm_read_word_far(CurrFlashAddress.Long)); + #else + Endpoint_Write_Word_LE(pgm_read_word(CurrFlashAddress.Long)); + #endif + + /* Adjust counters */ + CurrFlashAddress.Long += 2; + } + + /* Once reading is complete, start address equals the end address */ + StartAddr = EndAddr; + } + else if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x02)) // Read EEPROM + { + while (BytesRemaining--) + { + /* Check if endpoint is full - if so clear it and wait until ready for next packet */ + if (Endpoint_BytesInEndpoint() == FIXED_CONTROL_ENDPOINT_SIZE) + { + Endpoint_ClearIN(); + + while (!(Endpoint_IsINReady())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + + /* Read the EEPROM byte and send it via USB to the host */ + Endpoint_Write_Byte(eeprom_read_byte((uint8_t*)StartAddr)); + + /* Adjust counters */ + StartAddr++; + } + } + + /* Return to idle state */ + DFU_State = dfuIDLE; + } + + Endpoint_ClearIN(); + + Endpoint_ClearStatusStage(); + break; + case DFU_GETSTATUS: + Endpoint_ClearSETUP(); + + /* Write 8-bit status value */ + Endpoint_Write_Byte(DFU_Status); + + /* Write 24-bit poll timeout value */ + Endpoint_Write_Byte(0); + Endpoint_Write_Word_LE(0); + + /* Write 8-bit state value */ + Endpoint_Write_Byte(DFU_State); + + /* Write 8-bit state string ID number */ + Endpoint_Write_Byte(0); + + Endpoint_ClearIN(); + + Endpoint_ClearStatusStage(); + break; + case DFU_CLRSTATUS: + Endpoint_ClearSETUP(); + + /* Reset the status value variable to the default OK status */ + DFU_Status = OK; + + Endpoint_ClearStatusStage(); + break; + case DFU_GETSTATE: + Endpoint_ClearSETUP(); + + /* Write the current device state to the endpoint */ + Endpoint_Write_Byte(DFU_State); + + Endpoint_ClearIN(); + + Endpoint_ClearStatusStage(); + break; + case DFU_ABORT: + Endpoint_ClearSETUP(); + + /* Turn off TX/RX status LEDs so that they're not left on when application starts */ + LEDs_TurnOffLEDs(LEDMASK_TX); + LEDs_TurnOffLEDs(LEDMASK_RX); + + /* Reset the current state variable to the default idle state */ + DFU_State = dfuIDLE; + + Endpoint_ClearStatusStage(); + break; + } +} + +/** Routine to discard the specified number of bytes from the control endpoint stream. This is used to + * discard unused bytes in the stream from the host, including the memory program block suffix. + * + * \param[in] NumberOfBytes Number of bytes to discard from the host from the control endpoint + */ +static void DiscardFillerBytes(uint8_t NumberOfBytes) +{ + while (NumberOfBytes--) + { + if (!(Endpoint_BytesInEndpoint())) + { + Endpoint_ClearOUT(); + + /* Wait until next data packet received */ + while (!(Endpoint_IsOUTReceived())) + { + if (USB_DeviceState == DEVICE_STATE_Unattached) + return; + } + } + else + { + Endpoint_Discard_Byte(); + } + } +} + +/** Routine to process an issued command from the host, via a DFU_DNLOAD request wrapper. This routine ensures + * that the command is allowed based on the current secure mode flag value, and passes the command off to the + * appropriate handler function. + */ +static void ProcessBootloaderCommand(void) +{ + /* Check if device is in secure mode */ +// if (IsSecure) +// { +// /* Don't process command unless it is a READ or chip erase command */ +// if (!(((SentCommand.Command == COMMAND_WRITE) && +// IS_TWOBYTE_COMMAND(SentCommand.Data, 0x00, 0xFF)) || +// (SentCommand.Command == COMMAND_READ))) +// { +// /* Set the state and status variables to indicate the error */ +// DFU_State = dfuERROR; +// DFU_Status = errWRITE; +// +// /* Stall command */ +// Endpoint_StallTransaction(); +// +// /* Don't process the command */ +// return; +// } +// } + + /* Dispatch the required command processing routine based on the command type */ + switch (SentCommand.Command) + { + case COMMAND_PROG_START: + ProcessMemProgCommand(); + break; + case COMMAND_DISP_DATA: + ProcessMemReadCommand(); + break; + case COMMAND_WRITE: + ProcessWriteCommand(); + break; + case COMMAND_READ: + ProcessReadCommand(); + break; + case COMMAND_CHANGE_BASE_ADDR: + if (IS_TWOBYTE_COMMAND(SentCommand.Data, 0x03, 0x00)) // Set 64KB flash page command + Flash64KBPage = SentCommand.Data[2]; + break; + } +} + +/** Routine to concatenate the given pair of 16-bit memory start and end addresses from the host, and store them + * in the StartAddr and EndAddr global variables. + */ +static void LoadStartEndAddresses(void) +{ + union + { + uint8_t Bytes[2]; + uint16_t Word; + } Address[2] = {{.Bytes = {SentCommand.Data[2], SentCommand.Data[1]}}, + {.Bytes = {SentCommand.Data[4], SentCommand.Data[3]}}}; + + /* Load in the start and ending read addresses from the sent data packet */ + StartAddr = Address[0].Word; + EndAddr = Address[1].Word; +} + +/** Handler for a Memory Program command issued by the host. This routine handles the preparations needed + * to write subsequent data from the host into the specified memory. + */ +static void ProcessMemProgCommand(void) +{ + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00) || // Write FLASH command + IS_ONEBYTE_COMMAND(SentCommand.Data, 0x01)) // Write EEPROM command + { + /* Load in the start and ending read addresses */ + LoadStartEndAddresses(); + + /* If FLASH is being written to, we need to pre-erase the first page to write to */ + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00)) + { + union + { + uint16_t Words[2]; + uint32_t Long; + } CurrFlashAddress = {.Words = {StartAddr, Flash64KBPage}}; + + /* Erase the current page's temp buffer */ + boot_page_erase(CurrFlashAddress.Long); + boot_spm_busy_wait(); + } + + /* Set the state so that the next DNLOAD requests reads in the firmware */ + DFU_State = dfuDNLOAD_IDLE; + } +} + +/** Handler for a Memory Read command issued by the host. This routine handles the preparations needed + * to read subsequent data from the specified memory out to the host, as well as implementing the memory + * blank check command. + */ +static void ProcessMemReadCommand(void) +{ + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00) || // Read FLASH command + IS_ONEBYTE_COMMAND(SentCommand.Data, 0x02)) // Read EEPROM command + { + /* Load in the start and ending read addresses */ + LoadStartEndAddresses(); + + /* Set the state so that the next UPLOAD requests read out the firmware */ + DFU_State = dfuUPLOAD_IDLE; + } + else if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x01)) // Blank check FLASH command + { + uint32_t CurrFlashAddress = 0; + + while (CurrFlashAddress < BOOT_START_ADDR) + { + /* Check if the current byte is not blank */ + #if (FLASHEND > 0xFFFF) + if (pgm_read_byte_far(CurrFlashAddress) != 0xFF) + #else + if (pgm_read_byte(CurrFlashAddress) != 0xFF) + #endif + { + /* Save the location of the first non-blank byte for response back to the host */ + Flash64KBPage = (CurrFlashAddress >> 16); + StartAddr = CurrFlashAddress; + + /* Set state and status variables to the appropriate error values */ + DFU_State = dfuERROR; + DFU_Status = errCHECK_ERASED; + + break; + } + + CurrFlashAddress++; + } + } +} + +/** Handler for a Data Write command issued by the host. This routine handles non-programming commands such as + * bootloader exit (both via software jumps and hardware watchdog resets) and flash memory erasure. + */ +static void ProcessWriteCommand(void) +{ + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x03)) // Start application + { + /* Indicate that the bootloader is terminating */ + WaitForExit = true; + + /* Check if data supplied for the Start Program command - no data executes the program */ + if (SentCommand.DataSize) + { + if (SentCommand.Data[1] == 0x01) // Start via jump + { + union + { + uint8_t Bytes[2]; + AppPtr_t FuncPtr; + } Address = {.Bytes = {SentCommand.Data[4], SentCommand.Data[3]}}; + + /* Load in the jump address into the application start address pointer */ + AppStartPtr = Address.FuncPtr; + } + } + else + { + if (SentCommand.Data[1] == 0x00) // Start via watchdog + { + /* Start the watchdog to reset the AVR once the communications are finalized */ + wdt_enable(WDTO_250MS); + } + else // Start via jump + { + /* Set the flag to terminate the bootloader at next opportunity */ + RunBootloader = false; + } + } + } + else if (IS_TWOBYTE_COMMAND(SentCommand.Data, 0x00, 0xFF)) // Erase flash + { + uint32_t CurrFlashAddress = 0; + + /* Clear the application section of flash */ + while (CurrFlashAddress < BOOT_START_ADDR) + { + boot_page_erase(CurrFlashAddress); + boot_spm_busy_wait(); + boot_page_write(CurrFlashAddress); + boot_spm_busy_wait(); + + CurrFlashAddress += SPM_PAGESIZE; + } + + /* Re-enable the RWW section of flash as writing to the flash locks it out */ + boot_rww_enable(); + + /* Memory has been erased, reset the security bit so that programming/reading is allowed */ +// IsSecure = false; + } +} + +/** Handler for a Data Read command issued by the host. This routine handles bootloader information retrieval + * commands such as device signature and bootloader version retrieval. + */ +static void ProcessReadCommand(void) +{ + const uint8_t BootloaderInfo[3] = {BOOTLOADER_VERSION, BOOTLOADER_ID_BYTE1, BOOTLOADER_ID_BYTE2}; + const uint8_t SignatureInfo[3] = {AVR_SIGNATURE_1, AVR_SIGNATURE_2, AVR_SIGNATURE_3}; + + uint8_t DataIndexToRead = SentCommand.Data[1]; + + if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x00)) // Read bootloader info + ResponseByte = BootloaderInfo[DataIndexToRead]; + else if (IS_ONEBYTE_COMMAND(SentCommand.Data, 0x01)) // Read signature byte + ResponseByte = SignatureInfo[DataIndexToRead - 0x30]; +} diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.h new file mode 100644 index 0000000..4adc7e5 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Arduino-usbdfu.h @@ -0,0 +1,220 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Arduino-usbdfu.c. + */ + +#ifndef _ARDUINO_USB_DFU_BOOTLOADER_H_ +#define _ARDUINO_USB_DFU_BOOTLOADER_H_ + + /* Includes: */ + #include + #include + #include + #include + #include + #include + #include + #include + + #include "Descriptors.h" + + #include + #include + + /* Macros: */ + /** LED mask for the library LED driver, to indicate TX activity. */ + #define LEDMASK_TX LEDS_LED1 + + /** LED mask for the library LED driver, to indicate RX activity. */ + #define LEDMASK_RX LEDS_LED2 + + /** LED mask for the library LED driver, to indicate that an error has occurred in the USB interface. */ + #define LEDMASK_ERROR (LEDS_LED1 | LEDS_LED2) + + /** LED mask for the library LED driver, to indicate that the USB interface is busy. */ + #define LEDMASK_BUSY (LEDS_LED1 | LEDS_LED2) + + /** Configuration define. Define this token to true to case the bootloader to reject all memory commands + * until a memory erase has been performed. When used in conjunction with the lockbits of the AVR, this + * can protect the AVR's firmware from being dumped from a secured AVR. When false, memory operations are + * allowed at any time. + */ +// #define SECURE_MODE false + + /** Major bootloader version number. */ + #define BOOTLOADER_VERSION_MINOR 2 + + /** Minor bootloader version number. */ + #define BOOTLOADER_VERSION_REV 0 + + /** Complete bootloader version number expressed as a packed byte, constructed from the + * two individual bootloader version macros. + */ + #define BOOTLOADER_VERSION ((BOOTLOADER_VERSION_MINOR << 4) | BOOTLOADER_VERSION_REV) + + /** First byte of the bootloader identification bytes, used to identify a device's bootloader. */ + #define BOOTLOADER_ID_BYTE1 0xDC + + /** Second byte of the bootloader identification bytes, used to identify a device's bootloader. */ + #define BOOTLOADER_ID_BYTE2 0xFB + + /** Convenience macro, used to determine if the issued command is the given one-byte long command. + * + * \param[in] dataarr Command byte array to check against + * \param[in] cb1 First command byte to check + */ + #define IS_ONEBYTE_COMMAND(dataarr, cb1) (dataarr[0] == (cb1)) + + /** Convenience macro, used to determine if the issued command is the given two-byte long command. + * + * \param[in] dataarr Command byte array to check against + * \param[in] cb1 First command byte to check + * \param[in] cb2 Second command byte to check + */ + #define IS_TWOBYTE_COMMAND(dataarr, cb1, cb2) ((dataarr[0] == (cb1)) && (dataarr[1] == (cb2))) + + /** Length of the DFU file suffix block, appended to the end of each complete memory write command. + * The DFU file suffix is currently unused (but is designed to give extra file information, such as + * a CRC of the complete firmware for error checking) and so is discarded. + */ + #define DFU_FILE_SUFFIX_SIZE 16 + + /** Length of the DFU file filler block, appended to the start of each complete memory write command. + * Filler bytes are added to the start of each complete memory write command, and must be discarded. + */ + #define DFU_FILLER_BYTES_SIZE 26 + + /** DFU class command request to detach from the host. */ + #define DFU_DETATCH 0x00 + + /** DFU class command request to send data from the host to the bootloader. */ + #define DFU_DNLOAD 0x01 + + /** DFU class command request to send data from the bootloader to the host. */ + #define DFU_UPLOAD 0x02 + + /** DFU class command request to get the current DFU status and state from the bootloader. */ + #define DFU_GETSTATUS 0x03 + + /** DFU class command request to reset the current DFU status and state variables to their defaults. */ + #define DFU_CLRSTATUS 0x04 + + /** DFU class command request to get the current DFU state of the bootloader. */ + #define DFU_GETSTATE 0x05 + + /** DFU class command request to abort the current multi-request transfer and return to the dfuIDLE state. */ + #define DFU_ABORT 0x06 + + /** DFU command to begin programming the device's memory. */ + #define COMMAND_PROG_START 0x01 + + /** DFU command to begin reading the device's memory. */ + #define COMMAND_DISP_DATA 0x03 + + /** DFU command to issue a write command. */ + #define COMMAND_WRITE 0x04 + + /** DFU command to issue a read command. */ + #define COMMAND_READ 0x05 + + /** DFU command to issue a memory base address change command, to set the current 64KB flash page + * that subsequent flash operations should use. */ + #define COMMAND_CHANGE_BASE_ADDR 0x06 + + /* Type Defines: */ + /** Type define for a non-returning function pointer to the loaded application. */ + typedef void (*AppPtr_t)(void) ATTR_NO_RETURN; + + /** Type define for a structure containing a complete DFU command issued by the host. */ + typedef struct + { + uint8_t Command; /**< Single byte command to perform, one of the COMMAND_* macro values */ + uint8_t Data[5]; /**< Command parameters */ + uint16_t DataSize; /**< Size of the command parameters */ + } DFU_Command_t; + + /* Enums: */ + /** DFU bootloader states. Refer to the DFU class specification for information on each state. */ + enum DFU_State_t + { + appIDLE = 0, + appDETACH = 1, + dfuIDLE = 2, + dfuDNLOAD_SYNC = 3, + dfuDNBUSY = 4, + dfuDNLOAD_IDLE = 5, + dfuMANIFEST_SYNC = 6, + dfuMANIFEST = 7, + dfuMANIFEST_WAIT_RESET = 8, + dfuUPLOAD_IDLE = 9, + dfuERROR = 10 + }; + + /** DFU command status error codes. Refer to the DFU class specification for information on each error code. */ + enum DFU_Status_t + { + OK = 0, + errTARGET = 1, + errFILE = 2, + errWRITE = 3, + errERASE = 4, + errCHECK_ERASED = 5, + errPROG = 6, + errVERIFY = 7, + errADDRESS = 8, + errNOTDONE = 9, + errFIRMWARE = 10, + errVENDOR = 11, + errUSBR = 12, + errPOR = 13, + errUNKNOWN = 14, + errSTALLEDPKT = 15 + }; + + /* Function Prototypes: */ + void SetupHardware(void); + void ResetHardware(void); + + void EVENT_USB_Device_UnhandledControlRequest(void); + + #if defined(INCLUDE_FROM_BOOTLOADER_C) + static void DiscardFillerBytes(uint8_t NumberOfBytes); + static void ProcessBootloaderCommand(void); + static void LoadStartEndAddresses(void); + static void ProcessMemProgCommand(void); + static void ProcessMemReadCommand(void); + static void ProcessWriteCommand(void); + static void ProcessReadCommand(void); + #endif + +#endif /* _ARDUINO_USB_DFU_BOOTLOADER_H_ */ diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Board/LEDs.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Board/LEDs.h new file mode 100644 index 0000000..41465f2 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Board/LEDs.h @@ -0,0 +1,110 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/* + Board LEDs driver for the Benito board, from www.dorkbotpdx.org. +*/ + +#ifndef __LEDS_ARDUINOUNO_H__ +#define __LEDS_ARDUINOUNO_H__ + + /* Includes: */ + #include + +/* Enable C linkage for C++ Compilers: */ + #if defined(__cplusplus) + extern "C" { + #endif + + /* Preprocessor Checks: */ + #if !defined(INCLUDE_FROM_LEDS_H) + #error Do not include this file directly. Include LUFA/Drivers/Board/LEDS.h instead. + #endif + + /* Public Interface - May be used in end-application: */ + /* Macros: */ + /** LED mask for the first LED on the board. */ + #define LEDS_LED1 (1 << 5) + + /** LED mask for the second LED on the board. */ + #define LEDS_LED2 (1 << 4) + + /** LED mask for all the LEDs on the board. */ + #define LEDS_ALL_LEDS (LEDS_LED1 | LEDS_LED2) + + /** LED mask for the none of the board LEDs */ + #define LEDS_NO_LEDS 0 + + /* Inline Functions: */ + #if !defined(__DOXYGEN__) + static inline void LEDs_Init(void) + { + DDRD |= LEDS_ALL_LEDS; + PORTD |= LEDS_ALL_LEDS; + } + + static inline void LEDs_TurnOnLEDs(const uint8_t LEDMask) + { + PORTD &= ~LEDMask; + } + + static inline void LEDs_TurnOffLEDs(const uint8_t LEDMask) + { + PORTD |= LEDMask; + } + + static inline void LEDs_SetAllLEDs(const uint8_t LEDMask) + { + PORTD = ((PORTD | LEDS_ALL_LEDS) & ~LEDMask); + } + + static inline void LEDs_ChangeLEDs(const uint8_t LEDMask, const uint8_t ActiveMask) + { + PORTD = ((PORTD | ActiveMask) & ~LEDMask); + } + + static inline void LEDs_ToggleLEDs(const uint8_t LEDMask) + { + PORTD ^= LEDMask; + } + + static inline uint8_t LEDs_GetLEDs(void) ATTR_WARN_UNUSED_RESULT; + static inline uint8_t LEDs_GetLEDs(void) + { + return (PORTD & LEDS_ALL_LEDS); + } + #endif + + /* Disable C linkage for C++ Compilers: */ + #if defined(__cplusplus) + } + #endif + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.c b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.c new file mode 100644 index 0000000..1ec1cd2 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.c @@ -0,0 +1,189 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * USB Device Descriptors, for library use when in USB device mode. Descriptors are special + * computer-readable structures which the host requests upon device enumeration, to determine + * the device's capabilities and functions. + */ + +#include "Descriptors.h" + +/** Device descriptor structure. This descriptor, located in FLASH memory, describes the overall + * device characteristics, including the supported USB version, control endpoint size and the + * number of device configurations. The descriptor is read out by the USB host when the enumeration + * process begins. + */ +USB_Descriptor_Device_t DeviceDescriptor = +{ + .Header = {.Size = sizeof(USB_Descriptor_Device_t), .Type = DTYPE_Device}, + + .USBSpecification = VERSION_BCD(01.10), + .Class = 0x00, + .SubClass = 0x00, + .Protocol = 0x00, + + .Endpoint0Size = FIXED_CONTROL_ENDPOINT_SIZE, + + .VendorID = 0x03EB, // Atmel + .ProductID = PRODUCT_ID_CODE, // MCU-dependent + .ReleaseNumber = 0x0000, + + .ManufacturerStrIndex = NO_DESCRIPTOR, + .ProductStrIndex = 0x01, + .SerialNumStrIndex = NO_DESCRIPTOR, + + .NumberOfConfigurations = FIXED_NUM_CONFIGURATIONS +}; + +/** Configuration descriptor structure. This descriptor, located in FLASH memory, describes the usage + * of the device in one of its supported configurations, including information about any device interfaces + * and endpoints. The descriptor is read out by the USB host during the enumeration process when selecting + * a configuration so that the host may correctly communicate with the USB device. + */ +USB_Descriptor_Configuration_t ConfigurationDescriptor = +{ + .Config = + { + .Header = {.Size = sizeof(USB_Descriptor_Configuration_Header_t), .Type = DTYPE_Configuration}, + + .TotalConfigurationSize = sizeof(USB_Descriptor_Configuration_t), + .TotalInterfaces = 1, + + .ConfigurationNumber = 1, + .ConfigurationStrIndex = NO_DESCRIPTOR, + + .ConfigAttributes = USB_CONFIG_ATTR_BUSPOWERED, + + .MaxPowerConsumption = USB_CONFIG_POWER_MA(100) + }, + + .DFU_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 0, + .AlternateSetting = 0, + + .TotalEndpoints = 0, + + .Class = 0xFE, + .SubClass = 0x01, + .Protocol = 0x02, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .DFU_Functional = + { + .Header = {.Size = sizeof(USB_DFU_Functional_Descriptor_t), .Type = DTYPE_DFUFunctional}, + + .Attributes = (ATTR_CAN_UPLOAD | ATTR_CAN_DOWNLOAD), + + .DetachTimeout = 0x0000, + .TransferSize = 0x0c00, + + .DFUSpecification = VERSION_BCD(01.01) + } +}; + +/** Language descriptor structure. This descriptor, located in FLASH memory, is returned when the host requests + * the string descriptor with index 0 (the first index). It is actually an array of 16-bit integers, which indicate + * via the language ID table available at USB.org what languages the device supports for its string descriptors. + */ +USB_Descriptor_String_t LanguageString = +{ + .Header = {.Size = USB_STRING_LEN(1), .Type = DTYPE_String}, + + .UnicodeString = {LANGUAGE_ID_ENG} +}; + +/** Product descriptor string. This is a Unicode string containing the product's details in human readable form, + * and is read out upon request by the host when the appropriate string ID is requested, listed in the Device + * Descriptor. + */ +USB_Descriptor_String_t ProductString = +{ + #if (ARDUINO_MODEL_PID == ARDUINO_UNO_PID) + .Header = {.Size = USB_STRING_LEN(15), .Type = DTYPE_String}, + + .UnicodeString = L"Arduino Uno DFU" + #elif (ARDUINO_MODEL_PID == ARDUINO_MEGA2560_PID) + .Header = {.Size = USB_STRING_LEN(21), .Type = DTYPE_String}, + + .UnicodeString = L"Arduino Mega 2560 DFU" + #endif +}; + +/** This function is called by the library when in device mode, and must be overridden (see library "USB Descriptors" + * documentation) by the application code so that the address and size of a requested descriptor can be given + * to the USB library. When the device receives a Get Descriptor request on the control endpoint, this function + * is called so that the descriptor details can be passed back and the appropriate descriptor sent back to the + * USB host. + */ +uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + void** const DescriptorAddress) +{ + const uint8_t DescriptorType = (wValue >> 8); + const uint8_t DescriptorNumber = (wValue & 0xFF); + + void* Address = NULL; + uint16_t Size = NO_DESCRIPTOR; + + switch (DescriptorType) + { + case DTYPE_Device: + Address = &DeviceDescriptor; + Size = sizeof(USB_Descriptor_Device_t); + break; + case DTYPE_Configuration: + Address = &ConfigurationDescriptor; + Size = sizeof(USB_Descriptor_Configuration_t); + break; + case DTYPE_String: + if (!(DescriptorNumber)) + { + Address = &LanguageString; + Size = LanguageString.Header.Size; + } + else + { + Address = &ProductString; + Size = ProductString.Header.Size; + } + + break; + } + + *DescriptorAddress = Address; + return Size; +} diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.h new file mode 100644 index 0000000..cb3a8ca --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/Descriptors.h @@ -0,0 +1,177 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Descriptors.c. + */ + +#ifndef _DESCRIPTORS_H_ +#define _DESCRIPTORS_H_ + + /* Includes: */ + #include + + /* Product-specific definitions: */ + #define ARDUINO_UNO_PID 0x0001 + #define ARDUINO_MEGA2560_PID 0x0010 + + /* Macros: */ + /** Descriptor type value for a DFU class functional descriptor. */ + #define DTYPE_DFUFunctional 0x21 + + /** DFU attribute mask, indicating that the DFU device will detach and re-attach when a DFU_DETACH + * command is issued, rather than the host issuing a USB Reset. + */ + #define ATTR_WILL_DETATCH (1 << 3) + + /** DFU attribute mask, indicating that the DFU device can communicate during the manifestation phase + * (memory programming phase). + */ + #define ATTR_MANEFESTATION_TOLLERANT (1 << 2) + + /** DFU attribute mask, indicating that the DFU device can accept DFU_UPLOAD requests to send data from + * the device to the host. + */ + #define ATTR_CAN_UPLOAD (1 << 1) + + /** DFU attribute mask, indicating that the DFU device can accept DFU_DNLOAD requests to send data from + * the host to the device. + */ + #define ATTR_CAN_DOWNLOAD (1 << 0) + + #if defined(__AVR_AT90USB1287__) + #define PRODUCT_ID_CODE 0x2FFB + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB1286__) + #define PRODUCT_ID_CODE 0x2FFB + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x97 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB647__) + #define PRODUCT_ID_CODE 0x2FF9 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB646__) + #define PRODUCT_ID_CODE 0x2FF9 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x96 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_ATmega32U6__) + #define PRODUCT_ID_CODE 0x2FFB + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega32U4__) + #define PRODUCT_ID_CODE 0x2FF4 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x87 + #elif defined(__AVR_ATmega32U2__) + #define PRODUCT_ID_CODE 0x2FF0 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x95 + #define AVR_SIGNATURE_3 0x8A + #elif defined(__AVR_ATmega16U4__) + #define PRODUCT_ID_CODE 0x2FF3 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x88 + #elif defined(__AVR_ATmega16U2__) + #define PRODUCT_ID_CODE 0x2FEF + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_AT90USB162__) + #define PRODUCT_ID_CODE 0x2FFA + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x94 + #define AVR_SIGNATURE_3 0x82 + #elif defined(__AVR_AT90USB82__) + #define PRODUCT_ID_CODE 0x2FEE + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x93 + #define AVR_SIGNATURE_3 0x89 + #elif defined(__AVR_ATmega8U2__) + #define PRODUCT_ID_CODE 0x2FF7 + #define AVR_SIGNATURE_1 0x1E + #define AVR_SIGNATURE_2 0x93 + #define AVR_SIGNATURE_3 0x82 + #else + #error The selected AVR part is not currently supported by this bootloader. + #endif + + #if !defined(PRODUCT_ID_CODE) + #error Current AVR model is not supported by this bootloader. + #endif + + /* Type Defines: */ + /** Type define for a DFU class function descriptor. This descriptor gives DFU class information + * to the host when read, indicating the DFU device's capabilities. + */ + typedef struct + { + USB_Descriptor_Header_t Header; /**< Standard descriptor header structure */ + + uint8_t Attributes; /**< DFU device attributes, a mask comprising of the + * ATTR_* macros listed in this source file + */ + uint16_t DetachTimeout; /**< Timeout in milliseconds between a USB_DETACH + * command being issued and the device detaching + * from the USB bus + */ + uint16_t TransferSize; /**< Maximum number of bytes the DFU device can accept + * from the host in a transaction + */ + uint16_t DFUSpecification; /**< BCD packed DFU specification number this DFU + * device complies with + */ + } USB_DFU_Functional_Descriptor_t; + + /** Type define for the device configuration descriptor structure. This must be defined in the + * application code, as the configuration descriptor contains several sub-descriptors which + * vary between devices, and which describe the device's usage to the host. + */ + typedef struct + { + USB_Descriptor_Configuration_Header_t Config; + USB_Descriptor_Interface_t DFU_Interface; + USB_DFU_Functional_Descriptor_t DFU_Functional; + } USB_Descriptor_Configuration_t; + + /* Function Prototypes: */ + uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + void** const DescriptorAddress) ATTR_WARN_UNUSED_RESULT ATTR_NON_NULL_PTR_ARG(3); + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/makefile b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/makefile new file mode 100644 index 0000000..04a0521 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/makefile @@ -0,0 +1,710 @@ +# Hey Emacs, this is a -*- makefile -*- +#---------------------------------------------------------------------------- +# WinAVR Makefile Template written by Eric B. Weddington, Jrg Wunsch, et al. +# >> Modified for use with the LUFA project. << +# +# Released to the Public Domain +# +# Additional material for this makefile was written by: +# Peter Fleury +# Tim Henigan +# Colin O'Flynn +# Reiner Patommel +# Markus Pfaff +# Sander Pool +# Frederik Rouleau +# Carlos Lamas +# Dean Camera +# Opendous Inc. +# Denver Gingerich +# +#---------------------------------------------------------------------------- +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF. +# +# make extcoff = Convert ELF to AVR Extended COFF. +# +# make program = Download the hex file to the device, using avrdude. +# Please customize the avrdude settings below first! +# +# make doxygen = Generate DoxyGen documentation for the project (must have +# DoxyGen installed) +# +# make debug = Start either simulavr or avarice as specified for debugging, +# with avr-gdb or avr-insight as the front end for debugging. +# +# make filename.s = Just compile filename.c into the assembler code only. +# +# make filename.i = Create a preprocessed source file for use in submitting +# bug reports to the GCC project. +# +# To rebuild project do "make clean" then "make all". +#---------------------------------------------------------------------------- + + +# MCU name +MCU = atmega8u2 +MCU_AVRDUDE = at90usb82 + +# Specify the Arduino model using the assigned PID. This is used by Descriptors.c +# to set the product descriptor string (for DFU we must use the PID for each +# chip that dfu-bootloader or Flip expect) +# Uno PID: +ARDUINO_MODEL_PID = 0x0001 +# Mega 2560 PID: +#ARDUINO_MODEL_PID = 0x0010 + +# Target board (see library "Board Types" documentation, NONE for projects not requiring +# LUFA board drivers). If USER is selected, put custom board drivers in a directory called +# "Board" inside the application directory. +BOARD = USER + + +# Processor frequency. +# This will define a symbol, F_CPU, in all source code files equal to the +# processor frequency in Hz. You can then use this symbol in your source code to +# calculate timings. Do NOT tack on a 'UL' at the end, this will be done +# automatically to create a 32-bit value in your source code. +# +# This will be an integer division of F_CLOCK below, as it is sourced by +# F_CLOCK after it has run through any CPU prescalers. Note that this value +# does not *change* the processor frequency - it should merely be updated to +# reflect the processor speed set externally so that the code can use accurate +# software delays. +F_CPU = 16000000 + + +# Input clock frequency. +# This will define a symbol, F_CLOCK, in all source code files equal to the +# input clock frequency (before any prescaling is performed) in Hz. This value may +# differ from F_CPU if prescaling is used on the latter, and is required as the +# raw input clock is fed directly to the PLL sections of the AVR for high speed +# clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL' +# at the end, this will be done automatically to create a 32-bit value in your +# source code. +# +# If no clock division is performed on the input clock inside the AVR (via the +# CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU. +F_CLOCK = $(F_CPU) + + +# Starting byte address of the bootloader, as a byte address - computed via the formula +# BOOT_START = ((TOTAL_FLASH_BYTES - BOOTLOADER_SECTION_SIZE_BYTES) * 1024) +# +# Note that the bootloader size and start address given in AVRStudio is in words and not +# bytes, and so will need to be doubled to obtain the byte address needed by AVR-GCC. +BOOT_START = 0x1000 + + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + + +# Target file name (without extension). +TARGET = Arduino-usbdfu + + +# Object files directory +# To put object files in current directory, use a dot (.), do NOT make +# this an empty or blank macro! +OBJDIR = . + + +# Path to the LUFA library +LUFA_PATH = ../.. + + +# LUFA library compile-time options and predefined tokens +LUFA_OPTS = -D USB_DEVICE_ONLY +LUFA_OPTS += -D DEVICE_STATE_AS_GPIOR=0 +LUFA_OPTS += -D CONTROL_ONLY_DEVICE +LUFA_OPTS += -D FIXED_CONTROL_ENDPOINT_SIZE=32 +LUFA_OPTS += -D FIXED_NUM_CONFIGURATIONS=1 +LUFA_OPTS += -D USE_RAM_DESCRIPTORS +LUFA_OPTS += -D USE_STATIC_OPTIONS="(USB_DEVICE_OPT_FULLSPEED | USB_OPT_REG_ENABLED | USB_OPT_AUTO_PLL)" +LUFA_OPTS += -D NO_INTERNAL_SERIAL +LUFA_OPTS += -D NO_DEVICE_SELF_POWER +LUFA_OPTS += -D NO_DEVICE_REMOTE_WAKEUP +LUFA_OPTS += -D NO_STREAM_CALLBACKS + + +# Create the LUFA source path variables by including the LUFA root makefile +include $(LUFA_PATH)/LUFA/makefile + + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c \ + Descriptors.c \ + $(LUFA_SRC_USB) \ + + +# List C++ source files here. (C dependencies are automatically generated.) +CPPSRC = + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + + +# Debugging format. +# Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. +# AVR Studio 4.10 requires dwarf-2. +# AVR [Extended] COFF format requires stabs, plus an avr-objcopy run. +DEBUG = dwarf-2 + + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRAINCDIRS = $(LUFA_PATH)/ + + +# Compiler flag to set the C Standard level. +# c89 = "ANSI" C +# gnu89 = c89 plus GCC extensions +# c99 = ISO C99 standard (not yet fully implemented) +# gnu99 = c99 plus GCC extensions +CSTANDARD = -std=c99 + + +# Place -D or -U options here for C sources +CDEFS = -DF_CPU=$(F_CPU)UL +CDEFS += -DARDUINO_MODEL_PID=$(ARDUINO_MODEL_PID) +CDEFS += -DF_CLOCK=$(F_CLOCK)UL +CDEFS += -DBOARD=BOARD_$(BOARD) +CDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CDEFS += -DTX_RX_LED_PULSE_MS=3 +CDEFS += $(LUFA_OPTS) + + +# Place -D or -U options here for ASM sources +ADEFS = -DF_CPU=$(F_CPU) +ADEFS += -DF_CLOCK=$(F_CLOCK)UL +ADEFS += -DBOARD=BOARD_$(BOARD) +CDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +ADEFS += $(LUFA_OPTS) + +# Place -D or -U options here for C++ sources +CPPDEFS = -DF_CPU=$(F_CPU)UL +CPPDEFS += -DF_CLOCK=$(F_CLOCK)UL +CPPDEFS += -DBOARD=BOARD_$(BOARD) +CDEFS += -DBOOT_START_ADDR=$(BOOT_START)UL +CPPDEFS += $(LUFA_OPTS) +#CPPDEFS += -D__STDC_LIMIT_MACROS +#CPPDEFS += -D__STDC_CONSTANT_MACROS + + + +#---------------- Compiler Options C ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char +CFLAGS += -funsigned-bitfields +CFLAGS += -ffunction-sections +CFLAGS += -fno-inline-small-functions +CFLAGS += -fpack-struct +CFLAGS += -fshort-enums +CFLAGS += -fno-strict-aliasing +CFLAGS += -Wall +CFLAGS += -Wstrict-prototypes +#CFLAGS += -mshort-calls +#CFLAGS += -fno-unit-at-a-time +#CFLAGS += -Wundef +#CFLAGS += -Wunreachable-code +#CFLAGS += -Wsign-compare +CFLAGS += -Wa,-adhlns=$(<:%.c=$(OBJDIR)/%.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) + + +#---------------- Compiler Options C++ ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CPPFLAGS = -g$(DEBUG) +CPPFLAGS += $(CPPDEFS) +CPPFLAGS += -O$(OPT) +CPPFLAGS += -funsigned-char +CPPFLAGS += -funsigned-bitfields +CPPFLAGS += -fpack-struct +CPPFLAGS += -fshort-enums +CPPFLAGS += -fno-exceptions +CPPFLAGS += -Wall +CPPFLAGS += -Wundef +#CPPFLAGS += -mshort-calls +#CPPFLAGS += -fno-unit-at-a-time +#CPPFLAGS += -Wstrict-prototypes +#CPPFLAGS += -Wunreachable-code +#CPPFLAGS += -Wsign-compare +CPPFLAGS += -Wa,-adhlns=$(<:%.cpp=$(OBJDIR)/%.lst) +CPPFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +#CPPFLAGS += $(CSTANDARD) + + +#---------------- Assembler Options ---------------- +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +# -listing-cont-lines: Sets the maximum number of continuation lines of hex +# dump that will be displayed for a given single line of source input. +ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:%.S=$(OBJDIR)/%.lst),-gstabs,--listing-cont-lines=100 + + +#---------------- Library Options ---------------- +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt + +# If this is left blank, then it will use the Standard printf version. +PRINTF_LIB = +#PRINTF_LIB = $(PRINTF_LIB_MIN) +#PRINTF_LIB = $(PRINTF_LIB_FLOAT) + + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +# If this is left blank, then it will use the Standard scanf version. +SCANF_LIB = +#SCANF_LIB = $(SCANF_LIB_MIN) +#SCANF_LIB = $(SCANF_LIB_FLOAT) + + +MATH_LIB = -lm + + +# List any extra directories to look for libraries here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRALIBDIRS = + + + +#---------------- External Memory Options ---------------- + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--section-start,.data=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + + + +#---------------- Linker Options ---------------- +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += -Wl,--section-start=.text=$(BOOT_START) +LDFLAGS += -Wl,--relax +LDFLAGS += -Wl,--gc-sections +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(patsubst %,-L%,$(EXTRALIBDIRS)) +LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) +#LDFLAGS += -T linker_script.x + + + +#---------------- Programming Options (avrdude) ---------------- + +# Fuse settings for Arduino Uno DFU bootloader project +AVRDUDE_FUSES = -U efuse:w:0xF4:m -U hfuse:w:0xD9:m -U lfuse:w:0xFF:m + +# Lock settings for Arduino Uno DFU bootloader project +AVRDUDE_LOCK = -U lock:w:0x0F:m + +# Programming hardware +# Type: avrdude -c ? +# to get a full listing. +# +AVRDUDE_PROGRAMMER = avrispmkii + +# com1 = serial port. Use lpt1 to connect to parallel port. +AVRDUDE_PORT = usb + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FLAGS = -p $(MCU_AVRDUDE) -F -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) + + + +#---------------- Debugging Options ---------------- + +# For simulavr only - target MCU frequency. +DEBUG_MFREQ = $(F_CPU) + +# Set the DEBUG_UI to either gdb or insight. +# DEBUG_UI = gdb +DEBUG_UI = insight + +# Set the debugging back-end to either avarice, simulavr. +DEBUG_BACKEND = avarice +#DEBUG_BACKEND = simulavr + +# GDB Init Filename. +GDBINIT_FILE = __avr_gdbinit + +# When using avarice settings for the JTAG +JTAG_DEV = /dev/com1 + +# Debugging port used to communicate between GDB / avarice / simulavr. +DEBUG_PORT = 4242 + +# Debugging host used to communicate between GDB / avarice / simulavr, normally +# just set to localhost unless doing some sort of crazy debugging when +# avarice is running on a different computer. +DEBUG_HOST = localhost + + + +#============================================================================ + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +AR = avr-ar rcs +NM = avr-nm +AVRDUDE = avrdude +REMOVE = rm -f +REMOVEDIR = rm -rf +COPY = cp +WINSHELL = cmd + + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling C: +MSG_COMPILING_CPP = Compiling C++: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: +MSG_CREATING_LIBRARY = Creating library: + + + + +# Define all object files. +OBJ = $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + +# Define all listing files. +LST = $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + + +# Compiler flags to generate dependency files. +GENDEPFLAGS = -MMD -MP -MF .dep/$(@F).d + + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_CPPFLAGS = -mmcu=$(MCU) -I. -x c++ $(CPPFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + + + +# Default target. +all: begin gccversion sizebefore build sizeafter end + +# Change the build target to build a HEX file or a library. +build: elf hex eep lss sym +#build: lib + + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym +LIBNAME=lib$(TARGET).a +lib: $(LIBNAME) + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) $(MCU_FLAG) $(FORMAT_FLAG) $(TARGET).elf +MCU_FLAG = $(shell $(SIZE) --help | grep -- --mcu > /dev/null && echo --mcu=$(MCU) ) +FORMAT_FLAG = $(shell $(SIZE) --help | grep -- --format=.*avr > /dev/null && echo --format=avr ) + + +sizebefore: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +sizeafter: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \ + 2>/dev/null; echo; fi + + + +# Display compiler version information. +gccversion : + @$(CC) --version + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) $(AVRDUDE_FUSES) $(AVRDUDE_LOCK) + + +# Generate avr-gdb config/init file which does the following: +# define the reset signal, load the target file, connect to target, and set +# a breakpoint at main(). +gdb-config: + @$(REMOVE) $(GDBINIT_FILE) + @echo define reset >> $(GDBINIT_FILE) + @echo SIGNAL SIGHUP >> $(GDBINIT_FILE) + @echo end >> $(GDBINIT_FILE) + @echo file $(TARGET).elf >> $(GDBINIT_FILE) + @echo target remote $(DEBUG_HOST):$(DEBUG_PORT) >> $(GDBINIT_FILE) +ifeq ($(DEBUG_BACKEND),simulavr) + @echo load >> $(GDBINIT_FILE) +endif + @echo break main >> $(GDBINIT_FILE) + +debug: gdb-config $(TARGET).elf +ifeq ($(DEBUG_BACKEND), avarice) + @echo Starting AVaRICE - Press enter when "waiting to connect" message displays. + @$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \ + $(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT) + @$(WINSHELL) /c pause + +else + @$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \ + $(DEBUG_MFREQ) --port $(DEBUG_PORT) +endif + @$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE) + + + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT = $(OBJCOPY) --debugging +COFFCONVERT += --change-section-address .data-0x800000 +COFFCONVERT += --change-section-address .bss-0x800000 +COFFCONVERT += --change-section-address .noinit-0x800000 +COFFCONVERT += --change-section-address .eeprom-0x810000 + + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 --no-change-warnings -O $(FORMAT) $< $@ || exit 0 + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S -z $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Create library from object files. +.SECONDARY : $(TARGET).a +.PRECIOUS : $(OBJ) +%.a: $(OBJ) + @echo + @echo $(MSG_CREATING_LIBRARY) $@ + $(AR) $@ $(OBJ) + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +$(OBJDIR)/%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create object files from C++ source files. +$(OBJDIR)/%.o : %.cpp + @echo + @echo $(MSG_COMPILING_CPP) $< + $(CC) -c $(ALL_CPPFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C++ source files. +%.s : %.cpp + $(CC) -S $(ALL_CPPFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +$(OBJDIR)/%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + +# Create preprocessed source for use in sending a bug report. +%.i : %.c + $(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ + + +# Target: clean project. +clean: begin clean_list end + +clean_list : + @echo + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lss + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.o) + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.lst) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) $(SRC:.c=.i) + $(REMOVEDIR) .dep + +doxygen: + @echo Generating Project Documentation... + @doxygen Doxygen.conf + @echo Documentation Generation Complete. + +clean_doxygen: + rm -rf Documentation + +# Create object files directory +$(shell mkdir $(OBJDIR) 2>/dev/null) + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff doxygen clean \ +clean_list clean_doxygen program debug gdb-config diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/readme.txt b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/readme.txt new file mode 100644 index 0000000..e376679 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbdfu/readme.txt @@ -0,0 +1,7 @@ +To setup the project and program an ATMEG8U2 with the Arduino USB DFU bootloader: +1. unpack the source into LUFA's Bootloader directory +2. set ARDUINO_MODEL_PID in the makefile as appropriate +3. do "make clean; make; make program" + +Check that the board enumerates as either "Arduino Uno DFU" or "Arduino Mega 2560 DFU". Test by uploading the Arduino-usbserial application firmware (see instructions in Arduino-usbserial directory) + diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.c b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.c new file mode 100644 index 0000000..efa9998 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.c @@ -0,0 +1,242 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Main source file for the Arduino-usbserial project. This file contains the main tasks of + * the project and is responsible for the initial application hardware configuration. + */ + +#include "Arduino-usbserial.h" + +/** Circular buffer to hold data from the host before it is sent to the device via the serial port. */ +RingBuff_t USBtoUSART_Buffer; + +/** Circular buffer to hold data from the serial port before it is sent to the host. */ +RingBuff_t USARTtoUSB_Buffer; + +/** Pulse generation counters to keep track of the number of milliseconds remaining for each pulse type */ +volatile struct +{ + uint8_t TxLEDPulse; /**< Milliseconds remaining for data Tx LED pulse */ + uint8_t RxLEDPulse; /**< Milliseconds remaining for data Rx LED pulse */ + uint8_t PingPongLEDPulse; /**< Milliseconds remaining for enumeration Tx/Rx ping-pong LED pulse */ +} PulseMSRemaining; + +/** LUFA CDC Class driver interface configuration and state information. This structure is + * passed to all CDC Class driver functions, so that multiple instances of the same class + * within a device can be differentiated from one another. + */ +USB_ClassInfo_CDC_Device_t VirtualSerial_CDC_Interface = + { + .Config = + { + .ControlInterfaceNumber = 0, + + .DataINEndpointNumber = CDC_TX_EPNUM, + .DataINEndpointSize = CDC_TXRX_EPSIZE, + .DataINEndpointDoubleBank = false, + + .DataOUTEndpointNumber = CDC_RX_EPNUM, + .DataOUTEndpointSize = CDC_TXRX_EPSIZE, + .DataOUTEndpointDoubleBank = false, + + .NotificationEndpointNumber = CDC_NOTIFICATION_EPNUM, + .NotificationEndpointSize = CDC_NOTIFICATION_EPSIZE, + .NotificationEndpointDoubleBank = false, + }, + }; + +/** Main program entry point. This routine contains the overall program flow, including initial + * setup of all components and the main program loop. + */ +int main(void) +{ + SetupHardware(); + + RingBuffer_InitBuffer(&USBtoUSART_Buffer); + RingBuffer_InitBuffer(&USARTtoUSB_Buffer); + + sei(); + + for (;;) + { + /* Only try to read in bytes from the CDC interface if the transmit buffer is not full */ + if (!(RingBuffer_IsFull(&USBtoUSART_Buffer))) + { + int16_t ReceivedByte = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface); + + /* Read bytes from the USB OUT endpoint into the USART transmit buffer */ + if (!(ReceivedByte < 0)) + RingBuffer_Insert(&USBtoUSART_Buffer, ReceivedByte); + } + + /* Check if the UART receive buffer flush timer has expired or the buffer is nearly full */ + RingBuff_Count_t BufferCount = RingBuffer_GetCount(&USARTtoUSB_Buffer); + if ((TIFR0 & (1 << TOV0)) || (BufferCount > BUFFER_NEARLY_FULL)) + { + TIFR0 |= (1 << TOV0); + + if (USARTtoUSB_Buffer.Count) { + LEDs_TurnOnLEDs(LEDMASK_TX); + PulseMSRemaining.TxLEDPulse = TX_RX_LED_PULSE_MS; + } + + /* Read bytes from the USART receive buffer into the USB IN endpoint */ + while (BufferCount--) + CDC_Device_SendByte(&VirtualSerial_CDC_Interface, RingBuffer_Remove(&USARTtoUSB_Buffer)); + + /* Turn off TX LED(s) once the TX pulse period has elapsed */ + if (PulseMSRemaining.TxLEDPulse && !(--PulseMSRemaining.TxLEDPulse)) + LEDs_TurnOffLEDs(LEDMASK_TX); + + /* Turn off RX LED(s) once the RX pulse period has elapsed */ + if (PulseMSRemaining.RxLEDPulse && !(--PulseMSRemaining.RxLEDPulse)) + LEDs_TurnOffLEDs(LEDMASK_RX); + } + + /* Load the next byte from the USART transmit buffer into the USART */ + if (!(RingBuffer_IsEmpty(&USBtoUSART_Buffer))) { + Serial_TxByte(RingBuffer_Remove(&USBtoUSART_Buffer)); + + LEDs_TurnOnLEDs(LEDMASK_RX); + PulseMSRemaining.RxLEDPulse = TX_RX_LED_PULSE_MS; + } + + CDC_Device_USBTask(&VirtualSerial_CDC_Interface); + USB_USBTask(); + } +} + +/** Configures the board hardware and chip peripherals for the demo's functionality. */ +void SetupHardware(void) +{ + /* Disable watchdog if enabled by bootloader/fuses */ + MCUSR &= ~(1 << WDRF); + wdt_disable(); + + /* Hardware Initialization */ + Serial_Init(9600, false); + LEDs_Init(); + USB_Init(); + + /* Start the flush timer so that overflows occur rapidly to push received bytes to the USB interface */ + TCCR0B = (1 << CS02); + + /* Pull target /RESET line high */ + AVR_RESET_LINE_PORT |= AVR_RESET_LINE_MASK; + AVR_RESET_LINE_DDR |= AVR_RESET_LINE_MASK; +} + +/** Event handler for the library USB Configuration Changed event. */ +void EVENT_USB_Device_ConfigurationChanged(void) +{ + CDC_Device_ConfigureEndpoints(&VirtualSerial_CDC_Interface); +} + +/** Event handler for the library USB Unhandled Control Request event. */ +void EVENT_USB_Device_UnhandledControlRequest(void) +{ + CDC_Device_ProcessControlRequest(&VirtualSerial_CDC_Interface); +} + +/** Event handler for the CDC Class driver Line Encoding Changed event. + * + * \param[in] CDCInterfaceInfo Pointer to the CDC class interface configuration structure being referenced + */ +void EVENT_CDC_Device_LineEncodingChanged(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo) +{ + uint8_t ConfigMask = 0; + + switch (CDCInterfaceInfo->State.LineEncoding.ParityType) + { + case CDC_PARITY_Odd: + ConfigMask = ((1 << UPM11) | (1 << UPM10)); + break; + case CDC_PARITY_Even: + ConfigMask = (1 << UPM11); + break; + } + + if (CDCInterfaceInfo->State.LineEncoding.CharFormat == CDC_LINEENCODING_TwoStopBits) + ConfigMask |= (1 << USBS1); + + switch (CDCInterfaceInfo->State.LineEncoding.DataBits) + { + case 6: + ConfigMask |= (1 << UCSZ10); + break; + case 7: + ConfigMask |= (1 << UCSZ11); + break; + case 8: + ConfigMask |= ((1 << UCSZ11) | (1 << UCSZ10)); + break; + } + + /* Must turn off USART before reconfiguring it, otherwise incorrect operation may occur */ + UCSR1B = 0; + UCSR1A = 0; + UCSR1C = 0; + + /* Special case 57600 baud for compatibility with the ATmega328 bootloader. */ + UBRR1 = (CDCInterfaceInfo->State.LineEncoding.BaudRateBPS == 57600) + ? SERIAL_UBBRVAL(CDCInterfaceInfo->State.LineEncoding.BaudRateBPS) + : SERIAL_2X_UBBRVAL(CDCInterfaceInfo->State.LineEncoding.BaudRateBPS); + + UCSR1C = ConfigMask; + UCSR1A = (CDCInterfaceInfo->State.LineEncoding.BaudRateBPS == 57600) ? 0 : (1 << U2X1); + UCSR1B = ((1 << RXCIE1) | (1 << TXEN1) | (1 << RXEN1)); +} + +/** ISR to manage the reception of data from the serial port, placing received bytes into a circular buffer + * for later transmission to the host. + */ +ISR(USART1_RX_vect, ISR_BLOCK) +{ + uint8_t ReceivedByte = UDR1; + + if (USB_DeviceState == DEVICE_STATE_Configured) + RingBuffer_Insert(&USARTtoUSB_Buffer, ReceivedByte); +} + +/** Event handler for the CDC Class driver Host-to-Device Line Encoding Changed event. + * + * \param[in] CDCInterfaceInfo Pointer to the CDC class interface configuration structure being referenced + */ +void EVENT_CDC_Device_ControLineStateChanged(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo) +{ + bool CurrentDTRState = (CDCInterfaceInfo->State.ControlLineStates.HostToDevice & CDC_CONTROL_LINE_OUT_DTR); + + if (CurrentDTRState) + AVR_RESET_LINE_PORT &= ~AVR_RESET_LINE_MASK; + else + AVR_RESET_LINE_PORT |= AVR_RESET_LINE_MASK; +} diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.h new file mode 100644 index 0000000..2183512 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Arduino-usbserial.h @@ -0,0 +1,79 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Arduino-usbserial.c. + */ + +#ifndef _ARDUINO_USBSERIAL_H_ +#define _ARDUINO_USBSERIAL_H_ + + /* Includes: */ + #include + #include + #include + #include + + #include "Descriptors.h" + + #include "Lib/LightweightRingBuff.h" + + #include + #include + #include + #include + #include + + /* Macros: */ + /** LED mask for the library LED driver, to indicate TX activity. */ + #define LEDMASK_TX LEDS_LED1 + + /** LED mask for the library LED driver, to indicate RX activity. */ + #define LEDMASK_RX LEDS_LED2 + + /** LED mask for the library LED driver, to indicate that an error has occurred in the USB interface. */ + #define LEDMASK_ERROR (LEDS_LED1 | LEDS_LED2) + + /** LED mask for the library LED driver, to indicate that the USB interface is busy. */ + #define LEDMASK_BUSY (LEDS_LED1 | LEDS_LED2) + + /* Function Prototypes: */ + void SetupHardware(void); + + void EVENT_USB_Device_Connect(void); + void EVENT_USB_Device_Disconnect(void); + void EVENT_USB_Device_ConfigurationChanged(void); + void EVENT_USB_Device_UnhandledControlRequest(void); + + void EVENT_CDC_Device_LineEncodingChanged(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo); + void EVENT_CDC_Device_ControLineStateChanged(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo); + +#endif /* _ARDUINO_USBSERIAL_H_ */ diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Board/LEDs.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Board/LEDs.h new file mode 100644 index 0000000..41465f2 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Board/LEDs.h @@ -0,0 +1,110 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/* + Board LEDs driver for the Benito board, from www.dorkbotpdx.org. +*/ + +#ifndef __LEDS_ARDUINOUNO_H__ +#define __LEDS_ARDUINOUNO_H__ + + /* Includes: */ + #include + +/* Enable C linkage for C++ Compilers: */ + #if defined(__cplusplus) + extern "C" { + #endif + + /* Preprocessor Checks: */ + #if !defined(INCLUDE_FROM_LEDS_H) + #error Do not include this file directly. Include LUFA/Drivers/Board/LEDS.h instead. + #endif + + /* Public Interface - May be used in end-application: */ + /* Macros: */ + /** LED mask for the first LED on the board. */ + #define LEDS_LED1 (1 << 5) + + /** LED mask for the second LED on the board. */ + #define LEDS_LED2 (1 << 4) + + /** LED mask for all the LEDs on the board. */ + #define LEDS_ALL_LEDS (LEDS_LED1 | LEDS_LED2) + + /** LED mask for the none of the board LEDs */ + #define LEDS_NO_LEDS 0 + + /* Inline Functions: */ + #if !defined(__DOXYGEN__) + static inline void LEDs_Init(void) + { + DDRD |= LEDS_ALL_LEDS; + PORTD |= LEDS_ALL_LEDS; + } + + static inline void LEDs_TurnOnLEDs(const uint8_t LEDMask) + { + PORTD &= ~LEDMask; + } + + static inline void LEDs_TurnOffLEDs(const uint8_t LEDMask) + { + PORTD |= LEDMask; + } + + static inline void LEDs_SetAllLEDs(const uint8_t LEDMask) + { + PORTD = ((PORTD | LEDS_ALL_LEDS) & ~LEDMask); + } + + static inline void LEDs_ChangeLEDs(const uint8_t LEDMask, const uint8_t ActiveMask) + { + PORTD = ((PORTD | ActiveMask) & ~LEDMask); + } + + static inline void LEDs_ToggleLEDs(const uint8_t LEDMask) + { + PORTD ^= LEDMask; + } + + static inline uint8_t LEDs_GetLEDs(void) ATTR_WARN_UNUSED_RESULT; + static inline uint8_t LEDs_GetLEDs(void) + { + return (PORTD & LEDS_ALL_LEDS); + } + #endif + + /* Disable C linkage for C++ Compilers: */ + #if defined(__cplusplus) + } + #endif + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.c b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.c new file mode 100644 index 0000000..ff033e0 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.c @@ -0,0 +1,277 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * USB Device Descriptors, for library use when in USB device mode. Descriptors are special + * computer-readable structures which the host requests upon device enumeration, to determine + * the device's capabilities and functions. + */ + +#include "Descriptors.h" + +/* On some devices, there is a factory set internal serial number which can be automatically sent to the host as + * the device's serial number when the Device Descriptor's .SerialNumStrIndex entry is set to USE_INTERNAL_SERIAL. + * This allows the host to track a device across insertions on different ports, allowing them to retain allocated + * resources like COM port numbers and drivers. On demos using this feature, give a warning on unsupported devices + * so that the user can supply their own serial number descriptor instead or remove the USE_INTERNAL_SERIAL value + * from the Device Descriptor (forcing the host to generate a serial number for each device from the VID, PID and + * port location). + */ +#if (USE_INTERNAL_SERIAL == NO_DESCRIPTOR) + #warning USE_INTERNAL_SERIAL is not available on this AVR - please manually construct a device serial descriptor. +#endif + +/** Device descriptor structure. This descriptor, located in FLASH memory, describes the overall + * device characteristics, including the supported USB version, control endpoint size and the + * number of device configurations. The descriptor is read out by the USB host when the enumeration + * process begins. + */ +USB_Descriptor_Device_t PROGMEM DeviceDescriptor = +{ + .Header = {.Size = sizeof(USB_Descriptor_Device_t), .Type = DTYPE_Device}, + + .USBSpecification = VERSION_BCD(01.10), + .Class = 0x02, + .SubClass = 0x00, + .Protocol = 0x00, + + .Endpoint0Size = FIXED_CONTROL_ENDPOINT_SIZE, + + .VendorID = 0x03EB, // Atmel + + .ProductID = 0x204B, // LUFA USB to Serial Demo Application + .ReleaseNumber = 0x0001, + + .ManufacturerStrIndex = 0x01, + .ProductStrIndex = 0x02, + .SerialNumStrIndex = USE_INTERNAL_SERIAL, + + .NumberOfConfigurations = FIXED_NUM_CONFIGURATIONS +}; + +/** Configuration descriptor structure. This descriptor, located in FLASH memory, describes the usage + * of the device in one of its supported configurations, including information about any device interfaces + * and endpoints. The descriptor is read out by the USB host during the enumeration process when selecting + * a configuration so that the host may correctly communicate with the USB device. + */ +USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor = +{ + .Config = + { + .Header = {.Size = sizeof(USB_Descriptor_Configuration_Header_t), .Type = DTYPE_Configuration}, + + .TotalConfigurationSize = sizeof(USB_Descriptor_Configuration_t), + .TotalInterfaces = 2, + + .ConfigurationNumber = 1, + .ConfigurationStrIndex = NO_DESCRIPTOR, + + .ConfigAttributes = (USB_CONFIG_ATTR_BUSPOWERED | USB_CONFIG_ATTR_SELFPOWERED), + + .MaxPowerConsumption = USB_CONFIG_POWER_MA(100) + }, + + .CDC_CCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 0, + .AlternateSetting = 0, + + .TotalEndpoints = 1, + + .Class = 0x02, + .SubClass = 0x02, + .Protocol = 0x01, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_Functional_IntHeader = + { + .Header = {.Size = sizeof(CDC_FUNCTIONAL_DESCRIPTOR(2)), .Type = 0x24}, + .SubType = 0x00, + + .Data = {0x01, 0x10} + }, + + .CDC_Functional_AbstractControlManagement = + { + .Header = {.Size = sizeof(CDC_FUNCTIONAL_DESCRIPTOR(1)), .Type = 0x24}, + .SubType = 0x02, + + .Data = {0x06} + }, + + .CDC_Functional_Union = + { + .Header = {.Size = sizeof(CDC_FUNCTIONAL_DESCRIPTOR(2)), .Type = 0x24}, + .SubType = 0x06, + + .Data = {0x00, 0x01} + }, + + .CDC_NotificationEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DESCRIPTOR_DIR_IN | CDC_NOTIFICATION_EPNUM), + .Attributes = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_NOTIFICATION_EPSIZE, + .PollingIntervalMS = 0xFF + }, + + .CDC_DCI_Interface = + { + .Header = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface}, + + .InterfaceNumber = 1, + .AlternateSetting = 0, + + .TotalEndpoints = 2, + + .Class = 0x0A, + .SubClass = 0x00, + .Protocol = 0x00, + + .InterfaceStrIndex = NO_DESCRIPTOR + }, + + .CDC_DataOutEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DESCRIPTOR_DIR_OUT | CDC_RX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + }, + + .CDC_DataInEndpoint = + { + .Header = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint}, + + .EndpointAddress = (ENDPOINT_DESCRIPTOR_DIR_IN | CDC_TX_EPNUM), + .Attributes = (EP_TYPE_BULK | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA), + .EndpointSize = CDC_TXRX_EPSIZE, + .PollingIntervalMS = 0x01 + } +}; + +/** Language descriptor structure. This descriptor, located in FLASH memory, is returned when the host requests + * the string descriptor with index 0 (the first index). It is actually an array of 16-bit integers, which indicate + * via the language ID table available at USB.org what languages the device supports for its string descriptors. + */ +USB_Descriptor_String_t PROGMEM LanguageString = +{ + .Header = {.Size = USB_STRING_LEN(1), .Type = DTYPE_String}, + + .UnicodeString = {LANGUAGE_ID_ENG} +}; + +/** Manufacturer descriptor string. This is a Unicode string containing the manufacturer's details in human readable + * form, and is read out upon request by the host when the appropriate string ID is requested, listed in the Device + * Descriptor. + */ +USB_Descriptor_String_t PROGMEM ManufacturerString = +{ + .Header = {.Size = USB_STRING_LEN(24), .Type = DTYPE_String}, + + .UnicodeString = L"Arduino (www.arduino.cc)" +}; + +/** Product descriptor string. This is a Unicode string containing the product's details in human readable form, + * and is read out upon request by the host when the appropriate string ID is requested, listed in the Device + * Descriptor. + */ +USB_Descriptor_String_t PROGMEM ProductString = +{ + #if (ARDUINO_MODEL_PID == ARDUINO_UNO_PID) + .Header = {.Size = USB_STRING_LEN(11), .Type = DTYPE_String}, + + .UnicodeString = L"Arduino Uno" + #elif (ARDUINO_MODEL_PID == ARDUINO_MEGA2560_PID) + .Header = {.Size = USB_STRING_LEN(17), .Type = DTYPE_String}, + + .UnicodeString = L"Arduino Mega 2560" + #endif + +}; + +/** This function is called by the library when in device mode, and must be overridden (see library "USB Descriptors" + * documentation) by the application code so that the address and size of a requested descriptor can be given + * to the USB library. When the device receives a Get Descriptor request on the control endpoint, this function + * is called so that the descriptor details can be passed back and the appropriate descriptor sent back to the + * USB host. + */ +uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + void** const DescriptorAddress) +{ + const uint8_t DescriptorType = (wValue >> 8); + const uint8_t DescriptorNumber = (wValue & 0xFF); + + void* Address = NULL; + uint16_t Size = NO_DESCRIPTOR; + + switch (DescriptorType) + { + case DTYPE_Device: + Address = (void*)&DeviceDescriptor; + Size = sizeof(USB_Descriptor_Device_t); + break; + case DTYPE_Configuration: + Address = (void*)&ConfigurationDescriptor; + Size = sizeof(USB_Descriptor_Configuration_t); + break; + case DTYPE_String: + switch (DescriptorNumber) + { + case 0x00: + Address = (void*)&LanguageString; + Size = pgm_read_byte(&LanguageString.Header.Size); + break; + case 0x01: + Address = (void*)&ManufacturerString; + Size = pgm_read_byte(&ManufacturerString.Header.Size); + break; + case 0x02: + Address = (void*)&ProductString; + Size = pgm_read_byte(&ProductString.Header.Size); + break; + } + + break; + } + + *DescriptorAddress = Address; + return Size; +} diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.h new file mode 100644 index 0000000..2bce3d7 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Descriptors.h @@ -0,0 +1,88 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Header file for Descriptors.c. + */ + +#ifndef _DESCRIPTORS_H_ +#define _DESCRIPTORS_H_ + + /* Includes: */ + #include + + #include + #include + + /* Product-specific definitions: */ + #define ARDUINO_UNO_PID 0x0001 + #define ARDUINO_MEGA2560_PID 0x0010 + + /* Macros: */ + /** Endpoint number of the CDC device-to-host notification IN endpoint. */ + #define CDC_NOTIFICATION_EPNUM 2 + + /** Endpoint number of the CDC device-to-host data IN endpoint. */ + #define CDC_TX_EPNUM 3 + + /** Endpoint number of the CDC host-to-device data OUT endpoint. */ + #define CDC_RX_EPNUM 4 + + /** Size in bytes of the CDC device-to-host notification IN endpoint. */ + #define CDC_NOTIFICATION_EPSIZE 8 + + /** Size in bytes of the CDC data IN and OUT endpoints. */ + #define CDC_TXRX_EPSIZE 64 + + /* Type Defines: */ + /** Type define for the device configuration descriptor structure. This must be defined in the + * application code, as the configuration descriptor contains several sub-descriptors which + * vary between devices, and which describe the device's usage to the host. + */ + typedef struct + { + USB_Descriptor_Configuration_Header_t Config; + USB_Descriptor_Interface_t CDC_CCI_Interface; + CDC_FUNCTIONAL_DESCRIPTOR(2) CDC_Functional_IntHeader; + CDC_FUNCTIONAL_DESCRIPTOR(1) CDC_Functional_AbstractControlManagement; + CDC_FUNCTIONAL_DESCRIPTOR(2) CDC_Functional_Union; + USB_Descriptor_Endpoint_t CDC_NotificationEndpoint; + USB_Descriptor_Interface_t CDC_DCI_Interface; + USB_Descriptor_Endpoint_t CDC_DataOutEndpoint; + USB_Descriptor_Endpoint_t CDC_DataInEndpoint; + } USB_Descriptor_Configuration_t; + + /* Function Prototypes: */ + uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue, + const uint8_t wIndex, + void** const DescriptorAddress) ATTR_WARN_UNUSED_RESULT ATTR_NON_NULL_PTR_ARG(3); + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Lib/LightweightRingBuff.h b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Lib/LightweightRingBuff.h new file mode 100644 index 0000000..5a9a125 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/Lib/LightweightRingBuff.h @@ -0,0 +1,197 @@ +/* + LUFA Library + Copyright (C) Dean Camera, 2010. + + dean [at] fourwalledcubicle [dot] com + www.fourwalledcubicle.com +*/ + +/* + Copyright 2010 Dean Camera (dean [at] fourwalledcubicle [dot] com) + + Permission to use, copy, modify, distribute, and sell this + software and its documentation for any purpose is hereby granted + without fee, provided that the above copyright notice appear in + all copies and that both that the copyright notice and this + permission notice and warranty disclaimer appear in supporting + documentation, and that the name of the author not be used in + advertising or publicity pertaining to distribution of the + software without specific, written prior permission. + + The author disclaim all warranties with regard to this + software, including all implied warranties of merchantability + and fitness. In no event shall the author be liable for any + special, indirect or consequential damages or any damages + whatsoever resulting from loss of use, data or profits, whether + in an action of contract, negligence or other tortious action, + arising out of or in connection with the use or performance of + this software. +*/ + +/** \file + * + * Ultra lightweight ring buffer, for fast insertion/deletion. + */ + +#ifndef _ULW_RING_BUFF_H_ +#define _ULW_RING_BUFF_H_ + + /* Includes: */ + #include + + #include + #include + + /* Defines: */ + /** Size of each ring buffer, in data elements - must be between 1 and 255. */ + #define BUFFER_SIZE 128 + + /** Maximum number of data elements to buffer before forcing a flush. + * Must be less than BUFFER_SIZE + */ + #define BUFFER_NEARLY_FULL 96 + + /** Type of data to store into the buffer. */ + #define RingBuff_Data_t uint8_t + + /** Datatype which may be used to store the count of data stored in a buffer, retrieved + * via a call to \ref RingBuffer_GetCount(). + */ + #if (BUFFER_SIZE <= 0xFF) + #define RingBuff_Count_t uint8_t + #else + #define RingBuff_Count_t uint16_t + #endif + + /* Type Defines: */ + /** Type define for a new ring buffer object. Buffers should be initialized via a call to + * \ref RingBuffer_InitBuffer() before use. + */ + typedef struct + { + RingBuff_Data_t Buffer[BUFFER_SIZE]; /**< Internal ring buffer data, referenced by the buffer pointers. */ + RingBuff_Data_t* In; /**< Current storage location in the circular buffer */ + RingBuff_Data_t* Out; /**< Current retrieval location in the circular buffer */ + RingBuff_Count_t Count; + } RingBuff_t; + + /* Inline Functions: */ + /** Initializes a ring buffer ready for use. Buffers must be initialized via this function + * before any operations are called upon them. Already initialized buffers may be reset + * by re-initializing them using this function. + * + * \param[out] Buffer Pointer to a ring buffer structure to initialize + */ + static inline void RingBuffer_InitBuffer(RingBuff_t* const Buffer) + { + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + Buffer->In = Buffer->Buffer; + Buffer->Out = Buffer->Buffer; + } + } + + /** Retrieves the minimum number of bytes stored in a particular buffer. This value is computed + * by entering an atomic lock on the buffer while the IN and OUT locations are fetched, so that + * the buffer cannot be modified while the computation takes place. This value should be cached + * when reading out the contents of the buffer, so that as small a time as possible is spent + * in an atomic lock. + * + * \note The value returned by this function is guaranteed to only be the minimum number of bytes + * stored in the given buffer; this value may change as other threads write new data and so + * the returned number should be used only to determine how many successive reads may safely + * be performed on the buffer. + * + * \param[in] Buffer Pointer to a ring buffer structure whose count is to be computed + */ + static inline RingBuff_Count_t RingBuffer_GetCount(RingBuff_t* const Buffer) + { + RingBuff_Count_t Count; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + Count = Buffer->Count; + } + + return Count; + } + + /** Atomically determines if the specified ring buffer contains any free space. This should + * be tested before storing data to the buffer, to ensure that no data is lost due to a + * buffer overrun. + * + * \param[in,out] Buffer Pointer to a ring buffer structure to insert into + * + * \return Boolean true if the buffer contains no free space, false otherwise + */ + static inline bool RingBuffer_IsFull(RingBuff_t* const Buffer) + { + return (RingBuffer_GetCount(Buffer) == BUFFER_SIZE); + } + + /** Atomically determines if the specified ring buffer contains any data. This should + * be tested before removing data from the buffer, to ensure that the buffer does not + * underflow. + * + * If the data is to be removed in a loop, store the total number of bytes stored in the + * buffer (via a call to the \ref RingBuffer_GetCount() function) in a temporary variable + * to reduce the time spent in atomicity locks. + * + * \param[in,out] Buffer Pointer to a ring buffer structure to insert into + * + * \return Boolean true if the buffer contains no free space, false otherwise + */ + static inline bool RingBuffer_IsEmpty(RingBuff_t* const Buffer) + { + return (RingBuffer_GetCount(Buffer) == 0); + } + + /** Inserts an element into the ring buffer. + * + * \note Only one execution thread (main program thread or an ISR) may insert into a single buffer + * otherwise data corruption may occur. Insertion and removal may occur from different execution + * threads. + * + * \param[in,out] Buffer Pointer to a ring buffer structure to insert into + * \param[in] Data Data element to insert into the buffer + */ + static inline void RingBuffer_Insert(RingBuff_t* const Buffer, + const RingBuff_Data_t Data) + { + *Buffer->In = Data; + + if (++Buffer->In == &Buffer->Buffer[BUFFER_SIZE]) + Buffer->In = Buffer->Buffer; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + Buffer->Count++; + } + } + + /** Removes an element from the ring buffer. + * + * \note Only one execution thread (main program thread or an ISR) may remove from a single buffer + * otherwise data corruption may occur. Insertion and removal may occur from different execution + * threads. + * + * \param[in,out] Buffer Pointer to a ring buffer structure to retrieve from + * + * \return Next data element stored in the buffer + */ + static inline RingBuff_Data_t RingBuffer_Remove(RingBuff_t* const Buffer) + { + RingBuff_Data_t Data = *Buffer->Out; + + if (++Buffer->Out == &Buffer->Buffer[BUFFER_SIZE]) + Buffer->Out = Buffer->Buffer; + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) + { + Buffer->Count--; + } + + return Data; + } + +#endif diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/makefile b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/makefile new file mode 100644 index 0000000..de518ef --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/makefile @@ -0,0 +1,776 @@ +# Hey Emacs, this is a -*- makefile -*- +#---------------------------------------------------------------------------- +# WinAVR Makefile Template written by Eric B. Weddington, Jrg Wunsch, et al. +# >> Modified for use with the LUFA project. << +# +# Released to the Public Domain +# +# Additional material for this makefile was written by: +# Peter Fleury +# Tim Henigan +# Colin O'Flynn +# Reiner Patommel +# Markus Pfaff +# Sander Pool +# Frederik Rouleau +# Carlos Lamas +# Dean Camera +# Opendous Inc. +# Denver Gingerich +# +#---------------------------------------------------------------------------- +# On command line: +# +# make all = Make software. +# +# make clean = Clean out built project files. +# +# make coff = Convert ELF to AVR COFF. +# +# make extcoff = Convert ELF to AVR Extended COFF. +# +# make program = Download the hex file to the device, using avrdude. +# Please customize the avrdude settings below first! +# +# make dfu = Download the hex file to the device, using dfu-programmer (must +# have dfu-programmer installed). +# +# make flip = Download the hex file to the device, using Atmel FLIP (must +# have Atmel FLIP installed). +# +# make dfu-ee = Download the eeprom file to the device, using dfu-programmer +# (must have dfu-programmer installed). +# +# make flip-ee = Download the eeprom file to the device, using Atmel FLIP +# (must have Atmel FLIP installed). +# +# make doxygen = Generate DoxyGen documentation for the project (must have +# DoxyGen installed) +# +# make debug = Start either simulavr or avarice as specified for debugging, +# with avr-gdb or avr-insight as the front end for debugging. +# +# make filename.s = Just compile filename.c into the assembler code only. +# +# make filename.i = Create a preprocessed source file for use in submitting +# bug reports to the GCC project. +# +# To rebuild project do "make clean" then "make all". +#---------------------------------------------------------------------------- + +# MCU name(s) +# Since the ATMEGA8U2 part is not directly supported by the current +# versions of either avrdude or dfu-programmer, we specify a dummy +# part; AT90USB82 which is close enough in memory size and organization +MCU = atmega8u2 +MCU_AVRDUDE = at90usb82 +MCU_DFU = at90usb82 + +# Specify the Arduino model using the assigned PID. This is used by Descriptors.c +# to set PID and product descriptor string +# Uno PID: +ARDUINO_MODEL_PID = 0x0001 +# Mega 2560 PID: +#ARDUINO_MODEL_PID = 0x0010 + + +# Target board (see library "Board Types" documentation, NONE for projects not requiring +# LUFA board drivers). If USER is selected, put custom board drivers in a directory called +# "Board" inside the application directory. +BOARD = USER + + +# Processor frequency. +# This will define a symbol, F_CPU, in all source code files equal to the +# processor frequency in Hz. You can then use this symbol in your source code to +# calculate timings. Do NOT tack on a 'UL' at the end, this will be done +# automatically to create a 32-bit value in your source code. +# +# This will be an integer division of F_CLOCK below, as it is sourced by +# F_CLOCK after it has run through any CPU prescalers. Note that this value +# does not *change* the processor frequency - it should merely be updated to +# reflect the processor speed set externally so that the code can use accurate +# software delays. +F_CPU = 16000000 + + +# Input clock frequency. +# This will define a symbol, F_CLOCK, in all source code files equal to the +# input clock frequency (before any prescaling is performed) in Hz. This value may +# differ from F_CPU if prescaling is used on the latter, and is required as the +# raw input clock is fed directly to the PLL sections of the AVR for high speed +# clock generation for the USB and other AVR subsections. Do NOT tack on a 'UL' +# at the end, this will be done automatically to create a 32-bit value in your +# source code. +# +# If no clock division is performed on the input clock inside the AVR (via the +# CPU clock adjust registers or the clock division fuses), this will be equal to F_CPU. +F_CLOCK = $(F_CPU) + + +# Output format. (can be srec, ihex, binary) +FORMAT = ihex + + +# Target file name (without extension). +TARGET = Arduino-usbserial + + +# Object files directory +# To put object files in current directory, use a dot (.), do NOT make +# this an empty or blank macro! +OBJDIR = . + + +# Path to the LUFA library +LUFA_PATH = ../.. + + +# LUFA library compile-time options +LUFA_OPTS = -D USB_DEVICE_ONLY +LUFA_OPTS += -D FIXED_CONTROL_ENDPOINT_SIZE=8 +LUFA_OPTS += -D FIXED_NUM_CONFIGURATIONS=1 +LUFA_OPTS += -D USE_FLASH_DESCRIPTORS +LUFA_OPTS += -D INTERRUPT_CONTROL_ENDPOINT +LUFA_OPTS += -D DEVICE_STATE_AS_GPIOR=0 +LUFA_OPTS += -D USE_STATIC_OPTIONS="(USB_DEVICE_OPT_FULLSPEED | USB_OPT_REG_ENABLED | USB_OPT_AUTO_PLL)" + + +# Create the LUFA source path variables by including the LUFA root makefile +include $(LUFA_PATH)/LUFA/makefile + + +# List C source files here. (C dependencies are automatically generated.) +SRC = $(TARGET).c \ + Descriptors.c \ + $(LUFA_SRC_USB) \ + $(LUFA_SRC_USBCLASS) \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/Device.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/Endpoint.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/HighLevel/HostStandardReq.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/Host.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/Pipe.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/USBController.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/HighLevel/Events.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/LowLevel/USBInterrupt.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/HighLevel/USBTask.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/HighLevel/DeviceStandardReq.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/HighLevel/ConfigDescriptor.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/Class/Device/CDC.c \ + $(LUFA_PATH)/LUFA/Drivers/USB/Class/Host/CDC.c + + +# List C++ source files here. (C dependencies are automatically generated.) +CPPSRC = + + +# List Assembler source files here. +# Make them always end in a capital .S. Files ending in a lowercase .s +# will not be considered source files but generated files (assembler +# output from the compiler), and will be deleted upon "make clean"! +# Even though the DOS/Win* filesystem matches both .s and .S the same, +# it will preserve the spelling of the filenames, and gcc itself does +# care about how the name is spelled on its command-line. +ASRC = + + +# Optimization level, can be [0, 1, 2, 3, s]. +# 0 = turn off optimization. s = optimize for size. +# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) +OPT = s + + +# Debugging format. +# Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs. +# AVR Studio 4.10 requires dwarf-2. +# AVR [Extended] COFF format requires stabs, plus an avr-objcopy run. +DEBUG = dwarf-2 + + +# List any extra directories to look for include files here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRAINCDIRS = $(LUFA_PATH)/ + + +# Compiler flag to set the C Standard level. +# c89 = "ANSI" C +# gnu89 = c89 plus GCC extensions +# c99 = ISO C99 standard (not yet fully implemented) +# gnu99 = c99 plus GCC extensions +CSTANDARD = -std=gnu99 + + +# Place -D or -U options here for C sources +CDEFS = -DF_CPU=$(F_CPU)UL +CDEFS += -DF_CLOCK=$(F_CLOCK)UL +CDEFS += -DARDUINO_MODEL_PID=$(ARDUINO_MODEL_PID) +CDEFS += -DBOARD=BOARD_$(BOARD) +CDEFS += $(LUFA_OPTS) +CDEFS += -DAVR_RESET_LINE_PORT="PORTD" +CDEFS += -DAVR_RESET_LINE_DDR="DDRD" +CDEFS += -DAVR_RESET_LINE_MASK="(1 << 7)" +CDEFS += -DTX_RX_LED_PULSE_MS=3 +CDEFS += -DPING_PONG_LED_PULSE_MS=100 + +# Place -D or -U options here for ASM sources +ADEFS = -DF_CPU=$(F_CPU) +ADEFS += -DF_CLOCK=$(F_CLOCK)UL +ADEFS += -DBOARD=BOARD_$(BOARD) +ADEFS += $(LUFA_OPTS) + +# Place -D or -U options here for C++ sources +CPPDEFS = -DF_CPU=$(F_CPU)UL +CPPDEFS += -DF_CLOCK=$(F_CLOCK)UL +CPPDEFS += -DBOARD=BOARD_$(BOARD) +CPPDEFS += $(LUFA_OPTS) +#CPPDEFS += -D__STDC_LIMIT_MACROS +#CPPDEFS += -D__STDC_CONSTANT_MACROS + + + +#---------------- Compiler Options C ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CFLAGS = -g$(DEBUG) +CFLAGS += $(CDEFS) +CFLAGS += -O$(OPT) +CFLAGS += -funsigned-char +CFLAGS += -funsigned-bitfields +CFLAGS += -ffunction-sections +CFLAGS += -fno-inline-small-functions +CFLAGS += -fpack-struct +CFLAGS += -fshort-enums +CFLAGS += -fno-strict-aliasing +CFLAGS += -Wall +CFLAGS += -Wstrict-prototypes +#CFLAGS += -mshort-calls +#CFLAGS += -fno-unit-at-a-time +#CFLAGS += -Wundef +#CFLAGS += -Wunreachable-code +#CFLAGS += -Wsign-compare +CFLAGS += -Wa,-adhlns=$(<:%.c=$(OBJDIR)/%.lst) +CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +CFLAGS += $(CSTANDARD) + + +#---------------- Compiler Options C++ ---------------- +# -g*: generate debugging information +# -O*: optimization level +# -f...: tuning, see GCC manual and avr-libc documentation +# -Wall...: warning level +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns...: create assembler listing +CPPFLAGS = -g$(DEBUG) +CPPFLAGS += $(CPPDEFS) +CPPFLAGS += -O$(OPT) +CPPFLAGS += -funsigned-char +CPPFLAGS += -funsigned-bitfields +CPPFLAGS += -fpack-struct +CPPFLAGS += -fshort-enums +CPPFLAGS += -fno-exceptions +CPPFLAGS += -Wall +CPPFLAGS += -Wundef +CFLAGS += -Wundef +#CPPFLAGS += -mshort-calls +#CPPFLAGS += -fno-unit-at-a-time +#CPPFLAGS += -Wstrict-prototypes +#CPPFLAGS += -Wunreachable-code +#CPPFLAGS += -Wsign-compare +CPPFLAGS += -Wa,-adhlns=$(<:%.cpp=$(OBJDIR)/%.lst) +CPPFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) +#CPPFLAGS += $(CSTANDARD) + + +#---------------- Assembler Options ---------------- +# -Wa,...: tell GCC to pass this to the assembler. +# -adhlns: create listing +# -gstabs: have the assembler create line number information; note that +# for use in COFF files, additional information about filenames +# and function names needs to be present in the assembler source +# files -- see avr-libc docs [FIXME: not yet described there] +# -listing-cont-lines: Sets the maximum number of continuation lines of hex +# dump that will be displayed for a given single line of source input. +ASFLAGS = $(ADEFS) -Wa,-adhlns=$(<:%.S=$(OBJDIR)/%.lst),-gstabs,--listing-cont-lines=100 + + +#---------------- Library Options ---------------- +# Minimalistic printf version +PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min + +# Floating point printf version (requires MATH_LIB = -lm below) +PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt + +# If this is left blank, then it will use the Standard printf version. +PRINTF_LIB = +#PRINTF_LIB = $(PRINTF_LIB_MIN) +#PRINTF_LIB = $(PRINTF_LIB_FLOAT) + + +# Minimalistic scanf version +SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min + +# Floating point + %[ scanf version (requires MATH_LIB = -lm below) +SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt + +# If this is left blank, then it will use the Standard scanf version. +SCANF_LIB = +#SCANF_LIB = $(SCANF_LIB_MIN) +#SCANF_LIB = $(SCANF_LIB_FLOAT) + + +MATH_LIB = -lm + + +# List any extra directories to look for libraries here. +# Each directory must be seperated by a space. +# Use forward slashes for directory separators. +# For a directory that has spaces, enclose it in quotes. +EXTRALIBDIRS = + + + +#---------------- External Memory Options ---------------- + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# used for variables (.data/.bss) and heap (malloc()). +#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff + +# 64 KB of external RAM, starting after internal RAM (ATmega128!), +# only used for heap (malloc()). +#EXTMEMOPTS = -Wl,--section-start,.data=0x801100,--defsym=__heap_end=0x80ffff + +EXTMEMOPTS = + + + +#---------------- Linker Options ---------------- +# -Wl,...: tell GCC to pass this to linker. +# -Map: create map file +# --cref: add cross reference to map file +LDFLAGS = -Wl,-Map=$(TARGET).map,--cref +LDFLAGS += -Wl,--relax +LDFLAGS += -Wl,--gc-sections +LDFLAGS += $(EXTMEMOPTS) +LDFLAGS += $(patsubst %,-L%,$(EXTRALIBDIRS)) +LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) +#LDFLAGS += -T linker_script.x + + + +#---------------- Programming Options (avrdude) ---------------- + +# Programming hardware +# Type: avrdude -c ? +# to get a full listing. +# +AVRDUDE_PROGRAMMER = avrispmkii + +# com1 = serial port. Use lpt1 to connect to parallel port. +AVRDUDE_PORT = usb + +AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex +#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep + + +# Uncomment the following if you want avrdude's erase cycle counter. +# Note that this counter needs to be initialized first using -Yn, +# see avrdude manual. +#AVRDUDE_ERASE_COUNTER = -y + +# Uncomment the following if you do /not/ wish a verification to be +# performed after programming the device. +#AVRDUDE_NO_VERIFY = -V + +# Increase verbosity level. Please use this when submitting bug +# reports about avrdude. See +# to submit bug reports. +#AVRDUDE_VERBOSE = -v -v + +AVRDUDE_FORCE = -F + +AVRDUDE_FLAGS = -p $(MCU_AVRDUDE) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) +AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY) +AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE) +AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER) +AVRDUDE_FLAGS += $(AVRDUDE_FORCE) + + + +#---------------- Debugging Options ---------------- + +# For simulavr only - target MCU frequency. +DEBUG_MFREQ = $(F_CPU) + +# Set the DEBUG_UI to either gdb or insight. +# DEBUG_UI = gdb +DEBUG_UI = insight + +# Set the debugging back-end to either avarice, simulavr. +DEBUG_BACKEND = avarice +#DEBUG_BACKEND = simulavr + +# GDB Init Filename. +GDBINIT_FILE = __avr_gdbinit + +# When using avarice settings for the JTAG +JTAG_DEV = /dev/com1 + +# Debugging port used to communicate between GDB / avarice / simulavr. +DEBUG_PORT = 4242 + +# Debugging host used to communicate between GDB / avarice / simulavr, normally +# just set to localhost unless doing some sort of crazy debugging when +# avarice is running on a different computer. +DEBUG_HOST = localhost + + + +#============================================================================ + + +# Define programs and commands. +SHELL = sh +CC = avr-gcc +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump +SIZE = avr-size +AR = avr-ar rcs +NM = avr-nm +AVRDUDE = avrdude +REMOVE = rm -f +REMOVEDIR = rm -rf +COPY = cp +WINSHELL = cmd + +# Define Messages +# English +MSG_ERRORS_NONE = Errors: none +MSG_BEGIN = -------- begin -------- +MSG_END = -------- end -------- +MSG_SIZE_BEFORE = Size before: +MSG_SIZE_AFTER = Size after: +MSG_COFF = Converting to AVR COFF: +MSG_EXTENDED_COFF = Converting to AVR Extended COFF: +MSG_FLASH = Creating load file for Flash: +MSG_EEPROM = Creating load file for EEPROM: +MSG_EXTENDED_LISTING = Creating Extended Listing: +MSG_SYMBOL_TABLE = Creating Symbol Table: +MSG_LINKING = Linking: +MSG_COMPILING = Compiling C: +MSG_COMPILING_CPP = Compiling C++: +MSG_ASSEMBLING = Assembling: +MSG_CLEANING = Cleaning project: +MSG_CREATING_LIBRARY = Creating library: + + + + +# Define all object files. +OBJ = $(SRC:%.c=$(OBJDIR)/%.o) $(CPPSRC:%.cpp=$(OBJDIR)/%.o) $(ASRC:%.S=$(OBJDIR)/%.o) + +# Define all listing files. +LST = $(SRC:%.c=$(OBJDIR)/%.lst) $(CPPSRC:%.cpp=$(OBJDIR)/%.lst) $(ASRC:%.S=$(OBJDIR)/%.lst) + + +# Compiler flags to generate dependency files. +GENDEPFLAGS = -MMD -MP -MF .dep/$(@F).d + + +# Combine all necessary flags and optional flags. +# Add target processor to flags. +ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS) +ALL_CPPFLAGS = -mmcu=$(MCU) -I. -x c++ $(CPPFLAGS) $(GENDEPFLAGS) +ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) + + + + + +# Default target. +#all: begin gccversion sizebefore build checkinvalidevents showliboptions showtarget sizeafter end +all: begin gccversion sizebefore build showliboptions showtarget sizeafter end + +# Change the build target to build a HEX file or a library. +build: elf hex eep lss sym asm +#build: lib + + +elf: $(TARGET).elf +hex: $(TARGET).hex +eep: $(TARGET).eep +lss: $(TARGET).lss +sym: $(TARGET).sym +asm: $(TARGET).s +LIBNAME=lib$(TARGET).a +lib: $(LIBNAME) + + + +# Eye candy. +# AVR Studio 3.x does not check make's exit code but relies on +# the following magic strings to be generated by the compile job. +begin: + @echo + @echo $(MSG_BEGIN) + +end: + @echo $(MSG_END) + @echo + + +# Display size of file. +HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex +ELFSIZE = $(SIZE) $(MCU_FLAG) $(FORMAT_FLAG) $(TARGET).elf +MCU_FLAG = $(shell $(SIZE) --help | grep -- --mcu > /dev/null && echo --mcu=$(MCU) ) +FORMAT_FLAG = $(shell $(SIZE) --help | grep -- --format=.*avr > /dev/null && echo --format=avr ) + +sizebefore: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +sizeafter: + @if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \ + 2>/dev/null; echo; fi + +#$(LUFA_PATH)/LUFA/LUFA_Events.lst: +# @make -C $(LUFA_PATH)/LUFA/ LUFA_Events.lst + +#checkinvalidevents: $(LUFA_PATH)/LUFA/LUFA_Events.lst +# @echo +# @echo Checking for invalid events... +# @$(shell) avr-nm $(OBJ) | sed -n -e 's/^.*EVENT_/EVENT_/p' | \ +# grep -F -v --file=$(LUFA_PATH)/LUFA/LUFA_Events.lst > InvalidEvents.tmp || true +# @sed -n -e 's/^/ WARNING - INVALID EVENT NAME: /p' InvalidEvents.tmp +# @if test -s InvalidEvents.tmp; then exit 1; fi + +showliboptions: + @echo + @echo ---- Compile Time Library Options ---- + @for i in $(LUFA_OPTS:-D%=%); do \ + echo $$i; \ + done + @echo -------------------------------------- + +showtarget: + @echo + @echo --------- Target Information --------- + @echo AVR Model: $(MCU) + @echo Board: $(BOARD) + @echo Clock: $(F_CPU)Hz CPU, $(F_CLOCK)Hz Master + @echo -------------------------------------- + + +# Display compiler version information. +gccversion : + @$(CC) --version + + +# Program the device. +program: $(TARGET).hex $(TARGET).eep + $(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) + +flip: $(TARGET).hex + batchisp -hardware usb -device $(MCU_DFU) -operation erase f + batchisp -hardware usb -device $(MCU_DFU) -operation loadbuffer $(TARGET).hex program + batchisp -hardware usb -device $(MCU_DFU) -operation start reset 0 + +dfu: $(TARGET).hex + dfu-programmer $(MCU_DFU) erase + dfu-programmer $(MCU_DFU) flash --debug 1 $(TARGET).hex + dfu-programmer $(MCU_DFU) reset + + +flip-ee: $(TARGET).hex $(TARGET).eep + $(COPY) $(TARGET).eep $(TARGET)eep.hex + batchisp -hardware usb -device $(MCU_DFU) -operation memory EEPROM erase + batchisp -hardware usb -device $(MCU_DFU) -operation memory EEPROM loadbuffer $(TARGET)eep.hex program + batchisp -hardware usb -device $(MCU_DFU) -operation start reset 0 + $(REMOVE) $(TARGET)eep.hex + +dfu-ee: $(TARGET).hex $(TARGET).eep + dfu-programmer $(MCU_DFU) flash-eeprom --debug 1 --suppress-bootloader-mem $(TARGET).eep + dfu-programmer $(MCU_DFU) reset + + +# Generate avr-gdb config/init file which does the following: +# define the reset signal, load the target file, connect to target, and set +# a breakpoint at main(). +gdb-config: + @$(REMOVE) $(GDBINIT_FILE) + @echo define reset >> $(GDBINIT_FILE) + @echo SIGNAL SIGHUP >> $(GDBINIT_FILE) + @echo end >> $(GDBINIT_FILE) + @echo file $(TARGET).elf >> $(GDBINIT_FILE) + @echo target remote $(DEBUG_HOST):$(DEBUG_PORT) >> $(GDBINIT_FILE) +ifeq ($(DEBUG_BACKEND),simulavr) + @echo load >> $(GDBINIT_FILE) +endif + @echo break main >> $(GDBINIT_FILE) + +debug: gdb-config $(TARGET).elf +ifeq ($(DEBUG_BACKEND), avarice) + @echo Starting AVaRICE - Press enter when "waiting to connect" message displays. + @$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \ + $(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT) + @$(WINSHELL) /c pause + +else + @$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \ + $(DEBUG_MFREQ) --port $(DEBUG_PORT) +endif + @$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE) + + + + +# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB. +COFFCONVERT = $(OBJCOPY) --debugging +COFFCONVERT += --change-section-address .data-0x800000 +COFFCONVERT += --change-section-address .bss-0x800000 +COFFCONVERT += --change-section-address .noinit-0x800000 +COFFCONVERT += --change-section-address .eeprom-0x810000 + + + +coff: $(TARGET).elf + @echo + @echo $(MSG_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-avr $< $(TARGET).cof + + +extcoff: $(TARGET).elf + @echo + @echo $(MSG_EXTENDED_COFF) $(TARGET).cof + $(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof + + + +# Create final output files (.hex, .eep) from ELF output file. +%.hex: %.elf + @echo + @echo $(MSG_FLASH) $@ + $(OBJCOPY) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@ + +%.eep: %.elf + @echo + @echo $(MSG_EEPROM) $@ + -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ + --change-section-lma .eeprom=0 --no-change-warnings -O $(FORMAT) $< $@ || exit 0 + +# Create extended listing file from ELF output file. +%.lss: %.elf + @echo + @echo $(MSG_EXTENDED_LISTING) $@ + $(OBJDUMP) -h -S -z $< > $@ + +# Create a symbol table from ELF output file. +%.sym: %.elf + @echo + @echo $(MSG_SYMBOL_TABLE) $@ + $(NM) -n $< > $@ + + + +# Create library from object files. +.SECONDARY : $(TARGET).a +.PRECIOUS : $(OBJ) +%.a: $(OBJ) + @echo + @echo $(MSG_CREATING_LIBRARY) $@ + $(AR) $@ $(OBJ) + + +# Link: create ELF output file from object files. +.SECONDARY : $(TARGET).elf +.PRECIOUS : $(OBJ) +%.elf: $(OBJ) + @echo + @echo $(MSG_LINKING) $@ + $(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS) + + +# Compile: create object files from C source files. +$(OBJDIR)/%.o : %.c + @echo + @echo $(MSG_COMPILING) $< + $(CC) -c $(ALL_CFLAGS) $< -o $@ + + +# Compile: create object files from C++ source files. +$(OBJDIR)/%.o : %.cpp + @echo + @echo $(MSG_COMPILING_CPP) $< + $(CC) -c $(ALL_CPPFLAGS) $< -o $@ + + +# Compile: create assembler files from C source files. +%.s : %.c + $(CC) -S $(ALL_CFLAGS) $< -o $@ + + +# Compile: create assembler files from C++ source files. +%.s : %.cpp + $(CC) -S $(ALL_CPPFLAGS) $< -o $@ + + +# Assemble: create object files from assembler source files. +$(OBJDIR)/%.o : %.S + @echo + @echo $(MSG_ASSEMBLING) $< + $(CC) -c $(ALL_ASFLAGS) $< -o $@ + + +# Create preprocessed source for use in sending a bug report. +%.i : %.c + $(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ + + +# Target: clean project. +clean: begin clean_list clean_binary end + +clean_binary: + $(REMOVE) $(TARGET).hex + +clean_list: + @echo $(MSG_CLEANING) + $(REMOVE) $(TARGET).hex + $(REMOVE) $(TARGET).eep + $(REMOVE) $(TARGET).cof + $(REMOVE) $(TARGET).elf + $(REMOVE) $(TARGET).map + $(REMOVE) $(TARGET).sym + $(REMOVE) $(TARGET).lss + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.o) + $(REMOVE) $(SRC:%.c=$(OBJDIR)/%.lst) + $(REMOVE) $(SRC:.c=.s) + $(REMOVE) $(SRC:.c=.d) + $(REMOVE) $(SRC:.c=.i) + $(REMOVEDIR) .dep + +doxygen: + @echo Generating Project Documentation... + @doxygen Doxygen.conf + @echo Documentation Generation Complete. + +clean_doxygen: + rm -rf Documentation + +# Create object files directory +$(shell mkdir $(OBJDIR) 2>/dev/null) + + +# Include the dependency files. +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + + +# Listing of phony targets. +.PHONY : all begin finish end sizebefore sizeafter gccversion \ +build elf hex eep lss sym coff extcoff doxygen clean \ +clean_list clean_doxygen program dfu flip flip-ee dfu-ee \ +debug gdb-config diff --git a/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/readme.txt b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/readme.txt new file mode 100644 index 0000000..289326b --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/firmwares/atmegaxxu2/arduino-usbserial/readme.txt @@ -0,0 +1,13 @@ +To setup the project and upload the Arduino usbserial application firmware to an ATMEGA8U2 using the Arduino USB DFU bootloader: +1. unpack the source into LUFA's Projects directory +2. set ARDUINO_MODEL_PID in the makefile as appropriate +3. do "make clean; make" +4. put the 8U2 into USB DFU mode: +4.a. assert and hold the 8U2's RESET line +4.b. assert and hold the 8U2's HWB line +4.c. release the 8U2's RESET line +4.d. release the 8U2's HWB line +5. confirm that the board enumerates as either "Arduino Uno DFU" or "Arduino Mega 2560 DFU" +6. do "make dfu" (OS X or Linux - dfu-programmer must be installed first) or "make flip" (Windows - Flip must be installed first) + +Check that the board enumerates as either "Arduino Uno" or "Arduino Mega 2560". Test by uploading a new Arduino sketch from the Arduino IDE. diff --git a/libs/arduino-1.0/hardware/arduino/programmers.txt b/libs/arduino-1.0/hardware/arduino/programmers.txt new file mode 100644 index 0000000..c34b88c --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/programmers.txt @@ -0,0 +1,26 @@ +# See: http://code.google.com/p/arduino/wiki/Platforms + +avrisp.name=AVR ISP +avrisp.communication=serial +avrisp.protocol=stk500v1 + +avrispmkii.name=AVRISP mkII +avrispmkii.communication=usb +avrispmkii.protocol=stk500v2 + +usbtinyisp.name=USBtinyISP +usbtinyisp.protocol=usbtiny + +usbasp.name=USBasp +usbasp.communication=usb +usbasp.protocol=usbasp + +parallel.name=Parallel Programmer +parallel.protocol=dapa +parallel.force=true +# parallel.delay=200 + +arduinoisp.name=Arduino as ISP +arduinoisp.communication=serial +arduinoisp.protocol=stk500v1 +arduinoisp.speed=19200 diff --git a/libs/arduino-1.0/hardware/arduino/variants/eightanaloginputs/pins_arduino.h b/libs/arduino-1.0/hardware/arduino/variants/eightanaloginputs/pins_arduino.h new file mode 100644 index 0000000..52b37ef --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/variants/eightanaloginputs/pins_arduino.h @@ -0,0 +1,27 @@ +/* + pins_arduino.h - Pin definition functions for Arduino + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2007 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ +*/ + +#include "../standard/pins_arduino.h" +#undef NUM_ANALOG_INPUTS +#define NUM_ANALOG_INPUTS 8 diff --git a/libs/arduino-1.0/hardware/arduino/variants/leonardo/pins_arduino.h b/libs/arduino-1.0/hardware/arduino/variants/leonardo/pins_arduino.h new file mode 100644 index 0000000..2c7f837 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/variants/leonardo/pins_arduino.h @@ -0,0 +1,337 @@ +/* + pins_arduino.h - Pin definition functions for Arduino + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2007 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ +*/ + +#ifndef Pins_Arduino_h +#define Pins_Arduino_h + +#include + +// Workaround for wrong definitions in "iom32u4.h". +// This should be fixed in the AVR toolchain. +#undef UHCON +#undef UHINT +#undef UHIEN +#undef UHADDR +#undef UHFNUM +#undef UHFNUML +#undef UHFNUMH +#undef UHFLEN +#undef UPINRQX +#undef UPINTX +#undef UPNUM +#undef UPRST +#undef UPCONX +#undef UPCFG0X +#undef UPCFG1X +#undef UPSTAX +#undef UPCFG2X +#undef UPIENX +#undef UPDATX +#undef TCCR2A +#undef WGM20 +#undef WGM21 +#undef COM2B0 +#undef COM2B1 +#undef COM2A0 +#undef COM2A1 +#undef TCCR2B +#undef CS20 +#undef CS21 +#undef CS22 +#undef WGM22 +#undef FOC2B +#undef FOC2A +#undef TCNT2 +#undef TCNT2_0 +#undef TCNT2_1 +#undef TCNT2_2 +#undef TCNT2_3 +#undef TCNT2_4 +#undef TCNT2_5 +#undef TCNT2_6 +#undef TCNT2_7 +#undef OCR2A +#undef OCR2_0 +#undef OCR2_1 +#undef OCR2_2 +#undef OCR2_3 +#undef OCR2_4 +#undef OCR2_5 +#undef OCR2_6 +#undef OCR2_7 +#undef OCR2B +#undef OCR2_0 +#undef OCR2_1 +#undef OCR2_2 +#undef OCR2_3 +#undef OCR2_4 +#undef OCR2_5 +#undef OCR2_6 +#undef OCR2_7 + +#define NUM_DIGITAL_PINS 30 +#define NUM_ANALOG_INPUTS 12 + +#define TX_RX_LED_INIT DDRD |= (1<<5), DDRB |= (1<<0) +#define TXLED0 PORTD |= (1<<5) +#define TXLED1 PORTD &= ~(1<<5) +#define RXLED0 PORTB |= (1<<0) +#define RXLED1 PORTB &= ~(1<<0) + +static const uint8_t SDA = 2; +static const uint8_t SCL = 3; + +// Map SPI port to 'new' pins D14..D17 +static const uint8_t SS = 17; +static const uint8_t MOSI = 16; +static const uint8_t MISO = 14; +static const uint8_t SCK = 15; + +// Mapping of analog pins as digital I/O +// A6-A11 share with digital pins +static const uint8_t A0 = 18; +static const uint8_t A1 = 19; +static const uint8_t A2 = 20; +static const uint8_t A3 = 21; +static const uint8_t A4 = 22; +static const uint8_t A5 = 23; +static const uint8_t A6 = 24; // D4 +static const uint8_t A7 = 25; // D6 +static const uint8_t A8 = 26; // D8 +static const uint8_t A9 = 27; // D9 +static const uint8_t A10 = 28; // D10 +static const uint8_t A11 = 29; // D12 + +#define digitalPinToPCICR(p) ((((p) >= 8 && (p) <= 11) || ((p) >= 14 && (p) <= 17) || ((p) >= A8 && (p) <= A10)) ? (&PCICR) : ((uint8_t *)0)) +#define digitalPinToPCICRbit(p) 0 +#define digitalPinToPCMSK(p) ((((p) >= 8 && (p) <= 11) || ((p) >= 14 && (p) <= 17) || ((p) >= A8 && (p) <= A10)) ? (&PCMSK0) : ((uint8_t *)0)) +#define digitalPinToPCMSKbit(p) ( ((p) >= 8 && (p) <= 11) ? (p) - 4 : ((p) == 14 ? 3 : ((p) == 15 ? 1 : ((p) == 16 ? 2 : ((p) == 17 ? 0 : (p - A8 + 4)))))) + +// __AVR_ATmega32U4__ has an unusual mapping of pins to channels +extern const uint8_t PROGMEM analog_pin_to_channel_PGM[]; +#define analogPinToChannel(P) ( pgm_read_byte( analog_pin_to_channel_PGM + (P) ) ) + +#ifdef ARDUINO_MAIN + +// On the Arduino board, digital pins are also used +// for the analog output (software PWM). Analog input +// pins are a separate set. + +// ATMEL ATMEGA32U4 / ARDUINO LEONARDO +// +// D0 PD2 RXD1/INT2 +// D1 PD3 TXD1/INT3 +// D2 PD1 SDA SDA/INT1 +// D3# PD0 PWM8/SCL OC0B/SCL/INT0 +// D4 A6 PD4 ADC8 +// D5# PC6 ??? OC3A/#OC4A +// D6# A7 PD7 FastPWM #OC4D/ADC10 +// D7 PE6 INT6/AIN0 +// +// D8 A8 PB4 ADC11/PCINT4 +// D9# A9 PB5 PWM16 OC1A/#OC4B/ADC12/PCINT5 +// D10# A10 PB6 PWM16 OC1B/0c4B/ADC13/PCINT6 +// D11# PB7 PWM8/16 0C0A/OC1C/#RTS/PCINT7 +// D12 A11 PD6 T1/#OC4D/ADC9 +// D13# PC7 PWM10 CLK0/OC4A +// +// A0 D18 PF7 ADC7 +// A1 D19 PF6 ADC6 +// A2 D20 PF5 ADC5 +// A3 D21 PF4 ADC4 +// A4 D22 PF1 ADC1 +// A5 D23 PF0 ADC0 +// +// New pins D14..D17 to map SPI port to digital pins +// +// MISO D14 PB3 MISO,PCINT3 +// SCK D15 PB1 SCK,PCINT1 +// MOSI D16 PB2 MOSI,PCINT2 +// SS D17 PB0 RXLED,SS/PCINT0 +// +// TXLED PD5 +// RXLED PB0 +// HWB PE2 HWB + +// these arrays map port names (e.g. port B) to the +// appropriate addresses for various functions (e.g. reading +// and writing) +const uint16_t PROGMEM port_to_mode_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &DDRB, + (uint16_t) &DDRC, + (uint16_t) &DDRD, + (uint16_t) &DDRE, + (uint16_t) &DDRF, +}; + +const uint16_t PROGMEM port_to_output_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &PORTB, + (uint16_t) &PORTC, + (uint16_t) &PORTD, + (uint16_t) &PORTE, + (uint16_t) &PORTF, +}; + +const uint16_t PROGMEM port_to_input_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &PINB, + (uint16_t) &PINC, + (uint16_t) &PIND, + (uint16_t) &PINE, + (uint16_t) &PINF, +}; + +const uint8_t PROGMEM digital_pin_to_port_PGM[] = { + PD, // D0 - PD2 + PD, // D1 - PD3 + PD, // D2 - PD1 + PD, // D3 - PD0 + PD, // D4 - PD4 + PC, // D5 - PC6 + PD, // D6 - PD7 + PE, // D7 - PE6 + + PB, // D8 - PB4 + PB, // D9 - PB5 + PB, // D10 - PB6 + PB, // D11 - PB7 + PD, // D12 - PD6 + PC, // D13 - PC7 + + PB, // D14 - MISO - PB3 + PB, // D15 - SCK - PB1 + PB, // D16 - MOSI - PB2 + PB, // D17 - SS - PB0 + + PF, // D18 - A0 - PF7 + PF, // D19 - A1 - PF6 + PF, // D20 - A2 - PF5 + PF, // D21 - A3 - PF4 + PF, // D22 - A4 - PF1 + PF, // D23 - A5 - PF0 + + PD, // D24 / D4 - A6 - PD4 + PD, // D25 / D6 - A7 - PD7 + PB, // D26 / D8 - A8 - PB4 + PB, // D27 / D9 - A9 - PB5 + PB, // D28 / D10 - A10 - PB6 + PD, // D29 / D12 - A11 - PD6 +}; + +const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[] = { + _BV(2), // D0 - PD2 + _BV(3), // D1 - PD3 + _BV(1), // D2 - PD1 + _BV(0), // D3 - PD0 + _BV(4), // D4 - PD4 + _BV(6), // D5 - PC6 + _BV(7), // D6 - PD7 + _BV(6), // D7 - PE6 + + _BV(4), // D8 - PB4 + _BV(5), // D9 - PB5 + _BV(6), // D10 - PB6 + _BV(7), // D11 - PB7 + _BV(6), // D12 - PD6 + _BV(7), // D13 - PC7 + + _BV(3), // D14 - MISO - PB3 + _BV(1), // D15 - SCK - PB1 + _BV(2), // D16 - MOSI - PB2 + _BV(0), // D17 - SS - PB0 + + _BV(7), // D18 - A0 - PF7 + _BV(6), // D19 - A1 - PF6 + _BV(5), // D20 - A2 - PF5 + _BV(4), // D21 - A3 - PF4 + _BV(1), // D22 - A4 - PF1 + _BV(0), // D23 - A5 - PF0 + + _BV(4), // D24 / D4 - A6 - PD4 + _BV(7), // D25 / D6 - A7 - PD7 + _BV(4), // D26 / D8 - A8 - PB4 + _BV(5), // D27 / D9 - A9 - PB5 + _BV(6), // D28 / D10 - A10 - PB6 + _BV(6), // D29 / D12 - A11 - PD6 +}; + +const uint8_t PROGMEM digital_pin_to_timer_PGM[] = { + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + TIMER0B, /* 3 */ + NOT_ON_TIMER, + TIMER3A, /* 5 */ + TIMER4D, /* 6 */ + NOT_ON_TIMER, + + NOT_ON_TIMER, + TIMER1A, /* 9 */ + TIMER1B, /* 10 */ + TIMER0A, /* 11 */ + + NOT_ON_TIMER, + TIMER4A, /* 13 */ + + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, +}; + +const uint8_t PROGMEM analog_pin_to_channel_PGM[] = { + 7, // A0 PF7 ADC7 + 6, // A1 PF6 ADC6 + 5, // A2 PF5 ADC5 + 4, // A3 PF4 ADC4 + 1, // A4 PF1 ADC1 + 0, // A5 PF0 ADC0 + 8, // A6 D4 PD4 ADC8 + 10, // A7 D6 PD7 ADC10 + 11, // A8 D8 PB4 ADC11 + 12, // A9 D9 PB5 ADC12 + 13, // A10 D10 PB6 ADC13 + 9 // A11 D12 PD6 ADC9 +}; + +#endif /* ARDUINO_MAIN */ +#endif /* Pins_Arduino_h */ diff --git a/libs/arduino-1.0/hardware/arduino/variants/mega/pins_arduino.h b/libs/arduino-1.0/hardware/arduino/variants/mega/pins_arduino.h new file mode 100644 index 0000000..5a9b4cb --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/variants/mega/pins_arduino.h @@ -0,0 +1,363 @@ +/* + pins_arduino.h - Pin definition functions for Arduino + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2007 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ +*/ + +#ifndef Pins_Arduino_h +#define Pins_Arduino_h + +#include + +#define NUM_DIGITAL_PINS 70 +#define NUM_ANALOG_INPUTS 16 +#define analogInputToDigitalPin(p) ((p < 16) ? (p) + 54 : -1) +#define digitalPinHasPWM(p) (((p) >= 2 && (p) <= 13) || ((p) >= 44 && (p)<= 46)) + +static const uint8_t SS = 53; +static const uint8_t MOSI = 51; +static const uint8_t MISO = 50; +static const uint8_t SCK = 52; + +static const uint8_t SDA = 20; +static const uint8_t SCL = 21; +static const uint8_t LED_BUILTIN = 13; + +static const uint8_t A0 = 54; +static const uint8_t A1 = 55; +static const uint8_t A2 = 56; +static const uint8_t A3 = 57; +static const uint8_t A4 = 58; +static const uint8_t A5 = 59; +static const uint8_t A6 = 60; +static const uint8_t A7 = 61; +static const uint8_t A8 = 62; +static const uint8_t A9 = 63; +static const uint8_t A10 = 64; +static const uint8_t A11 = 65; +static const uint8_t A12 = 66; +static const uint8_t A13 = 67; +static const uint8_t A14 = 68; +static const uint8_t A15 = 69; + +// A majority of the pins are NOT PCINTs, SO BE WARNED (i.e. you cannot use them as receive pins) +// Only pins available for RECEIVE (TRANSMIT can be on any pin): +// (I've deliberately left out pin mapping to the Hardware USARTs - seems senseless to me) +// Pins: 10, 11, 12, 13, 50, 51, 52, 53, 62, 63, 64, 65, 66, 67, 68, 69 + +#define digitalPinToPCICR(p) ( (((p) >= 10) && ((p) <= 13)) || \ + (((p) >= 50) && ((p) <= 53)) || \ + (((p) >= 62) && ((p) <= 69)) ? (&PCICR) : ((uint8_t *)0) ) + +#define digitalPinToPCICRbit(p) ( (((p) >= 10) && ((p) <= 13)) || (((p) >= 50) && ((p) <= 53)) ? 0 : \ + ( (((p) >= 62) && ((p) <= 69)) ? 2 : \ + 0 ) ) + +#define digitalPinToPCMSK(p) ( (((p) >= 10) && ((p) <= 13)) || (((p) >= 50) && ((p) <= 53)) ? (&PCMSK0) : \ + ( (((p) >= 62) && ((p) <= 69)) ? (&PCMSK2) : \ + ((uint8_t *)0) ) ) + +#define digitalPinToPCMSKbit(p) ( (((p) >= 10) && ((p) <= 13)) ? ((p) - 6) : \ + ( ((p) == 50) ? 3 : \ + ( ((p) == 51) ? 2 : \ + ( ((p) == 52) ? 1 : \ + ( ((p) == 53) ? 0 : \ + ( (((p) >= 62) && ((p) <= 69)) ? ((p) - 62) : \ + 0 ) ) ) ) ) ) + +#ifdef ARDUINO_MAIN + +const uint16_t PROGMEM port_to_mode_PGM[] = { + NOT_A_PORT, + (uint16_t) &DDRA, + (uint16_t) &DDRB, + (uint16_t) &DDRC, + (uint16_t) &DDRD, + (uint16_t) &DDRE, + (uint16_t) &DDRF, + (uint16_t) &DDRG, + (uint16_t) &DDRH, + NOT_A_PORT, + (uint16_t) &DDRJ, + (uint16_t) &DDRK, + (uint16_t) &DDRL, +}; + +const uint16_t PROGMEM port_to_output_PGM[] = { + NOT_A_PORT, + (uint16_t) &PORTA, + (uint16_t) &PORTB, + (uint16_t) &PORTC, + (uint16_t) &PORTD, + (uint16_t) &PORTE, + (uint16_t) &PORTF, + (uint16_t) &PORTG, + (uint16_t) &PORTH, + NOT_A_PORT, + (uint16_t) &PORTJ, + (uint16_t) &PORTK, + (uint16_t) &PORTL, +}; + +const uint16_t PROGMEM port_to_input_PGM[] = { + NOT_A_PIN, + (uint16_t) &PINA, + (uint16_t) &PINB, + (uint16_t) &PINC, + (uint16_t) &PIND, + (uint16_t) &PINE, + (uint16_t) &PINF, + (uint16_t) &PING, + (uint16_t) &PINH, + NOT_A_PIN, + (uint16_t) &PINJ, + (uint16_t) &PINK, + (uint16_t) &PINL, +}; + +const uint8_t PROGMEM digital_pin_to_port_PGM[] = { + // PORTLIST + // ------------------------------------------- + PE , // PE 0 ** 0 ** USART0_RX + PE , // PE 1 ** 1 ** USART0_TX + PE , // PE 4 ** 2 ** PWM2 + PE , // PE 5 ** 3 ** PWM3 + PG , // PG 5 ** 4 ** PWM4 + PE , // PE 3 ** 5 ** PWM5 + PH , // PH 3 ** 6 ** PWM6 + PH , // PH 4 ** 7 ** PWM7 + PH , // PH 5 ** 8 ** PWM8 + PH , // PH 6 ** 9 ** PWM9 + PB , // PB 4 ** 10 ** PWM10 + PB , // PB 5 ** 11 ** PWM11 + PB , // PB 6 ** 12 ** PWM12 + PB , // PB 7 ** 13 ** PWM13 + PJ , // PJ 1 ** 14 ** USART3_TX + PJ , // PJ 0 ** 15 ** USART3_RX + PH , // PH 1 ** 16 ** USART2_TX + PH , // PH 0 ** 17 ** USART2_RX + PD , // PD 3 ** 18 ** USART1_TX + PD , // PD 2 ** 19 ** USART1_RX + PD , // PD 1 ** 20 ** I2C_SDA + PD , // PD 0 ** 21 ** I2C_SCL + PA , // PA 0 ** 22 ** D22 + PA , // PA 1 ** 23 ** D23 + PA , // PA 2 ** 24 ** D24 + PA , // PA 3 ** 25 ** D25 + PA , // PA 4 ** 26 ** D26 + PA , // PA 5 ** 27 ** D27 + PA , // PA 6 ** 28 ** D28 + PA , // PA 7 ** 29 ** D29 + PC , // PC 7 ** 30 ** D30 + PC , // PC 6 ** 31 ** D31 + PC , // PC 5 ** 32 ** D32 + PC , // PC 4 ** 33 ** D33 + PC , // PC 3 ** 34 ** D34 + PC , // PC 2 ** 35 ** D35 + PC , // PC 1 ** 36 ** D36 + PC , // PC 0 ** 37 ** D37 + PD , // PD 7 ** 38 ** D38 + PG , // PG 2 ** 39 ** D39 + PG , // PG 1 ** 40 ** D40 + PG , // PG 0 ** 41 ** D41 + PL , // PL 7 ** 42 ** D42 + PL , // PL 6 ** 43 ** D43 + PL , // PL 5 ** 44 ** D44 + PL , // PL 4 ** 45 ** D45 + PL , // PL 3 ** 46 ** D46 + PL , // PL 2 ** 47 ** D47 + PL , // PL 1 ** 48 ** D48 + PL , // PL 0 ** 49 ** D49 + PB , // PB 3 ** 50 ** SPI_MISO + PB , // PB 2 ** 51 ** SPI_MOSI + PB , // PB 1 ** 52 ** SPI_SCK + PB , // PB 0 ** 53 ** SPI_SS + PF , // PF 0 ** 54 ** A0 + PF , // PF 1 ** 55 ** A1 + PF , // PF 2 ** 56 ** A2 + PF , // PF 3 ** 57 ** A3 + PF , // PF 4 ** 58 ** A4 + PF , // PF 5 ** 59 ** A5 + PF , // PF 6 ** 60 ** A6 + PF , // PF 7 ** 61 ** A7 + PK , // PK 0 ** 62 ** A8 + PK , // PK 1 ** 63 ** A9 + PK , // PK 2 ** 64 ** A10 + PK , // PK 3 ** 65 ** A11 + PK , // PK 4 ** 66 ** A12 + PK , // PK 5 ** 67 ** A13 + PK , // PK 6 ** 68 ** A14 + PK , // PK 7 ** 69 ** A15 +}; + +const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[] = { + // PIN IN PORT + // ------------------------------------------- + _BV( 0 ) , // PE 0 ** 0 ** USART0_RX + _BV( 1 ) , // PE 1 ** 1 ** USART0_TX + _BV( 4 ) , // PE 4 ** 2 ** PWM2 + _BV( 5 ) , // PE 5 ** 3 ** PWM3 + _BV( 5 ) , // PG 5 ** 4 ** PWM4 + _BV( 3 ) , // PE 3 ** 5 ** PWM5 + _BV( 3 ) , // PH 3 ** 6 ** PWM6 + _BV( 4 ) , // PH 4 ** 7 ** PWM7 + _BV( 5 ) , // PH 5 ** 8 ** PWM8 + _BV( 6 ) , // PH 6 ** 9 ** PWM9 + _BV( 4 ) , // PB 4 ** 10 ** PWM10 + _BV( 5 ) , // PB 5 ** 11 ** PWM11 + _BV( 6 ) , // PB 6 ** 12 ** PWM12 + _BV( 7 ) , // PB 7 ** 13 ** PWM13 + _BV( 1 ) , // PJ 1 ** 14 ** USART3_TX + _BV( 0 ) , // PJ 0 ** 15 ** USART3_RX + _BV( 1 ) , // PH 1 ** 16 ** USART2_TX + _BV( 0 ) , // PH 0 ** 17 ** USART2_RX + _BV( 3 ) , // PD 3 ** 18 ** USART1_TX + _BV( 2 ) , // PD 2 ** 19 ** USART1_RX + _BV( 1 ) , // PD 1 ** 20 ** I2C_SDA + _BV( 0 ) , // PD 0 ** 21 ** I2C_SCL + _BV( 0 ) , // PA 0 ** 22 ** D22 + _BV( 1 ) , // PA 1 ** 23 ** D23 + _BV( 2 ) , // PA 2 ** 24 ** D24 + _BV( 3 ) , // PA 3 ** 25 ** D25 + _BV( 4 ) , // PA 4 ** 26 ** D26 + _BV( 5 ) , // PA 5 ** 27 ** D27 + _BV( 6 ) , // PA 6 ** 28 ** D28 + _BV( 7 ) , // PA 7 ** 29 ** D29 + _BV( 7 ) , // PC 7 ** 30 ** D30 + _BV( 6 ) , // PC 6 ** 31 ** D31 + _BV( 5 ) , // PC 5 ** 32 ** D32 + _BV( 4 ) , // PC 4 ** 33 ** D33 + _BV( 3 ) , // PC 3 ** 34 ** D34 + _BV( 2 ) , // PC 2 ** 35 ** D35 + _BV( 1 ) , // PC 1 ** 36 ** D36 + _BV( 0 ) , // PC 0 ** 37 ** D37 + _BV( 7 ) , // PD 7 ** 38 ** D38 + _BV( 2 ) , // PG 2 ** 39 ** D39 + _BV( 1 ) , // PG 1 ** 40 ** D40 + _BV( 0 ) , // PG 0 ** 41 ** D41 + _BV( 7 ) , // PL 7 ** 42 ** D42 + _BV( 6 ) , // PL 6 ** 43 ** D43 + _BV( 5 ) , // PL 5 ** 44 ** D44 + _BV( 4 ) , // PL 4 ** 45 ** D45 + _BV( 3 ) , // PL 3 ** 46 ** D46 + _BV( 2 ) , // PL 2 ** 47 ** D47 + _BV( 1 ) , // PL 1 ** 48 ** D48 + _BV( 0 ) , // PL 0 ** 49 ** D49 + _BV( 3 ) , // PB 3 ** 50 ** SPI_MISO + _BV( 2 ) , // PB 2 ** 51 ** SPI_MOSI + _BV( 1 ) , // PB 1 ** 52 ** SPI_SCK + _BV( 0 ) , // PB 0 ** 53 ** SPI_SS + _BV( 0 ) , // PF 0 ** 54 ** A0 + _BV( 1 ) , // PF 1 ** 55 ** A1 + _BV( 2 ) , // PF 2 ** 56 ** A2 + _BV( 3 ) , // PF 3 ** 57 ** A3 + _BV( 4 ) , // PF 4 ** 58 ** A4 + _BV( 5 ) , // PF 5 ** 59 ** A5 + _BV( 6 ) , // PF 6 ** 60 ** A6 + _BV( 7 ) , // PF 7 ** 61 ** A7 + _BV( 0 ) , // PK 0 ** 62 ** A8 + _BV( 1 ) , // PK 1 ** 63 ** A9 + _BV( 2 ) , // PK 2 ** 64 ** A10 + _BV( 3 ) , // PK 3 ** 65 ** A11 + _BV( 4 ) , // PK 4 ** 66 ** A12 + _BV( 5 ) , // PK 5 ** 67 ** A13 + _BV( 6 ) , // PK 6 ** 68 ** A14 + _BV( 7 ) , // PK 7 ** 69 ** A15 +}; + +const uint8_t PROGMEM digital_pin_to_timer_PGM[] = { + // TIMERS + // ------------------------------------------- + NOT_ON_TIMER , // PE 0 ** 0 ** USART0_RX + NOT_ON_TIMER , // PE 1 ** 1 ** USART0_TX + TIMER3B , // PE 4 ** 2 ** PWM2 + TIMER3C , // PE 5 ** 3 ** PWM3 + TIMER0B , // PG 5 ** 4 ** PWM4 + TIMER3A , // PE 3 ** 5 ** PWM5 + TIMER4A , // PH 3 ** 6 ** PWM6 + TIMER4B , // PH 4 ** 7 ** PWM7 + TIMER4C , // PH 5 ** 8 ** PWM8 + TIMER2B , // PH 6 ** 9 ** PWM9 + TIMER2A , // PB 4 ** 10 ** PWM10 + TIMER1A , // PB 5 ** 11 ** PWM11 + TIMER1B , // PB 6 ** 12 ** PWM12 + TIMER0A , // PB 7 ** 13 ** PWM13 + NOT_ON_TIMER , // PJ 1 ** 14 ** USART3_TX + NOT_ON_TIMER , // PJ 0 ** 15 ** USART3_RX + NOT_ON_TIMER , // PH 1 ** 16 ** USART2_TX + NOT_ON_TIMER , // PH 0 ** 17 ** USART2_RX + NOT_ON_TIMER , // PD 3 ** 18 ** USART1_TX + NOT_ON_TIMER , // PD 2 ** 19 ** USART1_RX + NOT_ON_TIMER , // PD 1 ** 20 ** I2C_SDA + NOT_ON_TIMER , // PD 0 ** 21 ** I2C_SCL + NOT_ON_TIMER , // PA 0 ** 22 ** D22 + NOT_ON_TIMER , // PA 1 ** 23 ** D23 + NOT_ON_TIMER , // PA 2 ** 24 ** D24 + NOT_ON_TIMER , // PA 3 ** 25 ** D25 + NOT_ON_TIMER , // PA 4 ** 26 ** D26 + NOT_ON_TIMER , // PA 5 ** 27 ** D27 + NOT_ON_TIMER , // PA 6 ** 28 ** D28 + NOT_ON_TIMER , // PA 7 ** 29 ** D29 + NOT_ON_TIMER , // PC 7 ** 30 ** D30 + NOT_ON_TIMER , // PC 6 ** 31 ** D31 + NOT_ON_TIMER , // PC 5 ** 32 ** D32 + NOT_ON_TIMER , // PC 4 ** 33 ** D33 + NOT_ON_TIMER , // PC 3 ** 34 ** D34 + NOT_ON_TIMER , // PC 2 ** 35 ** D35 + NOT_ON_TIMER , // PC 1 ** 36 ** D36 + NOT_ON_TIMER , // PC 0 ** 37 ** D37 + NOT_ON_TIMER , // PD 7 ** 38 ** D38 + NOT_ON_TIMER , // PG 2 ** 39 ** D39 + NOT_ON_TIMER , // PG 1 ** 40 ** D40 + NOT_ON_TIMER , // PG 0 ** 41 ** D41 + NOT_ON_TIMER , // PL 7 ** 42 ** D42 + NOT_ON_TIMER , // PL 6 ** 43 ** D43 + TIMER5C , // PL 5 ** 44 ** D44 + TIMER5B , // PL 4 ** 45 ** D45 + TIMER5A , // PL 3 ** 46 ** D46 + NOT_ON_TIMER , // PL 2 ** 47 ** D47 + NOT_ON_TIMER , // PL 1 ** 48 ** D48 + NOT_ON_TIMER , // PL 0 ** 49 ** D49 + NOT_ON_TIMER , // PB 3 ** 50 ** SPI_MISO + NOT_ON_TIMER , // PB 2 ** 51 ** SPI_MOSI + NOT_ON_TIMER , // PB 1 ** 52 ** SPI_SCK + NOT_ON_TIMER , // PB 0 ** 53 ** SPI_SS + NOT_ON_TIMER , // PF 0 ** 54 ** A0 + NOT_ON_TIMER , // PF 1 ** 55 ** A1 + NOT_ON_TIMER , // PF 2 ** 56 ** A2 + NOT_ON_TIMER , // PF 3 ** 57 ** A3 + NOT_ON_TIMER , // PF 4 ** 58 ** A4 + NOT_ON_TIMER , // PF 5 ** 59 ** A5 + NOT_ON_TIMER , // PF 6 ** 60 ** A6 + NOT_ON_TIMER , // PF 7 ** 61 ** A7 + NOT_ON_TIMER , // PK 0 ** 62 ** A8 + NOT_ON_TIMER , // PK 1 ** 63 ** A9 + NOT_ON_TIMER , // PK 2 ** 64 ** A10 + NOT_ON_TIMER , // PK 3 ** 65 ** A11 + NOT_ON_TIMER , // PK 4 ** 66 ** A12 + NOT_ON_TIMER , // PK 5 ** 67 ** A13 + NOT_ON_TIMER , // PK 6 ** 68 ** A14 + NOT_ON_TIMER , // PK 7 ** 69 ** A15 +}; + +#endif + +#endif \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/variants/micro/pins_arduino.h b/libs/arduino-1.0/hardware/arduino/variants/micro/pins_arduino.h new file mode 100644 index 0000000..ea8e42d --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/variants/micro/pins_arduino.h @@ -0,0 +1,37 @@ +/* + pins_arduino.h - Pin definition functions for Arduino + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2007 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ +*/ + +#include "../leonardo/pins_arduino.h" + +#undef TXLED0 +#undef TXLED1 +#undef RXLED0 +#undef RXLED1 +#undef TX_RX_LED_INIT + +#define TXLED0 PORTD &= ~(1<<5) +#define TXLED1 PORTD |= (1<<5) +#define RXLED0 PORTB &= ~(1<<0) +#define RXLED1 PORTB |= (1<<0) +#define TX_RX_LED_INIT DDRD |= (1<<5), DDRB |= (1<<0), TXLED0, RXLED0 \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/arduino/variants/standard/pins_arduino.h b/libs/arduino-1.0/hardware/arduino/variants/standard/pins_arduino.h new file mode 100644 index 0000000..30b4266 --- /dev/null +++ b/libs/arduino-1.0/hardware/arduino/variants/standard/pins_arduino.h @@ -0,0 +1,218 @@ +/* + pins_arduino.h - Pin definition functions for Arduino + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2007 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ +*/ + +#ifndef Pins_Arduino_h +#define Pins_Arduino_h + +#include + +#define NUM_DIGITAL_PINS 20 +#define NUM_ANALOG_INPUTS 6 +#define analogInputToDigitalPin(p) ((p < 6) ? (p) + 14 : -1) + +#if defined(__AVR_ATmega8__) +#define digitalPinHasPWM(p) ((p) == 9 || (p) == 10 || (p) == 11) +#else +#define digitalPinHasPWM(p) ((p) == 3 || (p) == 5 || (p) == 6 || (p) == 9 || (p) == 10 || (p) == 11) +#endif + +static const uint8_t SS = 10; +static const uint8_t MOSI = 11; +static const uint8_t MISO = 12; +static const uint8_t SCK = 13; + +static const uint8_t SDA = 18; +static const uint8_t SCL = 19; +static const uint8_t LED_BUILTIN = 13; + +static const uint8_t A0 = 14; +static const uint8_t A1 = 15; +static const uint8_t A2 = 16; +static const uint8_t A3 = 17; +static const uint8_t A4 = 18; +static const uint8_t A5 = 19; +static const uint8_t A6 = 20; +static const uint8_t A7 = 21; + +#define digitalPinToPCICR(p) (((p) >= 0 && (p) <= 21) ? (&PCICR) : ((uint8_t *)0)) +#define digitalPinToPCICRbit(p) (((p) <= 7) ? 2 : (((p) <= 13) ? 0 : 1)) +#define digitalPinToPCMSK(p) (((p) <= 7) ? (&PCMSK2) : (((p) <= 13) ? (&PCMSK0) : (((p) <= 21) ? (&PCMSK1) : ((uint8_t *)0)))) +#define digitalPinToPCMSKbit(p) (((p) <= 7) ? (p) : (((p) <= 13) ? ((p) - 8) : ((p) - 14))) + +#ifdef ARDUINO_MAIN + +// On the Arduino board, digital pins are also used +// for the analog output (software PWM). Analog input +// pins are a separate set. + +// ATMEL ATMEGA8 & 168 / ARDUINO +// +// +-\/-+ +// PC6 1| |28 PC5 (AI 5) +// (D 0) PD0 2| |27 PC4 (AI 4) +// (D 1) PD1 3| |26 PC3 (AI 3) +// (D 2) PD2 4| |25 PC2 (AI 2) +// PWM+ (D 3) PD3 5| |24 PC1 (AI 1) +// (D 4) PD4 6| |23 PC0 (AI 0) +// VCC 7| |22 GND +// GND 8| |21 AREF +// PB6 9| |20 AVCC +// PB7 10| |19 PB5 (D 13) +// PWM+ (D 5) PD5 11| |18 PB4 (D 12) +// PWM+ (D 6) PD6 12| |17 PB3 (D 11) PWM +// (D 7) PD7 13| |16 PB2 (D 10) PWM +// (D 8) PB0 14| |15 PB1 (D 9) PWM +// +----+ +// +// (PWM+ indicates the additional PWM pins on the ATmega168.) + +// ATMEL ATMEGA1280 / ARDUINO +// +// 0-7 PE0-PE7 works +// 8-13 PB0-PB5 works +// 14-21 PA0-PA7 works +// 22-29 PH0-PH7 works +// 30-35 PG5-PG0 works +// 36-43 PC7-PC0 works +// 44-51 PJ7-PJ0 works +// 52-59 PL7-PL0 works +// 60-67 PD7-PD0 works +// A0-A7 PF0-PF7 +// A8-A15 PK0-PK7 + + +// these arrays map port names (e.g. port B) to the +// appropriate addresses for various functions (e.g. reading +// and writing) +const uint16_t PROGMEM port_to_mode_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &DDRB, + (uint16_t) &DDRC, + (uint16_t) &DDRD, +}; + +const uint16_t PROGMEM port_to_output_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &PORTB, + (uint16_t) &PORTC, + (uint16_t) &PORTD, +}; + +const uint16_t PROGMEM port_to_input_PGM[] = { + NOT_A_PORT, + NOT_A_PORT, + (uint16_t) &PINB, + (uint16_t) &PINC, + (uint16_t) &PIND, +}; + +const uint8_t PROGMEM digital_pin_to_port_PGM[] = { + PD, /* 0 */ + PD, + PD, + PD, + PD, + PD, + PD, + PD, + PB, /* 8 */ + PB, + PB, + PB, + PB, + PB, + PC, /* 14 */ + PC, + PC, + PC, + PC, + PC, +}; + +const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[] = { + _BV(0), /* 0, port D */ + _BV(1), + _BV(2), + _BV(3), + _BV(4), + _BV(5), + _BV(6), + _BV(7), + _BV(0), /* 8, port B */ + _BV(1), + _BV(2), + _BV(3), + _BV(4), + _BV(5), + _BV(0), /* 14, port C */ + _BV(1), + _BV(2), + _BV(3), + _BV(4), + _BV(5), +}; + +const uint8_t PROGMEM digital_pin_to_timer_PGM[] = { + NOT_ON_TIMER, /* 0 - port D */ + NOT_ON_TIMER, + NOT_ON_TIMER, + // on the ATmega168, digital pin 3 has hardware pwm +#if defined(__AVR_ATmega8__) + NOT_ON_TIMER, +#else + TIMER2B, +#endif + NOT_ON_TIMER, + // on the ATmega168, digital pins 5 and 6 have hardware pwm +#if defined(__AVR_ATmega8__) + NOT_ON_TIMER, + NOT_ON_TIMER, +#else + TIMER0B, + TIMER0A, +#endif + NOT_ON_TIMER, + NOT_ON_TIMER, /* 8 - port B */ + TIMER1A, + TIMER1B, +#if defined(__AVR_ATmega8__) + TIMER2, +#else + TIMER2A, +#endif + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, /* 14 - port C */ + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, + NOT_ON_TIMER, +}; + +#endif + +#endif diff --git a/libs/arduino-1.0/hardware/tools/avrdude b/libs/arduino-1.0/hardware/tools/avrdude new file mode 100644 index 0000000..2408472 --- /dev/null +++ b/libs/arduino-1.0/hardware/tools/avrdude @@ -0,0 +1 @@ +../../../../bin/avrdude \ No newline at end of file diff --git a/libs/arduino-1.0/hardware/tools/avrdude.conf b/libs/arduino-1.0/hardware/tools/avrdude.conf new file mode 100644 index 0000000..ea17eb9 --- /dev/null +++ b/libs/arduino-1.0/hardware/tools/avrdude.conf @@ -0,0 +1 @@ +/etc/avrdude.conf \ No newline at end of file diff --git a/libs/arduino-1.0/lib/RXTXcomm.jar b/libs/arduino-1.0/lib/RXTXcomm.jar new file mode 100644 index 0000000..116bc61 --- /dev/null +++ b/libs/arduino-1.0/lib/RXTXcomm.jar @@ -0,0 +1 @@ +../../java/RXTXcomm.jar \ No newline at end of file diff --git a/libs/arduino-1.0/lib/about.jpg b/libs/arduino-1.0/lib/about.jpg new file mode 100644 index 0000000..0b168bb Binary files /dev/null and b/libs/arduino-1.0/lib/about.jpg differ diff --git a/libs/arduino-1.0/lib/core.jar b/libs/arduino-1.0/lib/core.jar new file mode 100644 index 0000000..7a2c7b5 Binary files /dev/null and b/libs/arduino-1.0/lib/core.jar differ diff --git a/libs/arduino-1.0/lib/jna.jar b/libs/arduino-1.0/lib/jna.jar new file mode 100644 index 0000000..899c346 --- /dev/null +++ b/libs/arduino-1.0/lib/jna.jar @@ -0,0 +1 @@ +../../java/jna.jar \ No newline at end of file diff --git a/libs/arduino-1.0/lib/keywords.txt b/libs/arduino-1.0/lib/keywords.txt new file mode 100644 index 0000000..ac7de85 --- /dev/null +++ b/libs/arduino-1.0/lib/keywords.txt @@ -0,0 +1,199 @@ +# LITERAL1 specifies constants + +HIGH LITERAL1 Constants +LOW LITERAL1 Constants +INPUT LITERAL1 Constants +INPUT_PULLUP LITERAL1 Constants +OUTPUT LITERAL1 Constants +DEC LITERAL1 Serial_Print +BIN LITERAL1 Serial_Print +HEX LITERAL1 Serial_Print +OCT LITERAL1 Serial_Print +PI LITERAL1 +HALF_PI LITERAL1 +TWO_PI LITERAL1 +LSBFIRST LITERAL1 ShiftOut +MSBFIRST LITERAL1 ShiftOut +CHANGE LITERAL1 AttachInterrupt +FALLING LITERAL1 AttachInterrupt +RISING LITERAL1 AttachInterrupt +DEFAULT LITERAL1 AnalogReference +EXTERNAL LITERAL1 AnalogReference +INTERNAL LITERAL1 AnalogReference +INTERNAL1V1 LITERAL1 AnalogReference +INTERNAL2V56 LITERAL1 AnalogReference + +# KEYWORD1 specifies datatypes and C/C++ keywords + +boolean KEYWORD1 BooleanVariables +break KEYWORD1 Break +byte KEYWORD1 Byte +case KEYWORD1 SwitchCase +char KEYWORD1 Char +class KEYWORD1 +const KEYWORD1 Const +continue KEYWORD1 Continue +default KEYWORD1 SwitchCase +do KEYWORD1 DoWhile +double KEYWORD1 Double +else KEYWORD1 Else +false KEYWORD1 Constants +float KEYWORD1 Float +for KEYWORD1 For +if KEYWORD1 If +int KEYWORD1 Int +long KEYWORD1 Long +new KEYWORD1 +null KEYWORD1 +private KEYWORD1 +protected KEYWORD1 +public KEYWORD1 +register KEYWORD1 +return KEYWORD1 Return +short KEYWORD1 +signed KEYWORD1 +static KEYWORD1 Static +String KEYWORD1 String +switch KEYWORD1 SwitchCase +this KEYWORD1 +throw KEYWORD1 +try KEYWORD1 +true KEYWORD1 +unsigned KEYWORD1 +void KEYWORD1 Void +while KEYWORD1 While +word KEYWORD1 Word + +# operators aren't highlighted, but may have documentation + ++= IncrementCompound ++ Arithmetic +[] arrayaccess += assign +& BitwiseAnd +| BitwiseAnd +, +// Comments +?: +{} Braces +-- Increment +/ Arithmetic +/* Comments +. dot +== +< greaterthan +<= greaterthanorequalto +++ Increment +!= inequality +<< Bitshift +< lessthan +<= lessthanorequalto +&& Boolean +! Boolean +|| Boolean +- Arithmetic +% Modulo +* Arithmetic +() parentheses +>> Bitshift +; SemiColon +-= IncrementCompound + +# these are datatypes, but we've also defined functions to cast to them + +boolean KEYWORD2 boolean_ +byte KEYWORD2 byte_ +char KEYWORD2 char_ +float KEYWORD2 float_ +int KEYWORD2 int_ +long KEYWORD2 long_ +word KEYWORD2 word_ + +# KEYWORD2 specifies methods and functions + +abs KEYWORD2 Abs +acos KEYWORD2 ACos +asin KEYWORD2 ASin +atan KEYWORD2 ATan +atan2 KEYWORD2 ATan2 +ceil KEYWORD2 Ceil +constrain KEYWORD2 Constrain +cos KEYWORD2 Cos +degrees KEYWORD2 +exp KEYWORD2 Exp +floor KEYWORD2 Floor +log KEYWORD2 Log +map KEYWORD2 Map +max KEYWORD2 Max +min KEYWORD2 Min +radians KEYWORD2 +random KEYWORD2 Random +randomSeed KEYWORD2 RandomSeed +round KEYWORD2 +sin KEYWORD2 Sin +sq KEYWORD2 Sq +sqrt KEYWORD2 Sqrt +tan KEYWORD2 Tan + +bitRead KEYWORD2 BitRead +bitWrite KEYWORD2 BitWrite +bitSet KEYWORD2 BitSet +bitClear KEYWORD2 BitClear +bit KEYWORD2 Bit +highByte KEYWORD2 HighByte +lowByte KEYWORD2 LowByte + +analogReference KEYWORD2 AnalogReference +analogRead KEYWORD2 AnalogRead +analogWrite KEYWORD2 AnalogWrite +attachInterrupt KEYWORD2 AttachInterrupt +detachInterrupt KEYWORD2 DetachInterrupt +delay KEYWORD2 Delay +delayMicroseconds KEYWORD2 DelayMicroseconds +digitalWrite KEYWORD2 DigitalWrite +digitalRead KEYWORD2 DigitalRead +interrupts KEYWORD2 +millis KEYWORD2 Millis +micros KEYWORD2 Micros +noInterrupts KEYWORD2 NoInterrupts +noTone KEYWORD2 NoTone +pinMode KEYWORD2 PinMode +pulseIn KEYWORD2 PulseIn +shiftIn KEYWORD2 ShiftIn +shiftOut KEYWORD2 ShiftOut +tone KEYWORD2 Tone + +Serial KEYWORD3 Serial +Serial1 KEYWORD3 Serial +Serial2 KEYWORD3 Serial +Serial3 KEYWORD3 Serial +begin KEYWORD2 Serial_Begin +end KEYWORD2 Serial_End +peek KEYWORD2 Serial_Peek +read KEYWORD2 Serial_Read +print KEYWORD2 Serial_Print +println KEYWORD2 Serial_Println +available KEYWORD2 Serial_Available +flush KEYWORD2 Serial_Flush +setTimeout KEYWORD2 +find KEYWORD2 +findUntil KEYWORD2 +parseInt KEYWORD2 +parseFloat KEYWORD2 +readBytes KEYWORD2 +readBytesUntil KEYWORD2 + +# USB-related keywords + +Keyboard KEYWORD3 +Mouse KEYWORD3 +press KEYWORD2 +release KEYWORD2 +releaseAll KEYWORD2 +accept KEYWORD2 +click KEYWORD2 +move KEYWORD2 +isPressed KEYWORD2 + +setup KEYWORD3 Setup +loop KEYWORD3 Loop diff --git a/libs/arduino-1.0/lib/pde.jar b/libs/arduino-1.0/lib/pde.jar new file mode 100644 index 0000000..5d88bc7 Binary files /dev/null and b/libs/arduino-1.0/lib/pde.jar differ diff --git a/libs/arduino-1.0/lib/preferences.txt b/libs/arduino-1.0/lib/preferences.txt new file mode 100644 index 0000000..67246f8 --- /dev/null +++ b/libs/arduino-1.0/lib/preferences.txt @@ -0,0 +1,262 @@ +# !!!!!!!! UNLIKE PREVIOUS VERSIONS OF PROCESSING !!!!!!!!!! +# DO NOT MODIFY THIS FILE, OR DELETE SETTINGS FROM THIS FILE + +# These are the default preferences. If you want to modify +# them directly, use the per-user local version of the file: + +# Documents and Settings -> [username] -> Application Data -> +# Processing -> preferences.txt (on Windows XP) + +# Users -> [username] -> AppData -> Roaming -> +# Processing -> preferences.txt (on Windows Vista) + +# ~/Library -> Processing -> preferences.txt (on Mac OS X) + +# ~/.processing -> preferences.txt (on Linux) + +# The exact location of your preferences file can be found at +# the bottom of the Preferences window inside Processing. + +# Because AppData and Application Data may be considered +# hidden or system folders on Windows, you'll have to ensure +# that they're visible in order to get at preferences.txt + +# You'll have problems running Processing if you incorrectly +# modify lines in this file. + + +# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + +# DEFAULT PATHS FOR SKETCHBOOK AND SETTINGS + +# relative paths will be relative to processing.exe or procesing.app. +# absolute paths may also be used. + +# note that this path should use forward slashes (like unix) +# instead of \ on windows or : on macos or whatever else + +# If you don't want users to have their sketchbook default to +# "My Documents/Processing" on Windows and "Documents/Processing" on OS X, +# set this to another path that will be used by default. +# Note that this path must exist already otherwise it won't see +# the sketchbook folder, and will instead assume the sketchbook +# has gone missing, and that it should instead use the default. +#sketchbook.path= + +# if you don't want settings to go into "application data" on windows +# and "library" on macosx, set this to the alternate location. +#settings.path=data + +# temporary build path, normally this goes into the default +# "temp" folder for that platform (as defined by java) +# but this can be used to set a specific file in case of problems +#build.path=build + +# By default, no sketches currently open +last.sketch.count=0 + + +# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + +# by default, check the processing server for any updates +# (please avoid disabling, this also helps us know basic numbers +# on how many people are using Processing) +update.check = true + +# on windows, automatically associate .pde files with processing.exe +platform.auto_file_type_associations = true + +# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + +# default size for the main window +editor.window.width.default = 500 +editor.window.height.default = 600 + +editor.window.width.min = 400 +editor.window.height.min = 290 + +# the following commented out to better support netbooks +# http://code.google.com/p/arduino/issues/detail?id=52 +#editor.window.height.min = 500 +# tested as approx 440 on OS X +#editor.window.height.min.macosx = 450 +# tested to be 515 on Windows XP, this leaves some room +#editor.window.height.min.windows = 530 + + +# font size for editor +editor.font=Monospaced,plain,12 +# Monaco is nicer on Mac OS X, so use that explicitly +editor.font.macosx = Monaco,plain,10 + +# anti-aliased text, turned off by default +editor.antialias=false + +# color to be used for background when 'external editor' enabled +editor.external=false + +# caret blinking +editor.caret.blink=true + +# area that's not in use by the text (replaced with tildes) +editor.invalid=false + +# enable ctrl-ins, shift-ins, shift-delete for cut/copy/paste +# on windows and linux, but disable on the mac +editor.keys.alternative_cut_copy_paste = true +editor.keys.alternative_cut_copy_paste.macosx = false + +# true if shift-backspace sends the delete character, +# false if shift-backspace just means backspace +editor.keys.shift_backspace_is_delete = true + +# home and end keys should only travel to the start/end of the current line +editor.keys.home_and_end_travel_far = false +# the OS X HI Guidelines say that home/end are relative to the document +# if you don't like it, this is the preference to change +editor.keys.home_and_end_travel_far.macosx = true + +console = true +console.output.file = stdout.txt +console.error.file = stderr.txt +console.lines = 4 + +# set to false to disable automatically clearing the console +# each time 'run' is hit +console.auto_clear = true + +# set the maximum number of lines remembered by the console +# the default is 500, lengthen at your own peril +console.length = 500 + +# convert tabs to spaces? how many spaces? +editor.tabs.expand = true +editor.tabs.size = 2 + +# automatically indent each line +editor.indent = true + +# size of divider between editing area and the console +editor.divider.size = 0 +# the larger divider on windows is ugly with the little arrows +# this makes it large enough to see (mouse changes) and use, +# but keeps it from being annoyingly obtrusive +editor.divider.size.windows = 2 + +# any additional java options when running externally +# (for applets that are run external to the environment... +# those with a code folder, or using any libraries) +# if you hose this and can't run things, it's your own durn fault +run.options = + +# settings for the -XmsNNNm and -XmxNNNm command line option +run.options.memory = false +run.options.memory.initial = 64 +run.options.memory.maximum = 256 + +# example of increasing the memory size for applets run externally +#run.options = -Xms128m -Xmx1024m + +# index of the default display to use for present mode +# (this setting not yet completely implemented) +run.display = 1 + +# set internally +#run.window.bgcolor= + +# set to false to open a new untitled window when closing the last window +# (otherwise, the environment will quit) +# default to the relative norm for the different platforms, +# but the setting can be changed in the prefs dialog anyway +#sketchbook.closing_last_window_quits = true +#sketchbook.closing_last_window_quits.macosx = false + + +# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + +#history.recording = true + +# for advanced users, enable option to export a library +#export.library = false + +# which platforms to export by default +export.application.platform.windows = true +export.application.platform.macosx = true +export.application.platform.linux = true + +# whether or not to export as full screen (present) mode +export.application.fullscreen = false + +# whether to show the stop button when exporting to application +export.application.stop = true + +# false will place all exported files into a single .jar +export.applet.separate_jar_files = false + +# set to false to no longer delete applet or application folders before export +export.delete_target_folder = true + +# may be useful when attempting to debug the preprocessor +preproc.save_build_files=false + +# allows various preprocessor features to be toggled +# in case they are causing problems + +# preprocessor: pde.g +preproc.color_datatype = true +preproc.web_colors = true +preproc.enhanced_casting = true + +# preprocessor: PdeEmitter.java +preproc.substitute_floats = true +#preproc.substitute_image = false +#preproc.substitute_font = false + +# auto-convert non-ascii chars to unicode escape sequences +preproc.substitute_unicode = true + +# PdePreproc.java +# writes out the parse tree as parseTree.xml, which can be usefully +# viewed in (at least) Mozilla or IE. useful when debugging the preprocessor. +preproc.output_parse_tree = false + +# Changed after 1.0.9 to a new name, and also includes the specific entries +preproc.imports.list = java.applet.*,java.awt.Dimension,java.awt.Frame,java.awt.event.MouseEvent,java.awt.event.KeyEvent,java.awt.event.FocusEvent,java.awt.Image,java.io.*,java.net.*,java.text.*,java.util.*,java.util.zip.*,java.util.regex.* + +# set the browser to be used on linux +browser.linux = mozilla + +# set to the program to be used for launching apps on linux +#launcher.linux = xdg-open + +# FULL SCREEN (PRESENT MODE) +run.present.bgcolor = #666666 +run.present.stop.color = #cccccc +# starting in release 0159, don't use full screen exclusive anymore +run.present.exclusive = false +# use this by default to hide the menu bar and dock on osx +run.present.exclusive.macosx = true + +# ARDUINO PREFERENCES +board = uno +target = arduino + +programmer = arduino:avrispmkii + +upload.using = bootloader +upload.verify = true + +serial.port=COM1 +serial.databits=8 +serial.stopbits=1 +serial.parity=N +serial.debug_rate=9600 + +# I18 Preferences + +# default chosen language (none for none) +editor.languages.current = \ No newline at end of file diff --git a/libs/arduino-1.0/lib/theme/buttons.gif b/libs/arduino-1.0/lib/theme/buttons.gif new file mode 100644 index 0000000..4de0905 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/buttons.gif differ diff --git a/libs/arduino-1.0/lib/theme/resize.gif b/libs/arduino-1.0/lib/theme/resize.gif new file mode 100644 index 0000000..ed31c0a Binary files /dev/null and b/libs/arduino-1.0/lib/theme/resize.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-sel-left.gif b/libs/arduino-1.0/lib/theme/tab-sel-left.gif new file mode 100644 index 0000000..252ebcc Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-sel-left.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-sel-menu.gif b/libs/arduino-1.0/lib/theme/tab-sel-menu.gif new file mode 100644 index 0000000..3b213f4 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-sel-menu.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-sel-mid.gif b/libs/arduino-1.0/lib/theme/tab-sel-mid.gif new file mode 100644 index 0000000..4bd19a0 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-sel-mid.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-sel-right.gif b/libs/arduino-1.0/lib/theme/tab-sel-right.gif new file mode 100644 index 0000000..4ceb3ed Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-sel-right.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-unsel-left.gif b/libs/arduino-1.0/lib/theme/tab-unsel-left.gif new file mode 100644 index 0000000..cdc9886 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-unsel-left.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-unsel-menu.gif b/libs/arduino-1.0/lib/theme/tab-unsel-menu.gif new file mode 100644 index 0000000..3c91771 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-unsel-menu.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-unsel-mid.gif b/libs/arduino-1.0/lib/theme/tab-unsel-mid.gif new file mode 100644 index 0000000..c538ad2 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-unsel-mid.gif differ diff --git a/libs/arduino-1.0/lib/theme/tab-unsel-right.gif b/libs/arduino-1.0/lib/theme/tab-unsel-right.gif new file mode 100644 index 0000000..3150eb5 Binary files /dev/null and b/libs/arduino-1.0/lib/theme/tab-unsel-right.gif differ diff --git a/libs/arduino-1.0/lib/theme/theme.txt b/libs/arduino-1.0/lib/theme/theme.txt new file mode 100644 index 0000000..98a8ef7 --- /dev/null +++ b/libs/arduino-1.0/lib/theme/theme.txt @@ -0,0 +1,104 @@ +# GUI - STATUS +status.notice.fgcolor = #002325 +status.notice.bgcolor = #17A1A5 +status.error.fgcolor = #FFFFFF +status.error.bgcolor = #E34C00 +status.edit.fgcolor = #000000 +status.edit.bgcolor = #F1B500 +status.font = SansSerif,plain,12 + +# GUI - TABS +# settings for the tabs at the top +# (tab images are stored in the lib/theme folder) +header.bgcolor = #17A1A5 +header.text.selected.color = #005B5B +header.text.unselected.color = #007e82 +header.text.font = SansSerif,plain,12 + +# GUI - CONSOLE +console.font = Monospaced,plain,11 +console.font.macosx = Monaco,plain,10 +console.color = #000000 +console.output.color = #eeeeee +console.error.color = #E34C00 + +# GUI - BUTTONS +buttons.bgcolor = #006468 +buttons.status.font = SansSerif,plain,12 +buttons.status.color = #ffffff + +# GUI - LINESTATUS +linestatus.color = #ffffff +linestatus.bgcolor = #006468 + +# EDITOR - DETAILS + +# foreground and background colors +editor.fgcolor = #000000 +editor.bgcolor = #ffffff + +# highlight for the current line +editor.linehighlight.color=#e2e2e2 +# highlight for the current line +editor.linehighlight=true + +# caret blinking and caret color +editor.caret.color = #333300 + +# color to be used for background when 'external editor' enabled +editor.external.bgcolor = #c8d2dc + +# selection color +editor.selection.color = #ffcc00 + +# area that's not in use by the text (replaced with tildes) +editor.invalid.style = #7e7e7e,bold + +# little pooties at the end of lines that show where they finish +editor.eolmarkers = false +editor.eolmarkers.color = #999999 + +# bracket/brace highlighting +editor.brackethighlight = true +editor.brackethighlight.color = #006699 + + +# TEXT - KEYWORDS + +# e.g abstract, final, private +editor.keyword1.style = #cc6600,plain + +# e.g. beginShape, point, line +editor.keyword2.style = #cc6600,plain + +# e.g. byte, char, short, color +editor.keyword3.style = #cc6600,bold + + +# TEXT - LITERALS + +# constants: e.g. null, true, this, RGB, TWO_PI +editor.literal1.style = #006699,plain + +# p5 built in variables: e.g. mouseX, width, pixels +editor.literal2.style = #006699,plain + +# http://arduino.cc/ +editor.url.style = #0000ff,underlined + +# e.g. + - = / +editor.operator.style = #000000,plain + +# ?? maybe this is for words followed by a colon +# like in case statements or goto +editor.label.style = #7e7e7e,bold + + +# TEXT - COMMENTS +editor.comment1.style = #7e7e7e,plain +editor.comment2.style = #7e7e7e,plain + + +# LINE STATUS - editor line number status bar at the bottom of the screen +linestatus.font = SansSerif,plain,10 +linestatus.height = 20 diff --git a/libs/arduino-1.0/lib/version.txt b/libs/arduino-1.0/lib/version.txt new file mode 100644 index 0000000..21e8796 --- /dev/null +++ b/libs/arduino-1.0/lib/version.txt @@ -0,0 +1 @@ +1.0.3 diff --git a/libs/arduino-1.0/libraries/EEPROM/EEPROM.cpp b/libs/arduino-1.0/libraries/EEPROM/EEPROM.cpp new file mode 100644 index 0000000..dfa1deb --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/EEPROM.cpp @@ -0,0 +1,50 @@ +/* + EEPROM.cpp - EEPROM library + Copyright (c) 2006 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +/****************************************************************************** + * Includes + ******************************************************************************/ + +#include +#include "Arduino.h" +#include "EEPROM.h" + +/****************************************************************************** + * Definitions + ******************************************************************************/ + +/****************************************************************************** + * Constructors + ******************************************************************************/ + +/****************************************************************************** + * User API + ******************************************************************************/ + +uint8_t EEPROMClass::read(int address) +{ + return eeprom_read_byte((unsigned char *) address); +} + +void EEPROMClass::write(int address, uint8_t value) +{ + eeprom_write_byte((unsigned char *) address, value); +} + +EEPROMClass EEPROM; diff --git a/libs/arduino-1.0/libraries/EEPROM/EEPROM.h b/libs/arduino-1.0/libraries/EEPROM/EEPROM.h new file mode 100644 index 0000000..aa2b577 --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/EEPROM.h @@ -0,0 +1,35 @@ +/* + EEPROM.h - EEPROM library + Copyright (c) 2006 David A. Mellis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef EEPROM_h +#define EEPROM_h + +#include + +class EEPROMClass +{ + public: + uint8_t read(int); + void write(int, uint8_t); +}; + +extern EEPROMClass EEPROM; + +#endif + diff --git a/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino new file mode 100644 index 0000000..d1e29bd --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_clear/eeprom_clear.ino @@ -0,0 +1,23 @@ +/* + * EEPROM Clear + * + * Sets all of the bytes of the EEPROM to 0. + * This example code is in the public domain. + + */ + +#include + +void setup() +{ + // write a 0 to all 512 bytes of the EEPROM + for (int i = 0; i < 512; i++) + EEPROM.write(i, 0); + + // turn the LED on when we're done + digitalWrite(13, HIGH); +} + +void loop() +{ +} diff --git a/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino new file mode 100644 index 0000000..0709b2d --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_read/eeprom_read.ino @@ -0,0 +1,43 @@ +/* + * EEPROM Read + * + * Reads the value of each byte of the EEPROM and prints it + * to the computer. + * This example code is in the public domain. + */ + +#include + +// start reading from the first byte (address 0) of the EEPROM +int address = 0; +byte value; + +void setup() +{ + // initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } +} + +void loop() +{ + // read a byte from the current address of the EEPROM + value = EEPROM.read(address); + + Serial.print(address); + Serial.print("\t"); + Serial.print(value, DEC); + Serial.println(); + + // advance to the next address of the EEPROM + address = address + 1; + + // there are only 512 bytes of EEPROM, from 0 to 511, so if we're + // on address 512, wrap around to address 0 + if (address == 512) + address = 0; + + delay(500); +} diff --git a/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino new file mode 100644 index 0000000..ae7c57e --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/examples/eeprom_write/eeprom_write.ino @@ -0,0 +1,38 @@ +/* + * EEPROM Write + * + * Stores values read from analog input 0 into the EEPROM. + * These values will stay in the EEPROM when the board is + * turned off and may be retrieved later by another sketch. + */ + +#include + +// the current address in the EEPROM (i.e. which byte +// we're going to write to next) +int addr = 0; + +void setup() +{ +} + +void loop() +{ + // need to divide by 4 because analog inputs range from + // 0 to 1023 and each byte of the EEPROM can only hold a + // value from 0 to 255. + int val = analogRead(0) / 4; + + // write the value to the appropriate byte of the EEPROM. + // these values will remain there when the board is + // turned off. + EEPROM.write(addr, val); + + // advance to the next address. there are 512 bytes in + // the EEPROM, so go back to 0 when we hit 512. + addr = addr + 1; + if (addr == 512) + addr = 0; + + delay(100); +} diff --git a/libs/arduino-1.0/libraries/EEPROM/keywords.txt b/libs/arduino-1.0/libraries/EEPROM/keywords.txt new file mode 100644 index 0000000..d3218fe --- /dev/null +++ b/libs/arduino-1.0/libraries/EEPROM/keywords.txt @@ -0,0 +1,18 @@ +####################################### +# Syntax Coloring Map For Ultrasound +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +EEPROM KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/Esplora/Esplora.cpp b/libs/arduino-1.0/libraries/Esplora/Esplora.cpp new file mode 100644 index 0000000..83df0d7 --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/Esplora.cpp @@ -0,0 +1,175 @@ +/* + Esplora.cpp - Arduino Esplora board library + Written by Enrico Gueli + Copyright (c) 2012 Arduino(TM) All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + + +#include "Esplora.h" + +_Esplora Esplora; + +/* + * The following constants tell, for each accelerometer + * axis, which values are returned when the axis measures + * zero acceleration. + */ +const int ACCEL_ZERO_X = 320; +const int ACCEL_ZERO_Y = 330; +const int ACCEL_ZERO_Z = 310; + +const byte MUX_ADDR_PINS[] = { A0, A1, A2, A3 }; +const byte MUX_COM_PIN = A4; + +const int JOYSTICK_DEAD_ZONE = 100; + +const byte RED_PIN = 5; +const byte BLUE_PIN = 9; +const byte GREEN_PIN = 10; + +const byte BUZZER_PIN = 6; + +// non-multiplexer Esplora pins: +// Accelerometer: x-A5, y-A7, z-A6 +// External outputs: D3, D11 +// Buzzer: A8 +// RGB Led: red-D5, green-D10/A11, blue-D9/A10 +// Led 13: D13 + +const byte ACCEL_X_PIN = A5; +const byte ACCEL_Y_PIN = A11; +const byte ACCEL_Z_PIN = A6; + +const byte LED_PIN = 13; + +_Esplora::_Esplora() { + for (byte p=0; p<4; p++) { + pinMode(MUX_ADDR_PINS[p], OUTPUT); + } + pinMode(RED_PIN, OUTPUT); + pinMode(GREEN_PIN, OUTPUT); + pinMode(BLUE_PIN, OUTPUT); +} + +unsigned int _Esplora::readChannel(byte channel) { + digitalWrite(MUX_ADDR_PINS[0], (channel & 1) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[1], (channel & 2) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[2], (channel & 4) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[3], (channel & 8) ? HIGH : LOW); + // workaround to cope with lack of pullup resistor on joystick switch + if (channel == CH_JOYSTICK_SW) { + pinMode(MUX_COM_PIN, INPUT_PULLUP); + unsigned int joystickSwitchState = (digitalRead(MUX_COM_PIN) == HIGH) ? 1023 : 0; + digitalWrite(MUX_COM_PIN, LOW); + return joystickSwitchState; + } + else + return analogRead(MUX_COM_PIN); +} + +boolean _Esplora::joyLowHalf(byte joyCh) { + return (readChannel(joyCh) < 512 - JOYSTICK_DEAD_ZONE) + ? LOW : HIGH; +} + +boolean _Esplora::joyHighHalf(byte joyCh) { + return (readChannel(joyCh) > 512 + JOYSTICK_DEAD_ZONE) + ? LOW : HIGH; +} + +boolean _Esplora::readButton(byte ch) { + if (ch >= SWITCH_1 && ch <= SWITCH_4) { + ch--; + } + + switch(ch) { + case JOYSTICK_RIGHT: + return joyLowHalf(CH_JOYSTICK_X); + case JOYSTICK_LEFT: + return joyHighHalf(CH_JOYSTICK_X); + case JOYSTICK_UP: + return joyLowHalf(CH_JOYSTICK_Y); + case JOYSTICK_DOWN: + return joyHighHalf(CH_JOYSTICK_Y); + } + + unsigned int val = readChannel(ch); + return (val > 512) ? HIGH : LOW; +} + +void _Esplora::writeRGB(byte r, byte g, byte b) { + writeRed(r); + writeGreen(g); + writeBlue(b); +} + +#define RGB_FUNC(name, pin, lastVar) \ +void _Esplora::write##name(byte val) { \ + if (val == lastVar) \ + return; \ + analogWrite(pin, val); \ + lastVar = val; \ + delay(5); \ +} \ +\ +byte _Esplora::read##name() { \ + return lastVar; \ +} + +RGB_FUNC(Red, RED_PIN, lastRed) +RGB_FUNC(Green, GREEN_PIN, lastGreen) +RGB_FUNC(Blue, BLUE_PIN, lastBlue) + +void _Esplora::tone(unsigned int freq) { + if (freq > 0) + ::tone(BUZZER_PIN, freq); + else + ::noTone(BUZZER_PIN); +} + +void _Esplora::tone(unsigned int freq, unsigned long duration) { + if (freq > 0) + ::tone(BUZZER_PIN, freq, duration); + else + ::noTone(BUZZER_PIN); +} + +void _Esplora::noTone() { + ::noTone(BUZZER_PIN); +} + +int _Esplora::readTemperature(const byte scale) { + long rawT = readChannel(CH_TEMPERATURE); + if (scale == DEGREES_C) { + return (int)((rawT * 500 / 1024) - 50); + } + else if (scale == DEGREES_F) { + return (int)((rawT * 450 / 512 ) - 58); + } + else { + return readTemperature(DEGREES_C); + } +} + +int _Esplora::readAccelerometer(const byte axis) { + switch (axis) { + case X_AXIS: return analogRead(ACCEL_X_PIN) - ACCEL_ZERO_X; + case Y_AXIS: return analogRead(ACCEL_Y_PIN) - ACCEL_ZERO_Y; + case Z_AXIS: return analogRead(ACCEL_Z_PIN) - ACCEL_ZERO_Z; + default: return 0; + } +} diff --git a/libs/arduino-1.0/libraries/Esplora/Esplora.h b/libs/arduino-1.0/libraries/Esplora/Esplora.h new file mode 100644 index 0000000..74fa88b --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/Esplora.h @@ -0,0 +1,163 @@ +/* + Esplora.h - Arduino Esplora board library + Written by Enrico Gueli + Copyright (c) 2012 Arduino(TM) All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef ESPLORA_H_ +#define ESPLORA_H_ + +#include "Arduino.h" + +/* + * The following constants are used internally by the Esplora + * library code. + */ + +const byte JOYSTICK_BASE = 16; // it's a "virtual" channel: its ID won't conflict with real ones + +const byte MAX_CHANNELS = 13; + +const byte CH_SWITCH_1 = 0; +const byte CH_SWITCH_2 = 1; +const byte CH_SWITCH_3 = 2; +const byte CH_SWITCH_4 = 3; +const byte CH_SLIDER = 4; +const byte CH_LIGHT = 5; +const byte CH_TEMPERATURE = 6; +const byte CH_MIC = 7; +const byte CH_JOYSTICK_SW = 10; +const byte CH_JOYSTICK_X = 11; +const byte CH_JOYSTICK_Y = 12; + +/* + * The following constants can be used with the readButton() + * method. + */ + +const byte SWITCH_1 = 1; +const byte SWITCH_2 = 2; +const byte SWITCH_3 = 3; +const byte SWITCH_4 = 4; + +const byte SWITCH_DOWN = SWITCH_1; +const byte SWITCH_LEFT = SWITCH_2; +const byte SWITCH_UP = SWITCH_3; +const byte SWITCH_RIGHT = SWITCH_4; + +const byte JOYSTICK_DOWN = JOYSTICK_BASE; +const byte JOYSTICK_LEFT = JOYSTICK_BASE+1; +const byte JOYSTICK_UP = JOYSTICK_BASE+2; +const byte JOYSTICK_RIGHT = JOYSTICK_BASE+3; + +/* + * These constants can be use for comparison with the value returned + * by the readButton() method. + */ +const boolean PRESSED = LOW; +const boolean RELEASED = HIGH; + +/* + * The following constants can be used with the readTemperature() + * method to specify the desired scale. + */ +const byte DEGREES_C = 0; +const byte DEGREES_F = 1; + +/* + * The following constants can be used with the readAccelerometer() + * method to specify the desired axis to return. + */ +const byte X_AXIS = 0; +const byte Y_AXIS = 1; +const byte Z_AXIS = 2; + + +class _Esplora { +private: + byte lastRed; + byte lastGreen; + byte lastBlue; + + unsigned int readChannel(byte channel); + + boolean joyLowHalf(byte joyCh); + boolean joyHighHalf(byte joyCh); + +public: + _Esplora(); + + /* + * Returns a number corresponding to the position of the + * linear potentiometer. 0 means full right, 1023 means + * full left. + */ + inline unsigned int readSlider() { return readChannel(CH_SLIDER); } + + /* + * Returns a number corresponding to the amount of ambient + * light sensed by the light sensor. + */ + inline unsigned int readLightSensor() { return readChannel(CH_LIGHT); } + + /* + * Returns the current ambient temperature, expressed either in Celsius + * or Fahreneit scale. + */ + int readTemperature(const byte scale); + + /* + * Returns a number corresponding to the amount of ambient noise. + */ + inline unsigned int readMicrophone() { return readChannel(CH_MIC); } + + inline unsigned int readJoystickSwitch() { return readChannel(CH_JOYSTICK_SW); } + + inline int readJoystickX() { + return readChannel(CH_JOYSTICK_X) - 512; + } + inline int readJoystickY() { + return readChannel(CH_JOYSTICK_Y) - 512; + } + + int readAccelerometer(const byte axis); + + /* + * Reads the current state of a button. It will return + * LOW if the button is pressed, and HIGH otherwise. + */ + boolean readButton(byte channel); + + void writeRGB(byte red, byte green, byte blue); + void writeRed(byte red); + void writeGreen(byte green); + void writeBlue(byte blue); + + byte readRed(); + byte readGreen(); + byte readBlue(); + + void tone(unsigned int freq); + void tone(unsigned int freq, unsigned long duration); + void noTone(); +}; + + + +extern _Esplora Esplora; + +#endif // ESPLORA_H_ diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraKart/EsploraKart.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraKart/EsploraKart.ino new file mode 100644 index 0000000..4c1621c --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraKart/EsploraKart.ino @@ -0,0 +1,125 @@ +/* + Esplora Kart + + This sketch turns the Esplora into a PC game pad. + + It uses the both the analog joystick and the four switches. + By moving the joystick in a direction or by pressing a switch, + the PC will "see" that a key is pressed. If the PC is running + a game that has keyboard input, the Esplora can control it. + + The default configuration is suitable for SuperTuxKart, an + open-source racing game. It can be downloaded from + http://supertuxkart.sourceforge.net/ . + + Created on 22 november 2012 + By Enrico Gueli +*/ + + +#include + +/* + You're going to handle eight different buttons. You'll use arrays, + which are ordered lists of variables with a fixed size. Each array + has an index (counting from 0) to keep track of the position + you're reading in the array, and each position can contain a number. + + This code uses three different arrays: one for the buttons you'll read; + a second to hold the current states of those buttons; and a third to hold + the keystrokes associated with each button. + */ + +/* + This array holds the last sensed state of each of the buttons + you're reading. + Later in the code, you'll read the button states, and compare them + to the previous states that are stored in this array. If the two + states are different, it means that the button was either + pressed or released. + */ +boolean buttonStates[8]; + +/* + This array holds the names of the buttons being read. + Later in the sketch, you'll use these names with + the method Esplora.readButton(x), where x + is one of these buttons. + */ +const byte buttons[] = { + JOYSTICK_DOWN, + JOYSTICK_LEFT, + JOYSTICK_UP, + JOYSTICK_RIGHT, + SWITCH_RIGHT, // fire + SWITCH_LEFT, // bend + SWITCH_UP, // nitro + SWITCH_DOWN, // look back +}; + +/* + This array tells what keystroke to send to the PC when a + button is pressed. + If you look at this array and the above one, you can see that + the "cursor down" keystroke is sent when the joystick is moved + down, the "cursor up" keystroke when the joystick is moved up + and so on. +*/ +const char keystrokes[] = { + KEY_DOWN_ARROW, + KEY_LEFT_ARROW, + KEY_UP_ARROW, + KEY_RIGHT_ARROW, + ' ', + 'V', + 'N', + 'B' +}; + +/* + This is code is run only at startup, to initialize the + virtual USB keyboard. +*/ +void setup() { + Keyboard.begin(); +} + +/* + After setup() is finished, this code is run continuously. + Here we continuously check if something happened with the + buttons. +*/ +void loop() { + + // Iterate through all the buttons: + for (byte thisButton=0; thisButton<8; thisButton++) { + boolean lastState = buttonStates[thisButton]; + boolean newState = Esplora.readButton(buttons[thisButton]); + if (lastState != newState) { // Something changed! + /* + The Keyboard library allows you to "press" and "release" the + keys as two distinct actions. These actions can be + linked to the buttons we're handling. + */ + if (newState == PRESSED) { + Keyboard.press(keystrokes[thisButton]); + } + else if (newState == RELEASED) { + Keyboard.release(keystrokes[thisButton]); + } + } + + // Store the new button state, so you can sense a difference later: + buttonStates[thisButton] = newState; + } + + /* + Wait a little bit (50ms) between a check and another. + When a mechanical switch is pressed or released, the + contacts may bounce very rapidly. If the check is done too + fast, these bounces may be confused as multiple presses and + may lead to unexpected behaviour. + */ + delay(50); +} + diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow/EsploraLedShow.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow/EsploraLedShow.ino new file mode 100644 index 0000000..84f049a --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow/EsploraLedShow.ino @@ -0,0 +1,42 @@ +/* + Esplora LED Show + + Makes the RGB LED bright and glow as the joystick or the + slider are moved. + + Created on 22 november 2012 + By Enrico Gueli + Modified 24 Nov 2012 + by Tom Igoe +*/ +#include + +void setup() { + // initialize the serial communication: + Serial.begin(9600); +} + +void loop() { + // read the sensors into variables: + int xAxis = Esplora.readJoystickX(); + int yAxis = Esplora.readJoystickY(); + int slider = Esplora.readSlider(); + + // convert the sensor readings to light levels: + byte red = map(xAxis, -512, 512, 0, 255); + byte green = map(xAxis, -512, 512, 0, 255); + byte blue = slider/4; + + // print the light levels: + Serial.print(red); + Serial.print(' '); + Serial.print(green); + Serial.print(' '); + Serial.println(blue); + + // write the light levels to the LED. + Esplora.writeRGB(red, green, blue); + + // add a delay to keep the LED from flickering: + delay(10); +} diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow2/EsploraLedShow2.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow2/EsploraLedShow2.ino new file mode 100644 index 0000000..8f9f8a2 --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraLedShow2/EsploraLedShow2.ino @@ -0,0 +1,55 @@ +/* + Esplora Led/Microphone + + This simple sketch reads the microphone, light sensor, and slider. + Then it uses those readings to set the brightness of red, green and blue + channels of the RGB LED. The red channel will change with the loudness + "heared" by the microphone, the green channel changes as the + amount of light in the room and the blue channel will change + with the position of the slider. + + Created on 22 november 2012 + By Enrico Gueli + Modified 24 Nov 2012 + by Tom Igoe +*/ + +#include + +void setup() { + // initialize the serial communication: + Serial.begin(9600); +} + +int lowLight = 400; // the light sensor reading when it's covered +int highLight = 1023; // the maximum light sensor reading +int minGreen = 0; // minimum brightness of the green LED +int maxGreen = 100; // maximum brightness of the green LED + +void loop() { + // read the sensors into variables: + int mic = Esplora.readMicrophone(); + int light = Esplora.readLightSensor(); + int slider = Esplora.readSlider(); + + // convert the sensor readings to light levels: + byte red = constrain(mic, 0, 255); + byte green = constrain( + map(light, lowLight, highLight, minGreen, maxGreen), + 0, 255); + byte blue = slider/4; + + // print the light levels (to see what's going on): + Serial.print(red); + Serial.print(' '); + Serial.print(green); + Serial.print(' '); + Serial.println(blue); + + // write the light levels to the LED. + // note that the green value is always 0: + Esplora.writeRGB(red, green, blue); + + // add a delay to keep the LED from flickering: + delay(10); +} diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraMusic/EsploraMusic.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraMusic/EsploraMusic.ino new file mode 100644 index 0000000..10c17f7 --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraMusic/EsploraMusic.ino @@ -0,0 +1,52 @@ +/* + Esplora Music + + This sketch turns the Esplora in a simple musical instrument. + Press the Switch 1 and move the slider to see how it works. + + Created on 22 november 2012 + By Enrico Gueli + modified 24 Nov 2012 + by Tom Igoe +*/ + + +#include + + +const int note[] = { +262, // C +277, // C# +294, // D +311, // D# +330, // E +349, // F +370, // F# +392, // G +415, // G# +440, // A +466, // A# +494, // B +523 // C next octave +}; + +void setup() { +} + +void loop() { + // read the button labeled SWITCH_DOWN. If it's low, + // then play a note: + if (Esplora.readButton(SWITCH_DOWN) == LOW) { + int slider = Esplora.readSlider(); + + // use map() to map the slider's range to the + // range of notes you have: + byte thisNote = map(slider, 0, 1023, 0, 13); + // play the note corresponding to the slider's position: + Esplora.tone(note[thisNote]); + } + else { + // if the button isn't pressed, turn the note off: + Esplora.noTone(); + } +} diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraRemote/EsploraRemote.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraRemote/EsploraRemote.ino new file mode 100644 index 0000000..135b26a --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraRemote/EsploraRemote.ino @@ -0,0 +1,94 @@ +/* + Esplora Slave + + This sketch allows to test all the Esplora's peripherals. + It is also used with the ProcessingStart sketch (for Processing). + + When uploaded, you can open the Serial monitor and write one of + the following commands (without quotes) to get an answer: + + "D": prints the current value of all sensors, separated by a comma. + See the dumpInputs() function below to get the meaning of + each value. + + "Rxxx" + "Gxxx" + "Bxxx": set the color of the RGB led. For example, write "R255" + to turn on the red to full brightness, "G128" to turn + the green to half brightness, or "G0" to turn off + the green channel. + + "Txxxx": play a tone with the buzzer. The number is the + frequency, e.g. "T440" plays the central A note. + Write "T0" to turn off the buzzer. + + + Created on 22 november 2012 + By Enrico Gueli +*/ + +#include + +void setup() { + while(!Serial); // needed for Leonardo-based board like Esplora + Serial.begin(9600); +} + +void loop() { + if (Serial.available()) + parseCommand(); +} + +/* + * This function reads a character from the serial line and + * decide what to do next. The "what to do" part is given by + * function it calls (e.g. dumpInputs(), setRed() and so on). + */ +void parseCommand() { + char cmd = Serial.read(); + switch(cmd) { + case 'D': dumpInputs(); break; + case 'R': setRed(); break; + case 'G': setGreen(); break; + case 'B': setBlue(); break; + case 'T': setTone(); break; + } +} + +void dumpInputs() { + /* + * please note: a single row contains two instructions. + * one is to print the sensor value, the other to print the + * comma symbol. + */ + Serial.print(Esplora.readButton(SWITCH_1)); Serial.print(','); + Serial.print(Esplora.readButton(SWITCH_2)); Serial.print(','); + Serial.print(Esplora.readButton(SWITCH_3)); Serial.print(','); + Serial.print(Esplora.readButton(SWITCH_4)); Serial.print(','); + Serial.print(Esplora.readSlider()); Serial.print(','); + Serial.print(Esplora.readLightSensor()); Serial.print(','); + Serial.print(Esplora.readTemperature(DEGREES_C)); Serial.print(','); + Serial.print(Esplora.readMicrophone()); Serial.print(','); + Serial.print(Esplora.readJoystickSwitch()); Serial.print(','); + Serial.print(Esplora.readJoystickX()); Serial.print(','); + Serial.print(Esplora.readJoystickY()); Serial.print(','); + Serial.print(Esplora.readAccelerometer(X_AXIS)); Serial.print(','); + Serial.print(Esplora.readAccelerometer(Y_AXIS)); Serial.print(','); + Serial.print(Esplora.readAccelerometer(Z_AXIS)); Serial.println(); +} + +void setRed() { + Esplora.writeRed(Serial.parseInt()); +} + +void setGreen() { + Esplora.writeGreen(Serial.parseInt()); +} + +void setBlue() { + Esplora.writeBlue(Serial.parseInt()); +} + +void setTone() { + Esplora.tone(Serial.parseInt()); +} diff --git a/libs/arduino-1.0/libraries/Esplora/examples/EsploraTable/EsploraTable.ino b/libs/arduino-1.0/libraries/Esplora/examples/EsploraTable/EsploraTable.ino new file mode 100644 index 0000000..73d5652 --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/examples/EsploraTable/EsploraTable.ino @@ -0,0 +1,219 @@ +/* + Esplora Table + + Acts like a keyboard that prints some of its sensors' + data in a table-like text, row by row. + It is a sort of "data-logger". + + At startup, it does nothing. It just waits for you to open a + spreadsheet (e.g. Google Drive spreadsheet) so it can put its + data. Then, by pressing Switch 1, it starts printing the table + headers and the first row of data. It waits a bit, then it + will print another row, and so on. + + The amount of time between each row is given by the slider. + If put to full left, the sketch will wait 10 seconds; at + full right position, it will wait 5 minutes. An intermediate + position will make the sketch wait for some time in-between. + + Clicking the Switch 1 at any time will stop the logging. + + The color LED shows what the sketch is doing: + blue = idle, waiting for you to press Switch 1 to start logging + green = active; will print soon + red = printing data to the PC + + Created on 22 november 2012 + By Enrico Gueli + modified 24 Nov 2012 + by Tom Igoe +*/ + +#include + +/* + * this variable tells if the data-logging is currently active. + */ +boolean active = false; + +/* + * this variable holds the time in the future when the sketch + * will "sample" the data (sampling is the act of reading some + * input at a known time). This variable is checked continuously + * against millis() to know when it's time to sample. + */ +unsigned long nextSampleAt = 0; + +/* + * This variable just holds the millis() value at the time the + * logging was activated. This is needed to enter the correct + * value in the "Time" column in the printed table. + */ +unsigned long startedAt = 0; + + +/* + * when the "active" variable is set to true, the same is done + * with this variable. This is needed because the code that does + * the "just-after-activation" stuff is run some time later than + * the code that says "be active now". + */ +boolean justActivated = false; + + +/* + * this variable holds the last sensed status of the switch press + * button. If the code sees a difference between the value of + * this variable and the current status of the switch, it means + * that the button was either pressed or released. + */ +boolean lastStartBtn = HIGH; + +/* + * Initialization code. The virtual USB keyboard must be + * initialized; the Serial class is needed just for debugging. + */ +void setup() { + Keyboard.begin(); + Serial.begin(9600); +} + +/* + * This code is run continuously. + */ +void loop() { + /* + * note: we don't use Arduino's delay() here, because we can't + * normally do anything while delaying. Our own version lets us + * check for button presses often enough to not miss any event. + */ + activeDelay(50); + + /* + * the justActivated variable may be set to true in the + * checkSwitchPress() function. Here we check its status to + * print the table headers and configure what's needed to. + */ + if (justActivated == true) { + justActivated = false; // do this just once + printHeaders(); + // do next sampling ASAP + nextSampleAt = startedAt = millis(); + } + + if (active == true) { + if (nextSampleAt < millis()) { + // it's time to sample! + int slider = Esplora.readSlider(); + // the row below maps the slider position to a range between + // 10 and 290 seconds. + int sampleInterval = map(slider, 0, 1023, 10, 290); + nextSampleAt = millis() + sampleInterval * 1000; + + logAndPrint(); + } + + // let the RGB led blink green once per second, for 200ms. + unsigned int ms = millis() % 1000; + if (ms < 200) + Esplora.writeGreen(50); + else + Esplora.writeGreen(0); + + Esplora.writeBlue(0); + } + else + // while not active, keep a reassuring blue color coming + // from the Esplora... + Esplora.writeBlue(20); + +} + +/* + * Print the table headers. + */ +void printHeaders() { + Keyboard.print("Time"); + Keyboard.write(KEY_TAB); + activeDelay(300); // Some spreadsheets are slow, e.g. Google + // Drive that wants to save every edit. + Keyboard.print("Accel X"); + Keyboard.write(KEY_TAB); + activeDelay(300); + Keyboard.print("Accel Y"); + Keyboard.write(KEY_TAB); + activeDelay(300); + Keyboard.print("Accel Z"); + Keyboard.println(); + activeDelay(300); +} + +void logAndPrint() { + // do all the samplings at once, because keystrokes have delays + unsigned long timeSecs = (millis() - startedAt) /1000; + int xAxis = Esplora.readAccelerometer(X_AXIS); + int yAxis = Esplora.readAccelerometer(Y_AXIS); + int zAxis = Esplora.readAccelerometer(Z_AXIS); + + Esplora.writeRed(100); + + Keyboard.print(timeSecs); + Keyboard.write(KEY_TAB); + activeDelay(300); + Keyboard.print(xAxis); + Keyboard.write(KEY_TAB); + activeDelay(300); + Keyboard.print(yAxis); + Keyboard.write(KEY_TAB); + activeDelay(300); + Keyboard.print(zAxis); + Keyboard.println(); + activeDelay(300); + Keyboard.write(KEY_HOME); + + Esplora.writeRed(0); +} + +/** + * Similar to delay(), but allows to do something else + * in the meanwhile. In particular, it calls waitLoop(). + * Note 1: it may wait longer than the specified amount, not less; + * Note 2: beware of data synchronization issues, e.g. if the + * whileWaiting() function alters some variables used by the + * caller of this function. + * + * I discovered by chance that there's an ongoing discussion about + * adding yield() in the Arduino API: + * http://comments.gmane.org/gmane.comp.hardware.arduino.devel/1381 + * The purpose is the same, but for now I'm using this implementation. + */ +void activeDelay(unsigned long amount) { + unsigned long at = millis() + amount; + while (millis() < at) { + checkSwitchPress(); + } +} + +/* + * This function reads the status of the switch; if it sees that + * it was pressed, toggles the status of the "active" variable. + * If it's set to true, also the justActivated variable is set to + * true, so the loop() function above can do the right things. + * This function should be called as often as possible and do as + * little as possible, because it can be called while another + * function is running. + */ +void checkSwitchPress() { + boolean startBtn = Esplora.readButton(SWITCH_DOWN); + + if (startBtn != lastStartBtn) { + if (startBtn == HIGH) { // button released + active = !active; + if (active) + justActivated = true; + } + + lastStartBtn = startBtn; + } +} + diff --git a/libs/arduino-1.0/libraries/Esplora/keywords.txt b/libs/arduino-1.0/libraries/Esplora/keywords.txt new file mode 100644 index 0000000..02ba660 --- /dev/null +++ b/libs/arduino-1.0/libraries/Esplora/keywords.txt @@ -0,0 +1,68 @@ +####################################### +# Syntax Coloring Map For Esplora +####################################### +# Class +####################################### + +Esplora KEYWORD3 + +####################################### +# Methods and Functions +####################################### + +begin KEYWORD2 +readSlider KEYWORD2 +readLightSensor KEYWORD2 +readTemperature KEYWORD2 +readMicrophone KEYWORD2 +readJoystickSwitch KEYWORD2 +readJoystickX KEYWORD2 +readJoystickY KEYWORD2 +readAccelerometer KEYWORD2 +readButton KEYWORD2 +writeRGB KEYWORD2 +writeRed KEYWORD2 +writeGreen KEYWORD2 +writeBlue KEYWORD2 +readRed KEYWORD2 +readGreen KEYWORD2 +readBlue KEYWORD2 +tone KEYWORD2 +noTone KEYWORD2 + + +####################################### +# Constants +####################################### + +JOYSTICK_BASE LITERAL1 +MAX_CHANNELS LITERAL1 +CH_SWITCH_1 LITERAL1 +CH_SWITCH_2 LITERAL1 +CH_SWITCH_3 LITERAL1 +CH_SWITCH_4 LITERAL1 +CH_SLIDER LITERAL1 +CH_LIGHT LITERAL1 +CH_TEMPERATURE LITERAL1 +CH_MIC LITERAL1 +CH_JOYSTICK_SW LITERAL1 +CH_JOYSTICK_X LITERAL1 +CH_JOYSTICK_Y LITERAL1 +SWITCH_1 LITERAL1 +SWITCH_2 LITERAL1 +SWITCH_3 LITERAL1 +SWITCH_4 LITERAL1 +SWITCH_DOWN LITERAL1 +SWITCH_LEFT LITERAL1 +SWITCH_UP LITERAL1 +SWITCH_RIGHT LITERAL1 +JOYSTICK_DOWN LITERAL1 +JOYSTICK_LEFT LITERAL1 +JOYSTICK_UP LITERAL1 +PRESSED LITERAL1 +RELEASED LITERAL1 +DEGREES_C LITERAL1 +DEGREES_F LITERAL1 +X_AXIS LITERAL1 +Y_AXIS LITERAL1 +Z_AXIS LITERAL1 diff --git a/libs/arduino-1.0/libraries/Ethernet/Dhcp.cpp b/libs/arduino-1.0/libraries/Ethernet/Dhcp.cpp new file mode 100644 index 0000000..33522f7 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Dhcp.cpp @@ -0,0 +1,479 @@ +// DHCP Library v0.3 - April 25, 2009 +// Author: Jordan Terrell - blog.jordanterrell.com + +#include "w5100.h" + +#include +#include +#include "Dhcp.h" +#include "Arduino.h" +#include "util.h" + +int DhcpClass::beginWithDHCP(uint8_t *mac, unsigned long timeout, unsigned long responseTimeout) +{ + _dhcpLeaseTime=0; + _dhcpT1=0; + _dhcpT2=0; + _lastCheck=0; + _timeout = timeout; + _responseTimeout = responseTimeout; + + // zero out _dhcpMacAddr + memset(_dhcpMacAddr, 0, 6); + reset_DHCP_lease(); + + memcpy((void*)_dhcpMacAddr, (void*)mac, 6); + _dhcp_state = STATE_DHCP_START; + return request_DHCP_lease(); +} + +void DhcpClass::reset_DHCP_lease(){ + // zero out _dhcpSubnetMask, _dhcpGatewayIp, _dhcpLocalIp, _dhcpDhcpServerIp, _dhcpDnsServerIp + memset(_dhcpLocalIp, 0, 20); +} + +//return:0 on error, 1 if request is sent and response is received +int DhcpClass::request_DHCP_lease(){ + + uint8_t messageType = 0; + + + + // Pick an initial transaction ID + _dhcpTransactionId = random(1UL, 2000UL); + _dhcpInitialTransactionId = _dhcpTransactionId; + + if (_dhcpUdpSocket.begin(DHCP_CLIENT_PORT) == 0) + { + // Couldn't get a socket + return 0; + } + + presend_DHCP(); + + int result = 0; + + unsigned long startTime = millis(); + + while(_dhcp_state != STATE_DHCP_LEASED) + { + if(_dhcp_state == STATE_DHCP_START) + { + _dhcpTransactionId++; + + send_DHCP_MESSAGE(DHCP_DISCOVER, ((millis() - startTime) / 1000)); + _dhcp_state = STATE_DHCP_DISCOVER; + } + else if(_dhcp_state == STATE_DHCP_REREQUEST){ + _dhcpTransactionId++; + send_DHCP_MESSAGE(DHCP_REQUEST, ((millis() - startTime)/1000)); + _dhcp_state = STATE_DHCP_REQUEST; + } + else if(_dhcp_state == STATE_DHCP_DISCOVER) + { + uint32_t respId; + messageType = parseDHCPResponse(_responseTimeout, respId); + if(messageType == DHCP_OFFER) + { + // We'll use the transaction ID that the offer came with, + // rather than the one we were up to + _dhcpTransactionId = respId; + send_DHCP_MESSAGE(DHCP_REQUEST, ((millis() - startTime) / 1000)); + _dhcp_state = STATE_DHCP_REQUEST; + } + } + else if(_dhcp_state == STATE_DHCP_REQUEST) + { + uint32_t respId; + messageType = parseDHCPResponse(_responseTimeout, respId); + if(messageType == DHCP_ACK) + { + _dhcp_state = STATE_DHCP_LEASED; + result = 1; + //use default lease time if we didn't get it + if(_dhcpLeaseTime == 0){ + _dhcpLeaseTime = DEFAULT_LEASE; + } + //calculate T1 & T2 if we didn't get it + if(_dhcpT1 == 0){ + //T1 should be 50% of _dhcpLeaseTime + _dhcpT1 = _dhcpLeaseTime >> 1; + } + if(_dhcpT2 == 0){ + //T2 should be 87.5% (7/8ths) of _dhcpLeaseTime + _dhcpT2 = _dhcpT1 << 1; + } + _renewInSec = _dhcpT1; + _rebindInSec = _dhcpT2; + } + else if(messageType == DHCP_NAK) + _dhcp_state = STATE_DHCP_START; + } + + if(messageType == 255) + { + messageType = 0; + _dhcp_state = STATE_DHCP_START; + } + + if(result != 1 && ((millis() - startTime) > _timeout)) + break; + } + + // We're done with the socket now + _dhcpUdpSocket.stop(); + _dhcpTransactionId++; + + return result; +} + +void DhcpClass::presend_DHCP() +{ +} + +void DhcpClass::send_DHCP_MESSAGE(uint8_t messageType, uint16_t secondsElapsed) +{ + uint8_t buffer[32]; + memset(buffer, 0, 32); + IPAddress dest_addr( 255, 255, 255, 255 ); // Broadcast address + + if (-1 == _dhcpUdpSocket.beginPacket(dest_addr, DHCP_SERVER_PORT)) + { + // FIXME Need to return errors + return; + } + + buffer[0] = DHCP_BOOTREQUEST; // op + buffer[1] = DHCP_HTYPE10MB; // htype + buffer[2] = DHCP_HLENETHERNET; // hlen + buffer[3] = DHCP_HOPS; // hops + + // xid + unsigned long xid = htonl(_dhcpTransactionId); + memcpy(buffer + 4, &(xid), 4); + + // 8, 9 - seconds elapsed + buffer[8] = ((secondsElapsed & 0xff00) >> 8); + buffer[9] = (secondsElapsed & 0x00ff); + + // flags + unsigned short flags = htons(DHCP_FLAGSBROADCAST); + memcpy(buffer + 10, &(flags), 2); + + // ciaddr: already zeroed + // yiaddr: already zeroed + // siaddr: already zeroed + // giaddr: already zeroed + + //put data in W5100 transmit buffer + _dhcpUdpSocket.write(buffer, 28); + + memset(buffer, 0, 32); // clear local buffer + + memcpy(buffer, _dhcpMacAddr, 6); // chaddr + + //put data in W5100 transmit buffer + _dhcpUdpSocket.write(buffer, 16); + + memset(buffer, 0, 32); // clear local buffer + + // leave zeroed out for sname && file + // put in W5100 transmit buffer x 6 (192 bytes) + + for(int i = 0; i < 6; i++) { + _dhcpUdpSocket.write(buffer, 32); + } + + // OPT - Magic Cookie + buffer[0] = (uint8_t)((MAGIC_COOKIE >> 24)& 0xFF); + buffer[1] = (uint8_t)((MAGIC_COOKIE >> 16)& 0xFF); + buffer[2] = (uint8_t)((MAGIC_COOKIE >> 8)& 0xFF); + buffer[3] = (uint8_t)(MAGIC_COOKIE& 0xFF); + + // OPT - message type + buffer[4] = dhcpMessageType; + buffer[5] = 0x01; + buffer[6] = messageType; //DHCP_REQUEST; + + // OPT - client identifier + buffer[7] = dhcpClientIdentifier; + buffer[8] = 0x07; + buffer[9] = 0x01; + memcpy(buffer + 10, _dhcpMacAddr, 6); + + // OPT - host name + buffer[16] = hostName; + buffer[17] = strlen(HOST_NAME) + 6; // length of hostname + last 3 bytes of mac address + strcpy((char*)&(buffer[18]), HOST_NAME); + + printByte((char*)&(buffer[24]), _dhcpMacAddr[3]); + printByte((char*)&(buffer[26]), _dhcpMacAddr[4]); + printByte((char*)&(buffer[28]), _dhcpMacAddr[5]); + + //put data in W5100 transmit buffer + _dhcpUdpSocket.write(buffer, 30); + + if(messageType == DHCP_REQUEST) + { + buffer[0] = dhcpRequestedIPaddr; + buffer[1] = 0x04; + buffer[2] = _dhcpLocalIp[0]; + buffer[3] = _dhcpLocalIp[1]; + buffer[4] = _dhcpLocalIp[2]; + buffer[5] = _dhcpLocalIp[3]; + + buffer[6] = dhcpServerIdentifier; + buffer[7] = 0x04; + buffer[8] = _dhcpDhcpServerIp[0]; + buffer[9] = _dhcpDhcpServerIp[1]; + buffer[10] = _dhcpDhcpServerIp[2]; + buffer[11] = _dhcpDhcpServerIp[3]; + + //put data in W5100 transmit buffer + _dhcpUdpSocket.write(buffer, 12); + } + + buffer[0] = dhcpParamRequest; + buffer[1] = 0x06; + buffer[2] = subnetMask; + buffer[3] = routersOnSubnet; + buffer[4] = dns; + buffer[5] = domainName; + buffer[6] = dhcpT1value; + buffer[7] = dhcpT2value; + buffer[8] = endOption; + + //put data in W5100 transmit buffer + _dhcpUdpSocket.write(buffer, 9); + + _dhcpUdpSocket.endPacket(); +} + +uint8_t DhcpClass::parseDHCPResponse(unsigned long responseTimeout, uint32_t& transactionId) +{ + uint8_t type = 0; + uint8_t opt_len = 0; + + unsigned long startTime = millis(); + + while(_dhcpUdpSocket.parsePacket() <= 0) + { + if((millis() - startTime) > responseTimeout) + { + return 255; + } + delay(50); + } + // start reading in the packet + RIP_MSG_FIXED fixedMsg; + _dhcpUdpSocket.read((uint8_t*)&fixedMsg, sizeof(RIP_MSG_FIXED)); + + if(fixedMsg.op == DHCP_BOOTREPLY && _dhcpUdpSocket.remotePort() == DHCP_SERVER_PORT) + { + transactionId = ntohl(fixedMsg.xid); + if(memcmp(fixedMsg.chaddr, _dhcpMacAddr, 6) != 0 || (transactionId < _dhcpInitialTransactionId) || (transactionId > _dhcpTransactionId)) + { + // Need to read the rest of the packet here regardless + _dhcpUdpSocket.flush(); + return 0; + } + + memcpy(_dhcpLocalIp, fixedMsg.yiaddr, 4); + + // Skip to the option part + // Doing this a byte at a time so we don't have to put a big buffer + // on the stack (as we don't have lots of memory lying around) + for (int i =0; i < (240 - (int)sizeof(RIP_MSG_FIXED)); i++) + { + _dhcpUdpSocket.read(); // we don't care about the returned byte + } + + while (_dhcpUdpSocket.available() > 0) + { + switch (_dhcpUdpSocket.read()) + { + case endOption : + break; + + case padOption : + break; + + case dhcpMessageType : + opt_len = _dhcpUdpSocket.read(); + type = _dhcpUdpSocket.read(); + break; + + case subnetMask : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read(_dhcpSubnetMask, 4); + break; + + case routersOnSubnet : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read(_dhcpGatewayIp, 4); + for (int i = 0; i < opt_len-4; i++) + { + _dhcpUdpSocket.read(); + } + break; + + case dns : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read(_dhcpDnsServerIp, 4); + for (int i = 0; i < opt_len-4; i++) + { + _dhcpUdpSocket.read(); + } + break; + + case dhcpServerIdentifier : + opt_len = _dhcpUdpSocket.read(); + if( *((uint32_t*)_dhcpDhcpServerIp) == 0 || + IPAddress(_dhcpDhcpServerIp) == _dhcpUdpSocket.remoteIP() ) + { + _dhcpUdpSocket.read(_dhcpDhcpServerIp, sizeof(_dhcpDhcpServerIp)); + } + else + { + // Skip over the rest of this option + while (opt_len--) + { + _dhcpUdpSocket.read(); + } + } + break; + + case dhcpT1value : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read((uint8_t*)&_dhcpT1, sizeof(_dhcpT1)); + _dhcpT1 = ntohl(_dhcpT1); + break; + + case dhcpT2value : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read((uint8_t*)&_dhcpT2, sizeof(_dhcpT2)); + _dhcpT2 = ntohl(_dhcpT2); + break; + + case dhcpIPaddrLeaseTime : + opt_len = _dhcpUdpSocket.read(); + _dhcpUdpSocket.read((uint8_t*)&_dhcpLeaseTime, sizeof(_dhcpLeaseTime)); + _dhcpLeaseTime = ntohl(_dhcpLeaseTime); + _renewInSec = _dhcpLeaseTime; + break; + + default : + opt_len = _dhcpUdpSocket.read(); + // Skip over the rest of this option + while (opt_len--) + { + _dhcpUdpSocket.read(); + } + break; + } + } + } + + // Need to skip to end of the packet regardless here + _dhcpUdpSocket.flush(); + + return type; +} + + +/* + returns: + 0/DHCP_CHECK_NONE: nothing happened + 1/DHCP_CHECK_RENEW_FAIL: renew failed + 2/DHCP_CHECK_RENEW_OK: renew success + 3/DHCP_CHECK_REBIND_FAIL: rebind fail + 4/DHCP_CHECK_REBIND_OK: rebind success +*/ +int DhcpClass::checkLease(){ + //this uses a signed / unsigned trick to deal with millis overflow + unsigned long now = millis(); + signed long snow = (long)now; + int rc=DHCP_CHECK_NONE; + if (_lastCheck != 0){ + signed long factor; + //calc how many ms past the timeout we are + factor = snow - (long)_secTimeout; + //if on or passed the timeout, reduce the counters + if ( factor >= 0 ){ + //next timeout should be now plus 1000 ms minus parts of second in factor + _secTimeout = snow + 1000 - factor % 1000; + //how many seconds late are we, minimum 1 + factor = factor / 1000 +1; + + //reduce the counters by that mouch + //if we can assume that the cycle time (factor) is fairly constant + //and if the remainder is less than cycle time * 2 + //do it early instead of late + if(_renewInSec < factor*2 ) + _renewInSec = 0; + else + _renewInSec -= factor; + + if(_rebindInSec < factor*2 ) + _rebindInSec = 0; + else + _rebindInSec -= factor; + } + + //if we have a lease but should renew, do it + if (_dhcp_state == STATE_DHCP_LEASED && _renewInSec <=0){ + _dhcp_state = STATE_DHCP_REREQUEST; + rc = 1 + request_DHCP_lease(); + } + + //if we have a lease or is renewing but should bind, do it + if( (_dhcp_state == STATE_DHCP_LEASED || _dhcp_state == STATE_DHCP_START) && _rebindInSec <=0){ + //this should basically restart completely + _dhcp_state = STATE_DHCP_START; + reset_DHCP_lease(); + rc = 3 + request_DHCP_lease(); + } + } + else{ + _secTimeout = snow + 1000; + } + + _lastCheck = now; + return rc; +} + +IPAddress DhcpClass::getLocalIp() +{ + return IPAddress(_dhcpLocalIp); +} + +IPAddress DhcpClass::getSubnetMask() +{ + return IPAddress(_dhcpSubnetMask); +} + +IPAddress DhcpClass::getGatewayIp() +{ + return IPAddress(_dhcpGatewayIp); +} + +IPAddress DhcpClass::getDhcpServerIp() +{ + return IPAddress(_dhcpDhcpServerIp); +} + +IPAddress DhcpClass::getDnsServerIp() +{ + return IPAddress(_dhcpDnsServerIp); +} + +void DhcpClass::printByte(char * buf, uint8_t n ) { + char *str = &buf[1]; + buf[0]='0'; + do { + unsigned long m = n; + n /= 16; + char c = m - 16 * n; + *str-- = c < 10 ? c + '0' : c + 'A' - 10; + } while(n); +} diff --git a/libs/arduino-1.0/libraries/Ethernet/Dhcp.h b/libs/arduino-1.0/libraries/Ethernet/Dhcp.h new file mode 100644 index 0000000..6f9c632 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Dhcp.h @@ -0,0 +1,178 @@ +// DHCP Library v0.3 - April 25, 2009 +// Author: Jordan Terrell - blog.jordanterrell.com + +#ifndef Dhcp_h +#define Dhcp_h + +#include "EthernetUdp.h" + +/* DHCP state machine. */ +#define STATE_DHCP_START 0 +#define STATE_DHCP_DISCOVER 1 +#define STATE_DHCP_REQUEST 2 +#define STATE_DHCP_LEASED 3 +#define STATE_DHCP_REREQUEST 4 +#define STATE_DHCP_RELEASE 5 + +#define DHCP_FLAGSBROADCAST 0x8000 + +/* UDP port numbers for DHCP */ +#define DHCP_SERVER_PORT 67 /* from server to client */ +#define DHCP_CLIENT_PORT 68 /* from client to server */ + +/* DHCP message OP code */ +#define DHCP_BOOTREQUEST 1 +#define DHCP_BOOTREPLY 2 + +/* DHCP message type */ +#define DHCP_DISCOVER 1 +#define DHCP_OFFER 2 +#define DHCP_REQUEST 3 +#define DHCP_DECLINE 4 +#define DHCP_ACK 5 +#define DHCP_NAK 6 +#define DHCP_RELEASE 7 +#define DHCP_INFORM 8 + +#define DHCP_HTYPE10MB 1 +#define DHCP_HTYPE100MB 2 + +#define DHCP_HLENETHERNET 6 +#define DHCP_HOPS 0 +#define DHCP_SECS 0 + +#define MAGIC_COOKIE 0x63825363 +#define MAX_DHCP_OPT 16 + +#define HOST_NAME "WIZnet" +#define DEFAULT_LEASE (900) //default lease time in seconds + +#define DHCP_CHECK_NONE (0) +#define DHCP_CHECK_RENEW_FAIL (1) +#define DHCP_CHECK_RENEW_OK (2) +#define DHCP_CHECK_REBIND_FAIL (3) +#define DHCP_CHECK_REBIND_OK (4) + +enum +{ + padOption = 0, + subnetMask = 1, + timerOffset = 2, + routersOnSubnet = 3, + /* timeServer = 4, + nameServer = 5,*/ + dns = 6, + /*logServer = 7, + cookieServer = 8, + lprServer = 9, + impressServer = 10, + resourceLocationServer = 11,*/ + hostName = 12, + /*bootFileSize = 13, + meritDumpFile = 14,*/ + domainName = 15, + /*swapServer = 16, + rootPath = 17, + extentionsPath = 18, + IPforwarding = 19, + nonLocalSourceRouting = 20, + policyFilter = 21, + maxDgramReasmSize = 22, + defaultIPTTL = 23, + pathMTUagingTimeout = 24, + pathMTUplateauTable = 25, + ifMTU = 26, + allSubnetsLocal = 27, + broadcastAddr = 28, + performMaskDiscovery = 29, + maskSupplier = 30, + performRouterDiscovery = 31, + routerSolicitationAddr = 32, + staticRoute = 33, + trailerEncapsulation = 34, + arpCacheTimeout = 35, + ethernetEncapsulation = 36, + tcpDefaultTTL = 37, + tcpKeepaliveInterval = 38, + tcpKeepaliveGarbage = 39, + nisDomainName = 40, + nisServers = 41, + ntpServers = 42, + vendorSpecificInfo = 43, + netBIOSnameServer = 44, + netBIOSdgramDistServer = 45, + netBIOSnodeType = 46, + netBIOSscope = 47, + xFontServer = 48, + xDisplayManager = 49,*/ + dhcpRequestedIPaddr = 50, + dhcpIPaddrLeaseTime = 51, + /*dhcpOptionOverload = 52,*/ + dhcpMessageType = 53, + dhcpServerIdentifier = 54, + dhcpParamRequest = 55, + /*dhcpMsg = 56, + dhcpMaxMsgSize = 57,*/ + dhcpT1value = 58, + dhcpT2value = 59, + /*dhcpClassIdentifier = 60,*/ + dhcpClientIdentifier = 61, + endOption = 255 +}; + +typedef struct _RIP_MSG_FIXED +{ + uint8_t op; + uint8_t htype; + uint8_t hlen; + uint8_t hops; + uint32_t xid; + uint16_t secs; + uint16_t flags; + uint8_t ciaddr[4]; + uint8_t yiaddr[4]; + uint8_t siaddr[4]; + uint8_t giaddr[4]; + uint8_t chaddr[6]; +}RIP_MSG_FIXED; + +class DhcpClass { +private: + uint32_t _dhcpInitialTransactionId; + uint32_t _dhcpTransactionId; + uint8_t _dhcpMacAddr[6]; + uint8_t _dhcpLocalIp[4]; + uint8_t _dhcpSubnetMask[4]; + uint8_t _dhcpGatewayIp[4]; + uint8_t _dhcpDhcpServerIp[4]; + uint8_t _dhcpDnsServerIp[4]; + uint32_t _dhcpLeaseTime; + uint32_t _dhcpT1, _dhcpT2; + signed long _renewInSec; + signed long _rebindInSec; + signed long _lastCheck; + unsigned long _timeout; + unsigned long _responseTimeout; + unsigned long _secTimeout; + uint8_t _dhcp_state; + EthernetUDP _dhcpUdpSocket; + + int request_DHCP_lease(); + void reset_DHCP_lease(); + void presend_DHCP(); + void send_DHCP_MESSAGE(uint8_t, uint16_t); + void printByte(char *, uint8_t); + + uint8_t parseDHCPResponse(unsigned long responseTimeout, uint32_t& transactionId); +public: + IPAddress getLocalIp(); + IPAddress getSubnetMask(); + IPAddress getGatewayIp(); + IPAddress getDhcpServerIp(); + IPAddress getDnsServerIp(); + + int beginWithDHCP(uint8_t *, unsigned long timeout = 60000, unsigned long responseTimeout = 4000); + int checkLease(); +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/Dns.cpp b/libs/arduino-1.0/libraries/Ethernet/Dns.cpp new file mode 100644 index 0000000..b3c1a9d --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Dns.cpp @@ -0,0 +1,423 @@ +// Arduino DNS client for WizNet5100-based Ethernet shield +// (c) Copyright 2009-2010 MCQN Ltd. +// Released under Apache License, version 2.0 + +#include "w5100.h" +#include "EthernetUdp.h" +#include "util.h" + +#include "Dns.h" +#include +//#include +#include "Arduino.h" + + +#define SOCKET_NONE 255 +// Various flags and header field values for a DNS message +#define UDP_HEADER_SIZE 8 +#define DNS_HEADER_SIZE 12 +#define TTL_SIZE 4 +#define QUERY_FLAG (0) +#define RESPONSE_FLAG (1<<15) +#define QUERY_RESPONSE_MASK (1<<15) +#define OPCODE_STANDARD_QUERY (0) +#define OPCODE_INVERSE_QUERY (1<<11) +#define OPCODE_STATUS_REQUEST (2<<11) +#define OPCODE_MASK (15<<11) +#define AUTHORITATIVE_FLAG (1<<10) +#define TRUNCATION_FLAG (1<<9) +#define RECURSION_DESIRED_FLAG (1<<8) +#define RECURSION_AVAILABLE_FLAG (1<<7) +#define RESP_NO_ERROR (0) +#define RESP_FORMAT_ERROR (1) +#define RESP_SERVER_FAILURE (2) +#define RESP_NAME_ERROR (3) +#define RESP_NOT_IMPLEMENTED (4) +#define RESP_REFUSED (5) +#define RESP_MASK (15) +#define TYPE_A (0x0001) +#define CLASS_IN (0x0001) +#define LABEL_COMPRESSION_MASK (0xC0) +// Port number that DNS servers listen on +#define DNS_PORT 53 + +// Possible return codes from ProcessResponse +#define SUCCESS 1 +#define TIMED_OUT -1 +#define INVALID_SERVER -2 +#define TRUNCATED -3 +#define INVALID_RESPONSE -4 + +void DNSClient::begin(const IPAddress& aDNSServer) +{ + iDNSServer = aDNSServer; + iRequestId = 0; +} + + +int DNSClient::inet_aton(const char* aIPAddrString, IPAddress& aResult) +{ + // See if we've been given a valid IP address + const char* p =aIPAddrString; + while (*p && + ( (*p == '.') || (*p >= '0') || (*p <= '9') )) + { + p++; + } + + if (*p == '\0') + { + // It's looking promising, we haven't found any invalid characters + p = aIPAddrString; + int segment =0; + int segmentValue =0; + while (*p && (segment < 4)) + { + if (*p == '.') + { + // We've reached the end of a segment + if (segmentValue > 255) + { + // You can't have IP address segments that don't fit in a byte + return 0; + } + else + { + aResult[segment] = (byte)segmentValue; + segment++; + segmentValue = 0; + } + } + else + { + // Next digit + segmentValue = (segmentValue*10)+(*p - '0'); + } + p++; + } + // We've reached the end of address, but there'll still be the last + // segment to deal with + if ((segmentValue > 255) || (segment > 3)) + { + // You can't have IP address segments that don't fit in a byte, + // or more than four segments + return 0; + } + else + { + aResult[segment] = (byte)segmentValue; + return 1; + } + } + else + { + return 0; + } +} + +int DNSClient::getHostByName(const char* aHostname, IPAddress& aResult) +{ + int ret =0; + + // See if it's a numeric IP address + if (inet_aton(aHostname, aResult)) + { + // It is, our work here is done + return 1; + } + + // Check we've got a valid DNS server to use + if (iDNSServer == INADDR_NONE) + { + return INVALID_SERVER; + } + + // Find a socket to use + if (iUdp.begin(1024+(millis() & 0xF)) == 1) + { + // Try up to three times + int retries = 0; +// while ((retries < 3) && (ret <= 0)) + { + // Send DNS request + ret = iUdp.beginPacket(iDNSServer, DNS_PORT); + if (ret != 0) + { + // Now output the request data + ret = BuildRequest(aHostname); + if (ret != 0) + { + // And finally send the request + ret = iUdp.endPacket(); + if (ret != 0) + { + // Now wait for a response + int wait_retries = 0; + ret = TIMED_OUT; + while ((wait_retries < 3) && (ret == TIMED_OUT)) + { + ret = ProcessResponse(5000, aResult); + wait_retries++; + } + } + } + } + retries++; + } + + // We're done with the socket now + iUdp.stop(); + } + + return ret; +} + +uint16_t DNSClient::BuildRequest(const char* aName) +{ + // Build header + // 1 1 1 1 1 1 + // 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // | ID | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // |QR| Opcode |AA|TC|RD|RA| Z | RCODE | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // | QDCOUNT | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // | ANCOUNT | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // | NSCOUNT | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // | ARCOUNT | + // +--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+--+ + // As we only support one request at a time at present, we can simplify + // some of this header + iRequestId = millis(); // generate a random ID + uint16_t twoByteBuffer; + + // FIXME We should also check that there's enough space available to write to, rather + // FIXME than assume there's enough space (as the code does at present) + iUdp.write((uint8_t*)&iRequestId, sizeof(iRequestId)); + + twoByteBuffer = htons(QUERY_FLAG | OPCODE_STANDARD_QUERY | RECURSION_DESIRED_FLAG); + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + + twoByteBuffer = htons(1); // One question record + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + + twoByteBuffer = 0; // Zero answer records + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + // and zero additional records + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + + // Build question + const char* start =aName; + const char* end =start; + uint8_t len; + // Run through the name being requested + while (*end) + { + // Find out how long this section of the name is + end = start; + while (*end && (*end != '.') ) + { + end++; + } + + if (end-start > 0) + { + // Write out the size of this section + len = end-start; + iUdp.write(&len, sizeof(len)); + // And then write out the section + iUdp.write((uint8_t*)start, end-start); + } + start = end+1; + } + + // We've got to the end of the question name, so + // terminate it with a zero-length section + len = 0; + iUdp.write(&len, sizeof(len)); + // Finally the type and class of question + twoByteBuffer = htons(TYPE_A); + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + + twoByteBuffer = htons(CLASS_IN); // Internet class of question + iUdp.write((uint8_t*)&twoByteBuffer, sizeof(twoByteBuffer)); + // Success! Everything buffered okay + return 1; +} + + +uint16_t DNSClient::ProcessResponse(uint16_t aTimeout, IPAddress& aAddress) +{ + uint32_t startTime = millis(); + + // Wait for a response packet + while(iUdp.parsePacket() <= 0) + { + if((millis() - startTime) > aTimeout) + return TIMED_OUT; + delay(50); + } + + // We've had a reply! + // Read the UDP header + uint8_t header[DNS_HEADER_SIZE]; // Enough space to reuse for the DNS header + // Check that it's a response from the right server and the right port + if ( (iDNSServer != iUdp.remoteIP()) || + (iUdp.remotePort() != DNS_PORT) ) + { + // It's not from who we expected + return INVALID_SERVER; + } + + // Read through the rest of the response + if (iUdp.available() < DNS_HEADER_SIZE) + { + return TRUNCATED; + } + iUdp.read(header, DNS_HEADER_SIZE); + + uint16_t header_flags = htons(*((uint16_t*)&header[2])); + // Check that it's a response to this request + if ( ( iRequestId != (*((uint16_t*)&header[0])) ) || + ((header_flags & QUERY_RESPONSE_MASK) != (uint16_t)RESPONSE_FLAG) ) + { + // Mark the entire packet as read + iUdp.flush(); + return INVALID_RESPONSE; + } + // Check for any errors in the response (or in our request) + // although we don't do anything to get round these + if ( (header_flags & TRUNCATION_FLAG) || (header_flags & RESP_MASK) ) + { + // Mark the entire packet as read + iUdp.flush(); + return -5; //INVALID_RESPONSE; + } + + // And make sure we've got (at least) one answer + uint16_t answerCount = htons(*((uint16_t*)&header[6])); + if (answerCount == 0 ) + { + // Mark the entire packet as read + iUdp.flush(); + return -6; //INVALID_RESPONSE; + } + + // Skip over any questions + for (uint16_t i =0; i < htons(*((uint16_t*)&header[4])); i++) + { + // Skip over the name + uint8_t len; + do + { + iUdp.read(&len, sizeof(len)); + if (len > 0) + { + // Don't need to actually read the data out for the string, just + // advance ptr to beyond it + while(len--) + { + iUdp.read(); // we don't care about the returned byte + } + } + } while (len != 0); + + // Now jump over the type and class + for (int i =0; i < 4; i++) + { + iUdp.read(); // we don't care about the returned byte + } + } + + // Now we're up to the bit we're interested in, the answer + // There might be more than one answer (although we'll just use the first + // type A answer) and some authority and additional resource records but + // we're going to ignore all of them. + + for (uint16_t i =0; i < answerCount; i++) + { + // Skip the name + uint8_t len; + do + { + iUdp.read(&len, sizeof(len)); + if ((len & LABEL_COMPRESSION_MASK) == 0) + { + // It's just a normal label + if (len > 0) + { + // And it's got a length + // Don't need to actually read the data out for the string, + // just advance ptr to beyond it + while(len--) + { + iUdp.read(); // we don't care about the returned byte + } + } + } + else + { + // This is a pointer to a somewhere else in the message for the + // rest of the name. We don't care about the name, and RFC1035 + // says that a name is either a sequence of labels ended with a + // 0 length octet or a pointer or a sequence of labels ending in + // a pointer. Either way, when we get here we're at the end of + // the name + // Skip over the pointer + iUdp.read(); // we don't care about the returned byte + // And set len so that we drop out of the name loop + len = 0; + } + } while (len != 0); + + // Check the type and class + uint16_t answerType; + uint16_t answerClass; + iUdp.read((uint8_t*)&answerType, sizeof(answerType)); + iUdp.read((uint8_t*)&answerClass, sizeof(answerClass)); + + // Ignore the Time-To-Live as we don't do any caching + for (int i =0; i < TTL_SIZE; i++) + { + iUdp.read(); // we don't care about the returned byte + } + + // And read out the length of this answer + // Don't need header_flags anymore, so we can reuse it here + iUdp.read((uint8_t*)&header_flags, sizeof(header_flags)); + + if ( (htons(answerType) == TYPE_A) && (htons(answerClass) == CLASS_IN) ) + { + if (htons(header_flags) != 4) + { + // It's a weird size + // Mark the entire packet as read + iUdp.flush(); + return -9;//INVALID_RESPONSE; + } + iUdp.read(aAddress.raw_address(), 4); + return SUCCESS; + } + else + { + // This isn't an answer type we're after, move onto the next one + for (uint16_t i =0; i < htons(header_flags); i++) + { + iUdp.read(); // we don't care about the returned byte + } + } + } + + // Mark the entire packet as read + iUdp.flush(); + + // If we get here then we haven't found an answer + return -10;//INVALID_RESPONSE; +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/Dns.h b/libs/arduino-1.0/libraries/Ethernet/Dns.h new file mode 100644 index 0000000..c99f5c3 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Dns.h @@ -0,0 +1,41 @@ +// Arduino DNS client for WizNet5100-based Ethernet shield +// (c) Copyright 2009-2010 MCQN Ltd. +// Released under Apache License, version 2.0 + +#ifndef DNSClient_h +#define DNSClient_h + +#include + +class DNSClient +{ +public: + // ctor + void begin(const IPAddress& aDNSServer); + + /** Convert a numeric IP address string into a four-byte IP address. + @param aIPAddrString IP address to convert + @param aResult IPAddress structure to store the returned IP address + @result 1 if aIPAddrString was successfully converted to an IP address, + else error code + */ + int inet_aton(const char *aIPAddrString, IPAddress& aResult); + + /** Resolve the given hostname to an IP address. + @param aHostname Name to be resolved + @param aResult IPAddress structure to store the returned IP address + @result 1 if aIPAddrString was successfully converted to an IP address, + else error code + */ + int getHostByName(const char* aHostname, IPAddress& aResult); + +protected: + uint16_t BuildRequest(const char* aName); + uint16_t ProcessResponse(uint16_t aTimeout, IPAddress& aAddress); + + IPAddress iDNSServer; + uint16_t iRequestId; + EthernetUDP iUdp; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/Ethernet.cpp b/libs/arduino-1.0/libraries/Ethernet/Ethernet.cpp new file mode 100644 index 0000000..16b3f62 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Ethernet.cpp @@ -0,0 +1,121 @@ +#include "w5100.h" +#include "Ethernet.h" +#include "Dhcp.h" + +// XXX: don't make assumptions about the value of MAX_SOCK_NUM. +uint8_t EthernetClass::_state[MAX_SOCK_NUM] = { + 0, 0, 0, 0 }; +uint16_t EthernetClass::_server_port[MAX_SOCK_NUM] = { + 0, 0, 0, 0 }; + +int EthernetClass::begin(uint8_t *mac_address) +{ + _dhcp = new DhcpClass(); + + + // Initialise the basic info + W5100.init(); + W5100.setMACAddress(mac_address); + W5100.setIPAddress(IPAddress(0,0,0,0).raw_address()); + + // Now try to get our config info from a DHCP server + int ret = _dhcp->beginWithDHCP(mac_address); + if(ret == 1) + { + // We've successfully found a DHCP server and got our configuration info, so set things + // accordingly + W5100.setIPAddress(_dhcp->getLocalIp().raw_address()); + W5100.setGatewayIp(_dhcp->getGatewayIp().raw_address()); + W5100.setSubnetMask(_dhcp->getSubnetMask().raw_address()); + _dnsServerAddress = _dhcp->getDnsServerIp(); + } + + return ret; +} + +void EthernetClass::begin(uint8_t *mac_address, IPAddress local_ip) +{ + // Assume the DNS server will be the machine on the same network as the local IP + // but with last octet being '1' + IPAddress dns_server = local_ip; + dns_server[3] = 1; + begin(mac_address, local_ip, dns_server); +} + +void EthernetClass::begin(uint8_t *mac_address, IPAddress local_ip, IPAddress dns_server) +{ + // Assume the gateway will be the machine on the same network as the local IP + // but with last octet being '1' + IPAddress gateway = local_ip; + gateway[3] = 1; + begin(mac_address, local_ip, dns_server, gateway); +} + +void EthernetClass::begin(uint8_t *mac_address, IPAddress local_ip, IPAddress dns_server, IPAddress gateway) +{ + IPAddress subnet(255, 255, 255, 0); + begin(mac_address, local_ip, dns_server, gateway, subnet); +} + +void EthernetClass::begin(uint8_t *mac, IPAddress local_ip, IPAddress dns_server, IPAddress gateway, IPAddress subnet) +{ + W5100.init(); + W5100.setMACAddress(mac); + W5100.setIPAddress(local_ip._address.a8); + W5100.setGatewayIp(gateway._address.a8); + W5100.setSubnetMask(subnet._address.a8); + _dnsServerAddress = dns_server; +} + +int EthernetClass::maintain(){ + int rc = DHCP_CHECK_NONE; + if(_dhcp != NULL){ + //we have a pointer to dhcp, use it + rc = _dhcp->checkLease(); + switch ( rc ){ + case DHCP_CHECK_NONE: + //nothing done + break; + case DHCP_CHECK_RENEW_OK: + case DHCP_CHECK_REBIND_OK: + //we might have got a new IP. + W5100.setIPAddress(_dhcp->getLocalIp().raw_address()); + W5100.setGatewayIp(_dhcp->getGatewayIp().raw_address()); + W5100.setSubnetMask(_dhcp->getSubnetMask().raw_address()); + _dnsServerAddress = _dhcp->getDnsServerIp(); + break; + default: + //this is actually a error, it will retry though + break; + } + } + return rc; +} + +IPAddress EthernetClass::localIP() +{ + IPAddress ret; + W5100.getIPAddress(ret.raw_address()); + return ret; +} + +IPAddress EthernetClass::subnetMask() +{ + IPAddress ret; + W5100.getSubnetMask(ret.raw_address()); + return ret; +} + +IPAddress EthernetClass::gatewayIP() +{ + IPAddress ret; + W5100.getGatewayIp(ret.raw_address()); + return ret; +} + +IPAddress EthernetClass::dnsServerIP() +{ + return _dnsServerAddress; +} + +EthernetClass Ethernet; diff --git a/libs/arduino-1.0/libraries/Ethernet/Ethernet.h b/libs/arduino-1.0/libraries/Ethernet/Ethernet.h new file mode 100644 index 0000000..2a07ff3 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/Ethernet.h @@ -0,0 +1,41 @@ +#ifndef ethernet_h +#define ethernet_h + +#include +//#include "w5100.h" +#include "IPAddress.h" +#include "EthernetClient.h" +#include "EthernetServer.h" +#include "Dhcp.h" + +#define MAX_SOCK_NUM 4 + +class EthernetClass { +private: + IPAddress _dnsServerAddress; + DhcpClass* _dhcp; +public: + static uint8_t _state[MAX_SOCK_NUM]; + static uint16_t _server_port[MAX_SOCK_NUM]; + // Initialise the Ethernet shield to use the provided MAC address and gain the rest of the + // configuration through DHCP. + // Returns 0 if the DHCP configuration failed, and 1 if it succeeded + int begin(uint8_t *mac_address); + void begin(uint8_t *mac_address, IPAddress local_ip); + void begin(uint8_t *mac_address, IPAddress local_ip, IPAddress dns_server); + void begin(uint8_t *mac_address, IPAddress local_ip, IPAddress dns_server, IPAddress gateway); + void begin(uint8_t *mac_address, IPAddress local_ip, IPAddress dns_server, IPAddress gateway, IPAddress subnet); + int maintain(); + + IPAddress localIP(); + IPAddress subnetMask(); + IPAddress gatewayIP(); + IPAddress dnsServerIP(); + + friend class EthernetClient; + friend class EthernetServer; +}; + +extern EthernetClass Ethernet; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetClient.cpp b/libs/arduino-1.0/libraries/Ethernet/EthernetClient.cpp new file mode 100644 index 0000000..9885efb --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetClient.cpp @@ -0,0 +1,165 @@ +#include "w5100.h" +#include "socket.h" + +extern "C" { + #include "string.h" +} + +#include "Arduino.h" + +#include "Ethernet.h" +#include "EthernetClient.h" +#include "EthernetServer.h" +#include "Dns.h" + +uint16_t EthernetClient::_srcport = 1024; + +EthernetClient::EthernetClient() : _sock(MAX_SOCK_NUM) { +} + +EthernetClient::EthernetClient(uint8_t sock) : _sock(sock) { +} + +int EthernetClient::connect(const char* host, uint16_t port) { + // Look up the host first + int ret = 0; + DNSClient dns; + IPAddress remote_addr; + + dns.begin(Ethernet.dnsServerIP()); + ret = dns.getHostByName(host, remote_addr); + if (ret == 1) { + return connect(remote_addr, port); + } else { + return ret; + } +} + +int EthernetClient::connect(IPAddress ip, uint16_t port) { + if (_sock != MAX_SOCK_NUM) + return 0; + + for (int i = 0; i < MAX_SOCK_NUM; i++) { + uint8_t s = W5100.readSnSR(i); + if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT || s == SnSR::CLOSE_WAIT) { + _sock = i; + break; + } + } + + if (_sock == MAX_SOCK_NUM) + return 0; + + _srcport++; + if (_srcport == 0) _srcport = 1024; + socket(_sock, SnMR::TCP, _srcport, 0); + + if (!::connect(_sock, rawIPAddress(ip), port)) { + _sock = MAX_SOCK_NUM; + return 0; + } + + while (status() != SnSR::ESTABLISHED) { + delay(1); + if (status() == SnSR::CLOSED) { + _sock = MAX_SOCK_NUM; + return 0; + } + } + + return 1; +} + +size_t EthernetClient::write(uint8_t b) { + return write(&b, 1); +} + +size_t EthernetClient::write(const uint8_t *buf, size_t size) { + if (_sock == MAX_SOCK_NUM) { + setWriteError(); + return 0; + } + if (!send(_sock, buf, size)) { + setWriteError(); + return 0; + } + return size; +} + +int EthernetClient::available() { + if (_sock != MAX_SOCK_NUM) + return W5100.getRXReceivedSize(_sock); + return 0; +} + +int EthernetClient::read() { + uint8_t b; + if ( recv(_sock, &b, 1) > 0 ) + { + // recv worked + return b; + } + else + { + // No data available + return -1; + } +} + +int EthernetClient::read(uint8_t *buf, size_t size) { + return recv(_sock, buf, size); +} + +int EthernetClient::peek() { + uint8_t b; + // Unlike recv, peek doesn't check to see if there's any data available, so we must + if (!available()) + return -1; + ::peek(_sock, &b); + return b; +} + +void EthernetClient::flush() { + while (available()) + read(); +} + +void EthernetClient::stop() { + if (_sock == MAX_SOCK_NUM) + return; + + // attempt to close the connection gracefully (send a FIN to other side) + disconnect(_sock); + unsigned long start = millis(); + + // wait a second for the connection to close + while (status() != SnSR::CLOSED && millis() - start < 1000) + delay(1); + + // if it hasn't closed, close it forcefully + if (status() != SnSR::CLOSED) + close(_sock); + + EthernetClass::_server_port[_sock] = 0; + _sock = MAX_SOCK_NUM; +} + +uint8_t EthernetClient::connected() { + if (_sock == MAX_SOCK_NUM) return 0; + + uint8_t s = status(); + return !(s == SnSR::LISTEN || s == SnSR::CLOSED || s == SnSR::FIN_WAIT || + (s == SnSR::CLOSE_WAIT && !available())); +} + +uint8_t EthernetClient::status() { + if (_sock == MAX_SOCK_NUM) return SnSR::CLOSED; + return W5100.readSnSR(_sock); +} + +// the next function allows us to use the client returned by +// EthernetServer::available() as the condition in an if-statement. + +EthernetClient::operator bool() { + return _sock != MAX_SOCK_NUM; +} diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetClient.h b/libs/arduino-1.0/libraries/Ethernet/EthernetClient.h new file mode 100644 index 0000000..44740fe --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetClient.h @@ -0,0 +1,37 @@ +#ifndef ethernetclient_h +#define ethernetclient_h +#include "Arduino.h" +#include "Print.h" +#include "Client.h" +#include "IPAddress.h" + +class EthernetClient : public Client { + +public: + EthernetClient(); + EthernetClient(uint8_t sock); + + uint8_t status(); + virtual int connect(IPAddress ip, uint16_t port); + virtual int connect(const char *host, uint16_t port); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *buf, size_t size); + virtual int available(); + virtual int read(); + virtual int read(uint8_t *buf, size_t size); + virtual int peek(); + virtual void flush(); + virtual void stop(); + virtual uint8_t connected(); + virtual operator bool(); + + friend class EthernetServer; + + using Print::write; + +private: + static uint16_t _srcport; + uint8_t _sock; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetServer.cpp b/libs/arduino-1.0/libraries/Ethernet/EthernetServer.cpp new file mode 100644 index 0000000..0308b92 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetServer.cpp @@ -0,0 +1,91 @@ +#include "w5100.h" +#include "socket.h" +extern "C" { +#include "string.h" +} + +#include "Ethernet.h" +#include "EthernetClient.h" +#include "EthernetServer.h" + +EthernetServer::EthernetServer(uint16_t port) +{ + _port = port; +} + +void EthernetServer::begin() +{ + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) { + EthernetClient client(sock); + if (client.status() == SnSR::CLOSED) { + socket(sock, SnMR::TCP, _port, 0); + listen(sock); + EthernetClass::_server_port[sock] = _port; + break; + } + } +} + +void EthernetServer::accept() +{ + int listening = 0; + + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) { + EthernetClient client(sock); + + if (EthernetClass::_server_port[sock] == _port) { + if (client.status() == SnSR::LISTEN) { + listening = 1; + } + else if (client.status() == SnSR::CLOSE_WAIT && !client.available()) { + client.stop(); + } + } + } + + if (!listening) { + begin(); + } +} + +EthernetClient EthernetServer::available() +{ + accept(); + + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) { + EthernetClient client(sock); + if (EthernetClass::_server_port[sock] == _port && + (client.status() == SnSR::ESTABLISHED || + client.status() == SnSR::CLOSE_WAIT)) { + if (client.available()) { + // XXX: don't always pick the lowest numbered socket. + return client; + } + } + } + + return EthernetClient(MAX_SOCK_NUM); +} + +size_t EthernetServer::write(uint8_t b) +{ + return write(&b, 1); +} + +size_t EthernetServer::write(const uint8_t *buffer, size_t size) +{ + size_t n = 0; + + accept(); + + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) { + EthernetClient client(sock); + + if (EthernetClass::_server_port[sock] == _port && + client.status() == SnSR::ESTABLISHED) { + n += client.write(buffer, size); + } + } + + return n; +} diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetServer.h b/libs/arduino-1.0/libraries/Ethernet/EthernetServer.h new file mode 100644 index 0000000..86ccafe --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetServer.h @@ -0,0 +1,22 @@ +#ifndef ethernetserver_h +#define ethernetserver_h + +#include "Server.h" + +class EthernetClient; + +class EthernetServer : +public Server { +private: + uint16_t _port; + void accept(); +public: + EthernetServer(uint16_t); + EthernetClient available(); + virtual void begin(); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *buf, size_t size); + using Print::write; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.cpp b/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.cpp new file mode 100644 index 0000000..3760052 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.cpp @@ -0,0 +1,218 @@ +/* + * Udp.cpp: Library to send/receive UDP packets with the Arduino ethernet shield. + * This version only offers minimal wrapping of socket.c/socket.h + * Drop Udp.h/.cpp into the Ethernet library directory at hardware/libraries/Ethernet/ + * + * MIT License: + * Copyright (c) 2008 Bjoern Hartmann + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * bjoern@cs.stanford.edu 12/30/2008 + */ + +#include "w5100.h" +#include "socket.h" +#include "Ethernet.h" +#include "Udp.h" +#include "Dns.h" + +/* Constructor */ +EthernetUDP::EthernetUDP() : _sock(MAX_SOCK_NUM) {} + +/* Start EthernetUDP socket, listening at local port PORT */ +uint8_t EthernetUDP::begin(uint16_t port) { + if (_sock != MAX_SOCK_NUM) + return 0; + + for (int i = 0; i < MAX_SOCK_NUM; i++) { + uint8_t s = W5100.readSnSR(i); + if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT) { + _sock = i; + break; + } + } + + if (_sock == MAX_SOCK_NUM) + return 0; + + _port = port; + _remaining = 0; + socket(_sock, SnMR::UDP, _port, 0); + + return 1; +} + +/* return number of bytes available in the current packet, + will return zero if parsePacket hasn't been called yet */ +int EthernetUDP::available() { + return _remaining; +} + +/* Release any resources being used by this EthernetUDP instance */ +void EthernetUDP::stop() +{ + if (_sock == MAX_SOCK_NUM) + return; + + close(_sock); + + EthernetClass::_server_port[_sock] = 0; + _sock = MAX_SOCK_NUM; +} + +int EthernetUDP::beginPacket(const char *host, uint16_t port) +{ + // Look up the host first + int ret = 0; + DNSClient dns; + IPAddress remote_addr; + + dns.begin(Ethernet.dnsServerIP()); + ret = dns.getHostByName(host, remote_addr); + if (ret == 1) { + return beginPacket(remote_addr, port); + } else { + return ret; + } +} + +int EthernetUDP::beginPacket(IPAddress ip, uint16_t port) +{ + _offset = 0; + return startUDP(_sock, rawIPAddress(ip), port); +} + +int EthernetUDP::endPacket() +{ + return sendUDP(_sock); +} + +size_t EthernetUDP::write(uint8_t byte) +{ + return write(&byte, 1); +} + +size_t EthernetUDP::write(const uint8_t *buffer, size_t size) +{ + uint16_t bytes_written = bufferData(_sock, _offset, buffer, size); + _offset += bytes_written; + return bytes_written; +} + +int EthernetUDP::parsePacket() +{ + // discard any remaining bytes in the last packet + flush(); + + if (W5100.getRXReceivedSize(_sock) > 0) + { + //HACK - hand-parse the UDP packet using TCP recv method + uint8_t tmpBuf[8]; + int ret =0; + //read 8 header bytes and get IP and port from it + ret = recv(_sock,tmpBuf,8); + if (ret > 0) + { + _remoteIP = tmpBuf; + _remotePort = tmpBuf[4]; + _remotePort = (_remotePort << 8) + tmpBuf[5]; + _remaining = tmpBuf[6]; + _remaining = (_remaining << 8) + tmpBuf[7]; + + // When we get here, any remaining bytes are the data + ret = _remaining; + } + return ret; + } + // There aren't any packets available + return 0; +} + +int EthernetUDP::read() +{ + uint8_t byte; + + if ((_remaining > 0) && (recv(_sock, &byte, 1) > 0)) + { + // We read things without any problems + _remaining--; + return byte; + } + + // If we get here, there's no data available + return -1; +} + +int EthernetUDP::read(unsigned char* buffer, size_t len) +{ + + if (_remaining > 0) + { + + int got; + + if (_remaining <= len) + { + // data should fit in the buffer + got = recv(_sock, buffer, _remaining); + } + else + { + // too much data for the buffer, + // grab as much as will fit + got = recv(_sock, buffer, len); + } + + if (got > 0) + { + _remaining -= got; + return got; + } + + } + + // If we get here, there's no data available or recv failed + return -1; + +} + +int EthernetUDP::peek() +{ + uint8_t b; + // Unlike recv, peek doesn't check to see if there's any data available, so we must. + // If the user hasn't called parsePacket yet then return nothing otherwise they + // may get the UDP header + if (!_remaining) + return -1; + ::peek(_sock, &b); + return b; +} + +void EthernetUDP::flush() +{ + // could this fail (loop endlessly) if _remaining > 0 and recv in read fails? + // should only occur if recv fails after telling us the data is there, lets + // hope the w5100 always behaves :) + + while (_remaining) + { + read(); + } +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.h b/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.h new file mode 100644 index 0000000..8a6b7ab --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/EthernetUdp.h @@ -0,0 +1,99 @@ +/* + * Udp.cpp: Library to send/receive UDP packets with the Arduino ethernet shield. + * This version only offers minimal wrapping of socket.c/socket.h + * Drop Udp.h/.cpp into the Ethernet library directory at hardware/libraries/Ethernet/ + * + * NOTE: UDP is fast, but has some important limitations (thanks to Warren Gray for mentioning these) + * 1) UDP does not guarantee the order in which assembled UDP packets are received. This + * might not happen often in practice, but in larger network topologies, a UDP + * packet can be received out of sequence. + * 2) UDP does not guard against lost packets - so packets *can* disappear without the sender being + * aware of it. Again, this may not be a concern in practice on small local networks. + * For more information, see http://www.cafeaulait.org/course/week12/35.html + * + * MIT License: + * Copyright (c) 2008 Bjoern Hartmann + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * bjoern@cs.stanford.edu 12/30/2008 + */ + +#ifndef ethernetudp_h +#define ethernetudp_h + +#include + +#define UDP_TX_PACKET_MAX_SIZE 24 + +class EthernetUDP : public UDP { +private: + uint8_t _sock; // socket ID for Wiz5100 + uint16_t _port; // local port to listen on + IPAddress _remoteIP; // remote IP address for the incoming packet whilst it's being processed + uint16_t _remotePort; // remote port for the incoming packet whilst it's being processed + uint16_t _offset; // offset into the packet being sent + uint16_t _remaining; // remaining bytes of incoming packet yet to be processed + +public: + EthernetUDP(); // Constructor + virtual uint8_t begin(uint16_t); // initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use + virtual void stop(); // Finish with the UDP socket + + // Sending UDP packets + + // Start building up a packet to send to the remote host specific in ip and port + // Returns 1 if successful, 0 if there was a problem with the supplied IP address or port + virtual int beginPacket(IPAddress ip, uint16_t port); + // Start building up a packet to send to the remote host specific in host and port + // Returns 1 if successful, 0 if there was a problem resolving the hostname or port + virtual int beginPacket(const char *host, uint16_t port); + // Finish off this packet and send it + // Returns 1 if the packet was sent successfully, 0 if there was an error + virtual int endPacket(); + // Write a single byte into the packet + virtual size_t write(uint8_t); + // Write size bytes from buffer into the packet + virtual size_t write(const uint8_t *buffer, size_t size); + + using Print::write; + + // Start processing the next available incoming packet + // Returns the size of the packet in bytes, or 0 if no packets are available + virtual int parsePacket(); + // Number of bytes remaining in the current packet + virtual int available(); + // Read a single byte from the current packet + virtual int read(); + // Read up to len bytes from the current packet and place them into buffer + // Returns the number of bytes read, or 0 if none are available + virtual int read(unsigned char* buffer, size_t len); + // Read up to len characters from the current packet and place them into buffer + // Returns the number of characters read, or 0 if none are available + virtual int read(char* buffer, size_t len) { return read((unsigned char*)buffer, len); }; + // Return the next byte from the current packet without moving on to the next byte + virtual int peek(); + virtual void flush(); // Finish reading the current packet + + // Return the IP address of the host who sent the current incoming packet + virtual IPAddress remoteIP() { return _remoteIP; }; + // Return the port of the host who sent the current incoming packet + virtual uint16_t remotePort() { return _remotePort; }; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/BarometricPressureWebServer/BarometricPressureWebServer.ino b/libs/arduino-1.0/libraries/Ethernet/examples/BarometricPressureWebServer/BarometricPressureWebServer.ino new file mode 100644 index 0000000..bfbcb6d --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/BarometricPressureWebServer/BarometricPressureWebServer.ino @@ -0,0 +1,222 @@ +/* + SCP1000 Barometric Pressure Sensor Display + + Serves the output of a Barometric Pressure Sensor as a web page. + Uses the SPI library. For details on the sensor, see: + http://www.sparkfun.com/commerce/product_info.php?products_id=8161 + http://www.vti.fi/en/support/obsolete_products/pressure_sensors/ + + This sketch adapted from Nathan Seidle's SCP1000 example for PIC: + http://www.sparkfun.com/datasheets/Sensors/SCP1000-Testing.zip + + Circuit: + SCP1000 sensor attached to pins 6,7, and 11 - 13: + DRDY: pin 6 + CSB: pin 7 + MOSI: pin 11 + MISO: pin 12 + SCK: pin 13 + + created 31 July 2010 + by Tom Igoe + */ + +#include +// the sensor communicates using SPI, so include the library: +#include + + +// assign a MAC address for the ethernet controller. +// fill in your address here: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED}; +// assign an IP address for the controller: +IPAddress ip(192,168,1,20); +IPAddress gateway(192,168,1,1); +IPAddress subnet(255, 255, 255, 0); + + +// Initialize the Ethernet server library +// with the IP address and port you want to use +// (port 80 is default for HTTP): +EthernetServer server(80); + + +//Sensor's memory register addresses: +const int PRESSURE = 0x1F; //3 most significant bits of pressure +const int PRESSURE_LSB = 0x20; //16 least significant bits of pressure +const int TEMPERATURE = 0x21; //16 bit temperature reading + +// pins used for the connection with the sensor +// the others you need are controlled by the SPI library): +const int dataReadyPin = 6; +const int chipSelectPin = 7; + +float temperature = 0.0; +long pressure = 0; +long lastReadingTime = 0; + +void setup() { + // start the SPI library: + SPI.begin(); + + // start the Ethernet connection and the server: + Ethernet.begin(mac, ip); + server.begin(); + + // initalize the data ready and chip select pins: + pinMode(dataReadyPin, INPUT); + pinMode(chipSelectPin, OUTPUT); + + Serial.begin(9600); + + //Configure SCP1000 for low noise configuration: + writeRegister(0x02, 0x2D); + writeRegister(0x01, 0x03); + writeRegister(0x03, 0x02); + + // give the sensor and Ethernet shield time to set up: + delay(1000); + + //Set the sensor to high resolution mode tp start readings: + writeRegister(0x03, 0x0A); + +} + +void loop() { + // check for a reading no more than once a second. + if (millis() - lastReadingTime > 1000){ + // if there's a reading ready, read it: + // don't do anything until the data ready pin is high: + if (digitalRead(dataReadyPin) == HIGH) { + getData(); + // timestamp the last time you got a reading: + lastReadingTime = millis(); + } + } + + // listen for incoming Ethernet connections: + listenForEthernetClients(); +} + + +void getData() { + Serial.println("Getting reading"); + //Read the temperature data + int tempData = readRegister(0x21, 2); + + // convert the temperature to celsius and display it: + temperature = (float)tempData / 20.0; + + //Read the pressure data highest 3 bits: + byte pressureDataHigh = readRegister(0x1F, 1); + pressureDataHigh &= 0b00000111; //you only needs bits 2 to 0 + + //Read the pressure data lower 16 bits: + unsigned int pressureDataLow = readRegister(0x20, 2); + //combine the two parts into one 19-bit number: + pressure = ((pressureDataHigh << 16) | pressureDataLow)/4; + + Serial.print("Temperature: "); + Serial.print(temperature); + Serial.println(" degrees C"); + Serial.print("Pressure: " + String(pressure)); + Serial.println(" Pa"); +} + +void listenForEthernetClients() { + // listen for incoming clients + EthernetClient client = server.available(); + if (client) { + Serial.println("Got a client"); + // an http request ends with a blank line + boolean currentLineIsBlank = true; + while (client.connected()) { + if (client.available()) { + char c = client.read(); + // if you've gotten to the end of the line (received a newline + // character) and the line is blank, the http request has ended, + // so you can send a reply + if (c == '\n' && currentLineIsBlank) { + // send a standard http response header + client.println("HTTP/1.1 200 OK"); + client.println("Content-Type: text/html"); + client.println(); + // print the current readings, in HTML format: + client.print("Temperature: "); + client.print(temperature); + client.print(" degrees C"); + client.println("
"); + client.print("Pressure: " + String(pressure)); + client.print(" Pa"); + client.println("
"); + break; + } + if (c == '\n') { + // you're starting a new line + currentLineIsBlank = true; + } + else if (c != '\r') { + // you've gotten a character on the current line + currentLineIsBlank = false; + } + } + } + // give the web browser time to receive the data + delay(1); + // close the connection: + client.stop(); + } +} + + +//Send a write command to SCP1000 +void writeRegister(byte registerName, byte registerValue) { + // SCP1000 expects the register name in the upper 6 bits + // of the byte: + registerName <<= 2; + // command (read or write) goes in the lower two bits: + registerName |= 0b00000010; //Write command + + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + + SPI.transfer(registerName); //Send register location + SPI.transfer(registerValue); //Send value to record into register + + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); +} + + +//Read register from the SCP1000: +unsigned int readRegister(byte registerName, int numBytes) { + byte inByte = 0; // incoming from the SPI read + unsigned int result = 0; // result to return + + // SCP1000 expects the register name in the upper 6 bits + // of the byte: + registerName <<= 2; + // command (read or write) goes in the lower two bits: + registerName &= 0b11111100; //Read command + + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + // send the device the register you want to read: + int command = SPI.transfer(registerName); + // send a value of 0 to read the first byte returned: + inByte = SPI.transfer(0x00); + + result = inByte; + // if there's more than one byte returned, + // shift the first byte then get the second byte: + if (numBytes > 1){ + result = inByte << 8; + inByte = SPI.transfer(0x00); + result = result |inByte; + } + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); + // return the result: + return(result); +} diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/ChatServer/ChatServer.ino b/libs/arduino-1.0/libraries/Ethernet/examples/ChatServer/ChatServer.ino new file mode 100644 index 0000000..d50e5a6 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/ChatServer/ChatServer.ino @@ -0,0 +1,79 @@ +/* + Chat Server + + A simple server that distributes any incoming messages to all + connected clients. To use telnet to your device's IP address and type. + You can see the client's input in the serial monitor as well. + Using an Arduino Wiznet Ethernet shield. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + * Analog inputs attached to pins A0 through A5 (optional) + + created 18 Dec 2009 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe + + */ + +#include +#include + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network. +// gateway and subnet are optional: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; +IPAddress ip(192,168,1, 177); +IPAddress gateway(192,168,1, 1); +IPAddress subnet(255, 255, 0, 0); + + +// telnet defaults to port 23 +EthernetServer server(23); +boolean alreadyConnected = false; // whether or not the client was connected previously + +void setup() { + // initialize the ethernet device + Ethernet.begin(mac, ip, gateway, subnet); + // start listening for clients + server.begin(); + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Chat server address:"); + Serial.println(Ethernet.localIP()); +} + +void loop() { + // wait for a new client: + EthernetClient client = server.available(); + + // when the client sends the first byte, say hello: + if (client) { + if (!alreadyConnected) { + // clead out the input buffer: + client.flush(); + Serial.println("We have a new client"); + client.println("Hello, client!"); + alreadyConnected = true; + } + + if (client.available() > 0) { + // read the bytes incoming from the client: + char thisChar = client.read(); + // echo the bytes back to the client: + server.write(thisChar); + // echo the bytes to the server as well: + Serial.write(thisChar); + } + } +} + + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/DhcpAddressPrinter/DhcpAddressPrinter.ino b/libs/arduino-1.0/libraries/Ethernet/examples/DhcpAddressPrinter/DhcpAddressPrinter.ino new file mode 100644 index 0000000..5eaaf24 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/DhcpAddressPrinter/DhcpAddressPrinter.ino @@ -0,0 +1,59 @@ +/* + DHCP-based IP printer + + This sketch uses the DHCP extensions to the Ethernet library + to get an IP address via DHCP and print the address obtained. + using an Arduino Wiznet Ethernet shield. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 12 April 2011 + modified 9 Apr 2012 + by Tom Igoe + + */ + +#include +#include + +// Enter a MAC address for your controller below. +// Newer Ethernet shields have a MAC address printed on a sticker on the shield +byte mac[] = { + 0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02 }; + +// Initialize the Ethernet client library +// with the IP address and port of the server +// that you want to connect to (port 80 is default for HTTP): +EthernetClient client; + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + // this check is only needed on the Leonardo: + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // start the Ethernet connection: + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // no point in carrying on, so do nothing forevermore: + for(;;) + ; + } + // print your local IP address: + Serial.print("My IP address: "); + for (byte thisByte = 0; thisByte < 4; thisByte++) { + // print the value of each byte of the IP address: + Serial.print(Ethernet.localIP()[thisByte], DEC); + Serial.print("."); + } + Serial.println(); +} + +void loop() { + +} + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/DhcpChatServer/DhcpChatServer.ino b/libs/arduino-1.0/libraries/Ethernet/examples/DhcpChatServer/DhcpChatServer.ino new file mode 100644 index 0000000..09cbd43 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/DhcpChatServer/DhcpChatServer.ino @@ -0,0 +1,87 @@ +/* + DHCP Chat Server + + A simple server that distributes any incoming messages to all + connected clients. To use telnet to your device's IP address and type. + You can see the client's input in the serial monitor as well. + Using an Arduino Wiznet Ethernet shield. + + THis version attempts to get an IP address using DHCP + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 21 May 2011 + modified 9 Apr 2012 + by Tom Igoe + Based on ChatServer example by David A. Mellis + + */ + +#include +#include + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network. +// gateway and subnet are optional: +byte mac[] = { + 0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02 }; +IPAddress ip(192,168,1, 177); +IPAddress gateway(192,168,1, 1); +IPAddress subnet(255, 255, 0, 0); + +// telnet defaults to port 23 +EthernetServer server(23); +boolean gotAMessage = false; // whether or not you got a message from the client yet + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + // this check is only needed on the Leonardo: + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // start the Ethernet connection: + Serial.println("Trying to get an IP address using DHCP"); + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // initialize the ethernet device not using DHCP: + Ethernet.begin(mac, ip, gateway, subnet); + } + // print your local IP address: + Serial.print("My IP address: "); + ip = Ethernet.localIP(); + for (byte thisByte = 0; thisByte < 4; thisByte++) { + // print the value of each byte of the IP address: + Serial.print(ip[thisByte], DEC); + Serial.print("."); + } + Serial.println(); + // start listening for clients + server.begin(); + +} + +void loop() { + // wait for a new client: + EthernetClient client = server.available(); + + // when the client sends the first byte, say hello: + if (client) { + if (!gotAMessage) { + Serial.println("We have a new client"); + client.println("Hello, client!"); + gotAMessage = true; + } + + // read the bytes incoming from the client: + char thisChar = client.read(); + // echo the bytes back to the client: + server.write(thisChar); + // echo the bytes to the server as well: + Serial.print(thisChar); + } +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/DnsWebClient/DnsWebClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/DnsWebClient/DnsWebClient.ino new file mode 100644 index 0000000..c14abf4 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/DnsWebClient/DnsWebClient.ino @@ -0,0 +1,81 @@ +/* + DNS and DHCP-based Web client + + This sketch connects to a website (http://www.google.com) + using an Arduino Wiznet Ethernet shield. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 18 Dec 2009 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe, based on work by Adrian McEwen + + */ + +#include +#include + +// Enter a MAC address for your controller below. +// Newer Ethernet shields have a MAC address printed on a sticker on the shield +byte mac[] = { 0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x02 }; +char serverName[] = "www.google.com"; + +// Initialize the Ethernet client library +// with the IP address and port of the server +// that you want to connect to (port 80 is default for HTTP): +EthernetClient client; + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // start the Ethernet connection: + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // no point in carrying on, so do nothing forevermore: + while(true); + } + // give the Ethernet shield a second to initialize: + delay(1000); + Serial.println("connecting..."); + + // if you get a connection, report back via serial: + + if (client.connect(serverName, 80)) { + Serial.println("connected"); + // Make a HTTP request: + client.println("GET /search?q=arduino HTTP/1.0"); + client.println(); + } + else { + // kf you didn't get a connection to the server: + Serial.println("connection failed"); + } +} + +void loop() +{ + // if there are incoming bytes available + // from the server, read them and print them: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if the server's disconnected, stop the client: + if (!client.connected()) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + + // do nothing forevermore: + while(true); + } +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClient/PachubeClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClient/PachubeClient.ino new file mode 100644 index 0000000..dfd2d40 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClient/PachubeClient.ino @@ -0,0 +1,163 @@ +/* + Pachube sensor client + + This sketch connects an analog sensor to Pachube (http://www.pachube.com) + using a Wiznet Ethernet shield. You can use the Arduino Ethernet shield, or + the Adafruit Ethernet shield, either one will work, as long as it's got + a Wiznet Ethernet module on board. + + This example has been updated to use version 2.0 of the Pachube.com API. + To make it work, create a feed with a datastream, and give it the ID + sensor1. Or change the code below to match your feed. + + + Circuit: + * Analog sensor attached to analog in 0 + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 15 March 2010 + modified 9 Apr 2012 + by Tom Igoe with input from Usman Haque and Joe Saavedra + +http://arduino.cc/en/Tutorial/PachubeClient + This code is in the public domain. + + */ + +#include +#include + +#define APIKEY "YOUR API KEY GOES HERE" // replace your pachube api key here +#define FEEDID 00000 // replace your feed ID +#define USERAGENT "My Project" // user agent is the project name + +// assign a MAC address for the ethernet controller. +// Newer Ethernet shields have a MAC address printed on a sticker on the shield +// fill in your address here: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED}; + +// fill in an available IP address on your network here, +// for manual configuration: +IPAddress ip(10,0,1,20); +// initialize the library instance: +EthernetClient client; + +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +IPAddress server(216,52,233,122); // numeric IP for api.pachube.com +//char server[] = "api.pachube.com"; // name address for pachube API + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 10*1000; //delay between updates to Pachube.com + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // start the Ethernet connection: + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // DHCP failed, so use a fixed IP address: + Ethernet.begin(mac, ip); + } +} + +void loop() { + // read the analog sensor: + int sensorReading = analogRead(A0); + + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + sendData(sensorReading); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void sendData(int thisData) { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.print("PUT /v2/feeds/"); + client.print(FEEDID); + client.println(".csv HTTP/1.1"); + client.println("Host: api.pachube.com"); + client.print("X-PachubeApiKey: "); + client.println(APIKEY); + client.print("User-Agent: "); + client.println(USERAGENT); + client.print("Content-Length: "); + + // calculate the length of the sensor reading in bytes: + // 8 bytes for "sensor1," + number of digits of the data: + int thisLength = 8 + getLength(thisData); + client.println(thisLength); + + // last pieces of the HTTP PUT request: + client.println("Content-Type: text/csv"); + client.println("Connection: close"); + client.println(); + + // here's the actual content of the PUT request: + client.print("sensor1,"); + client.println(thisData); + + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + // note the time that the connection was made or attempted: + lastConnectionTime = millis(); +} + + +// This method calculates the number of digits in the +// sensor reading. Since each digit of the ASCII decimal +// representation is a byte, the number of digits equals +// the number of bytes: + +int getLength(int someValue) { + // there's at least one byte: + int digits = 1; + // continually divide the value by ten, + // adding one to the digit count for each + // time you divide, until you're at 0: + int dividend = someValue /10; + while (dividend > 0) { + dividend = dividend /10; + digits++; + } + // return the number of digits: + return digits; +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClientString/PachubeClientString.ino b/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClientString/PachubeClientString.ino new file mode 100644 index 0000000..26472d1 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/PachubeClientString/PachubeClientString.ino @@ -0,0 +1,154 @@ +/* + Pachube sensor client with Strings + + This sketch connects an analog sensor to Pachube (http://www.pachube.com) + using a Wiznet Ethernet shield. You can use the Arduino Ethernet shield, or + the Adafruit Ethernet shield, either one will work, as long as it's got + a Wiznet Ethernet module on board. + + This example has been updated to use version 2.0 of the pachube.com API. + To make it work, create a feed with two datastreams, and give them the IDs + sensor1 and sensor2. Or change the code below to match your feed. + + This example uses the String library, which is part of the Arduino core from + version 0019. + + Circuit: + * Analog sensor attached to analog in 0 + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 15 March 2010 + modified 9 Apr 2012 + by Tom Igoe with input from Usman Haque and Joe Saavedra + modified 8 September 2012 + by Scott Fitzgerald + + http://arduino.cc/en/Tutorial/PachubeClientString + This code is in the public domain. + + */ + +#include +#include + + +#define APIKEY "YOUR API KEY GOES HERE" // replace your Pachube api key here +#define FEEDID 00000 // replace your feed ID +#define USERAGENT "My Project" // user agent is the project name + + +// assign a MAC address for the ethernet controller. +// fill in your address here: + byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED}; + +// fill in an available IP address on your network here, +// for manual configuration: +IPAddress ip(10,0,1,20); + +// initialize the library instance: +EthernetClient client; + +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +IPAddress server(216,52,233,121); // numeric IP for api.pachube.com +//char server[] = "api.pachube.com"; // name address for pachube API + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 10*1000; //delay between updates to pachube.com + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // give the ethernet module time to boot up: + delay(1000); + // start the Ethernet connection: + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // DHCP failed, so use a fixed IP address: + Ethernet.begin(mac, ip); + } +} + +void loop() { + // read the analog sensor: + int sensorReading = analogRead(A0); + // convert the data to a String to send it: + + String dataString = "sensor1,"; + dataString += sensorReading; + + // you can append multiple readings to this String if your + // pachube feed is set up to handle multiple values: + int otherSensorReading = analogRead(A1); + dataString += "\nsensor2,"; + dataString += otherSensorReading; + + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + sendData(dataString); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void sendData(String thisData) { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.print("PUT /v2/feeds/"); + client.print(FEEDID); + client.println(".csv HTTP/1.1"); + client.println("Host: api.pachube.com"); + client.print("X-pachubeApiKey: "); + client.println(APIKEY); + client.print("User-Agent: "); + client.println(USERAGENT); + client.print("Content-Length: "); + client.println(thisData.length()); + + // last pieces of the HTTP PUT request: + client.println("Content-Type: text/csv"); + client.println("Connection: close"); + client.println(); + + // here's the actual content of the PUT request: + client.println(thisData); + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + // note the time that the connection was made or attempted: + lastConnectionTime = millis(); +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/TelnetClient/TelnetClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/TelnetClient/TelnetClient.ino new file mode 100644 index 0000000..3457125 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/TelnetClient/TelnetClient.ino @@ -0,0 +1,93 @@ +/* + Telnet client + + This sketch connects to a a telnet server (http://www.google.com) + using an Arduino Wiznet Ethernet shield. You'll need a telnet server + to test this with. + Processing's ChatServer example (part of the network library) works well, + running on port 10002. It can be found as part of the examples + in the Processing application, available at + http://processing.org/ + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 14 Sep 2010 + modified 9 Apr 2012 + by Tom Igoe + + */ + +#include +#include + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; +IPAddress ip(192,168,1,177); + +// Enter the IP address of the server you're connecting to: +IPAddress server(1,1,1,1); + +// Initialize the Ethernet client library +// with the IP address and port of the server +// that you want to connect to (port 23 is default for telnet; +// if you're using Processing's ChatServer, use port 10002): +EthernetClient client; + +void setup() { + // start the Ethernet connection: + Ethernet.begin(mac, ip); + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // give the Ethernet shield a second to initialize: + delay(1000); + Serial.println("connecting..."); + + // if you get a connection, report back via serial: + if (client.connect(server, 10002)) { + Serial.println("connected"); + } + else { + // if you didn't get a connection to the server: + Serial.println("connection failed"); + } +} + +void loop() +{ + // if there are incoming bytes available + // from the server, read them and print them: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // as long as there are bytes in the serial queue, + // read them and send them out the socket if it's open: + while (Serial.available() > 0) { + char inChar = Serial.read(); + if (client.connected()) { + client.print(inChar); + } + } + + // if the server's disconnected, stop the client: + if (!client.connected()) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + // do nothing: + while(true); + } +} + + + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/TwitterClient/TwitterClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/TwitterClient/TwitterClient.ino new file mode 100644 index 0000000..3587d72 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/TwitterClient/TwitterClient.ino @@ -0,0 +1,135 @@ +/* + Twitter Client with Strings + + This sketch connects to Twitter using an Ethernet shield. It parses the XML + returned, and looks for this is a tweet + + You can use the Arduino Ethernet shield, or the Adafruit Ethernet shield, + either one will work, as long as it's got a Wiznet Ethernet module on board. + + This example uses the DHCP routines in the Ethernet library which is part of the + Arduino core from version 1.0 beta 1 + + This example uses the String library, which is part of the Arduino core from + version 0019. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 21 May 2011 + modified 9 Apr 2012 + by Tom Igoe + + This code is in the public domain. + + */ +#include +#include + + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network: +byte mac[] = { + 0x00, 0xAA, 0xBB, 0xCC, 0xDE, 0x01 }; +IPAddress ip(192,168,1,20); + +// initialize the library instance: +EthernetClient client; + +const unsigned long requestInterval = 60000; // delay between requests + +char serverName[] = "api.twitter.com"; // twitter URL + +boolean requested; // whether you've made a request since connecting +unsigned long lastAttemptTime = 0; // last time you connected to the server, in milliseconds + +String currentLine = ""; // string to hold the text from server +String tweet = ""; // string to hold the tweet +boolean readingTweet = false; // if you're currently reading the tweet + +void setup() { + // reserve space for the strings: + currentLine.reserve(256); + tweet.reserve(150); + + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // attempt a DHCP connection: + Serial.println("Attempting to get an IP address using DHCP:"); + if (!Ethernet.begin(mac)) { + // if DHCP fails, start with a hard-coded address: + Serial.println("failed to get an IP address using DHCP, trying manually"); + Ethernet.begin(mac, ip); + } + Serial.print("My address:"); + Serial.println(Ethernet.localIP()); + // connect to Twitter: + connectToServer(); +} + + + +void loop() +{ + if (client.connected()) { + if (client.available()) { + // read incoming bytes: + char inChar = client.read(); + + // add incoming byte to end of line: + currentLine += inChar; + + // if you get a newline, clear the line: + if (inChar == '\n') { + currentLine = ""; + } + // if the current line ends with , it will + // be followed by the tweet: + if ( currentLine.endsWith("")) { + // tweet is beginning. Clear the tweet string: + readingTweet = true; + tweet = ""; + } + // if you're currently reading the bytes of a tweet, + // add them to the tweet String: + if (readingTweet) { + if (inChar != '<') { + tweet += inChar; + } + else { + // if you got a "<" character, + // you've reached the end of the tweet: + readingTweet = false; + Serial.println(tweet); + // close the connection to the server: + client.stop(); + } + } + } + } + else if (millis() - lastAttemptTime > requestInterval) { + // if you're not connected, and two minutes have passed since + // your last connection, then attempt to connect again: + connectToServer(); + } +} + +void connectToServer() { + // attempt to connect, and wait a millisecond: + Serial.println("connecting to server..."); + if (client.connect(serverName, 80)) { + Serial.println("making HTTP request..."); + // make HTTP GET request to twitter: + client.println("GET /1/statuses/user_timeline.xml?screen_name=arduino&count=1 HTTP/1.1"); + client.println("HOST: api.twitter.com"); + client.println(); + } + // note the time of this connect attempt: + lastAttemptTime = millis(); +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/UDPSendReceiveString/UDPSendReceiveString.ino b/libs/arduino-1.0/libraries/Ethernet/examples/UDPSendReceiveString/UDPSendReceiveString.ino new file mode 100644 index 0000000..4d4045c --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/UDPSendReceiveString/UDPSendReceiveString.ino @@ -0,0 +1,118 @@ +/* + UDPSendReceive.pde: + This sketch receives UDP message strings, prints them to the serial port + and sends an "acknowledge" string back to the sender + + A Processing sketch is included at the end of file that can be used to send + and received messages for testing with a computer. + + created 21 Aug 2010 + by Michael Margolis + + This code is in the public domain. + */ + + +#include // needed for Arduino versions later than 0018 +#include +#include // UDP library from: bjoern@cs.stanford.edu 12/30/2008 + + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; +IPAddress ip(192, 168, 1, 177); + +unsigned int localPort = 8888; // local port to listen on + +// buffers for receiving and sending data +char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet, +char ReplyBuffer[] = "acknowledged"; // a string to send back + +// An EthernetUDP instance to let us send and receive packets over UDP +EthernetUDP Udp; + +void setup() { + // start the Ethernet and UDP: + Ethernet.begin(mac,ip); + Udp.begin(localPort); + + Serial.begin(9600); +} + +void loop() { + // if there's data available, read a packet + int packetSize = Udp.parsePacket(); + if(packetSize) + { + Serial.print("Received packet of size "); + Serial.println(packetSize); + Serial.print("From "); + IPAddress remote = Udp.remoteIP(); + for (int i =0; i < 4; i++) + { + Serial.print(remote[i], DEC); + if (i < 3) + { + Serial.print("."); + } + } + Serial.print(", port "); + Serial.println(Udp.remotePort()); + + // read the packet into packetBufffer + Udp.read(packetBuffer,UDP_TX_PACKET_MAX_SIZE); + Serial.println("Contents:"); + Serial.println(packetBuffer); + + // send a reply, to the IP address and port that sent us the packet we received + Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); + Udp.write(ReplyBuffer); + Udp.endPacket(); + } + delay(10); +} + + +/* + Processing sketch to run with this example + ===================================================== + + // Processing UDP example to send and receive string data from Arduino + // press any key to send the "Hello Arduino" message + + + import hypermedia.net.*; + + UDP udp; // define the UDP object + + + void setup() { + udp = new UDP( this, 6000 ); // create a new datagram connection on port 6000 + //udp.log( true ); // <-- printout the connection activity + udp.listen( true ); // and wait for incoming message + } + + void draw() + { + } + + void keyPressed() { + String ip = "192.168.1.177"; // the remote IP address + int port = 8888; // the destination port + + udp.send("Hello World", ip, port ); // the message to send + + } + + void receive( byte[] data ) { // <-- default handler + //void receive( byte[] data, String ip, int port ) { // <-- extended handler + + for(int i=0; i < data.length; i++) + print(char(data[i])); + println(); + } + */ + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/UdpNtpClient/UdpNtpClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/UdpNtpClient/UdpNtpClient.ino new file mode 100644 index 0000000..6b3b53d --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/UdpNtpClient/UdpNtpClient.ino @@ -0,0 +1,150 @@ +/* + + Udp NTP Client + + Get the time from a Network Time Protocol (NTP) time server + Demonstrates use of UDP sendPacket and ReceivePacket + For more on NTP time servers and the messages needed to communicate with them, + see http://en.wikipedia.org/wiki/Network_Time_Protocol + + Warning: NTP Servers are subject to temporary failure or IP address change. + Plese check + + http://tf.nist.gov/tf-cgi/servers.cgi + + if the time server used in the example didn't work. + + created 4 Sep 2010 + by Michael Margolis + modified 9 Apr 2012 + by Tom Igoe + + This code is in the public domain. + + */ + +#include +#include +#include + +// Enter a MAC address for your controller below. +// Newer Ethernet shields have a MAC address printed on a sticker on the shield +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; + +unsigned int localPort = 8888; // local port to listen for UDP packets + +IPAddress timeServer(132, 163, 4, 101); // time-a.timefreq.bldrdoc.gov NTP server +// IPAddress timeServer(132, 163, 4, 102); // time-b.timefreq.bldrdoc.gov NTP server +// IPAddress timeServer(132, 163, 4, 103); // time-c.timefreq.bldrdoc.gov NTP server + +const int NTP_PACKET_SIZE= 48; // NTP time stamp is in the first 48 bytes of the message + +byte packetBuffer[ NTP_PACKET_SIZE]; //buffer to hold incoming and outgoing packets + +// A UDP instance to let us send and receive packets over UDP +EthernetUDP Udp; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // start Ethernet and UDP + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // no point in carrying on, so do nothing forevermore: + for(;;) + ; + } + Udp.begin(localPort); +} + +void loop() +{ + sendNTPpacket(timeServer); // send an NTP packet to a time server + + // wait to see if a reply is available + delay(1000); + if ( Udp.parsePacket() ) { + // We've received a packet, read the data from it + Udp.read(packetBuffer,NTP_PACKET_SIZE); // read the packet into the buffer + + //the timestamp starts at byte 40 of the received packet and is four bytes, + // or two words, long. First, esxtract the two words: + + unsigned long highWord = word(packetBuffer[40], packetBuffer[41]); + unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]); + // combine the four bytes (two words) into a long integer + // this is NTP time (seconds since Jan 1 1900): + unsigned long secsSince1900 = highWord << 16 | lowWord; + Serial.print("Seconds since Jan 1 1900 = " ); + Serial.println(secsSince1900); + + // now convert NTP time into everyday time: + Serial.print("Unix time = "); + // Unix time starts on Jan 1 1970. In seconds, that's 2208988800: + const unsigned long seventyYears = 2208988800UL; + // subtract seventy years: + unsigned long epoch = secsSince1900 - seventyYears; + // print Unix time: + Serial.println(epoch); + + + // print the hour, minute and second: + Serial.print("The UTC time is "); // UTC is the time at Greenwich Meridian (GMT) + Serial.print((epoch % 86400L) / 3600); // print the hour (86400 equals secs per day) + Serial.print(':'); + if ( ((epoch % 3600) / 60) < 10 ) { + // In the first 10 minutes of each hour, we'll want a leading '0' + Serial.print('0'); + } + Serial.print((epoch % 3600) / 60); // print the minute (3600 equals secs per minute) + Serial.print(':'); + if ( (epoch % 60) < 10 ) { + // In the first 10 seconds of each minute, we'll want a leading '0' + Serial.print('0'); + } + Serial.println(epoch %60); // print the second + } + // wait ten seconds before asking for the time again + delay(10000); +} + +// send an NTP request to the time server at the given address +unsigned long sendNTPpacket(IPAddress& address) +{ + // set all bytes in the buffer to 0 + memset(packetBuffer, 0, NTP_PACKET_SIZE); + // Initialize values needed to form NTP request + // (see URL above for details on the packets) + packetBuffer[0] = 0b11100011; // LI, Version, Mode + packetBuffer[1] = 0; // Stratum, or type of clock + packetBuffer[2] = 6; // Polling Interval + packetBuffer[3] = 0xEC; // Peer Clock Precision + // 8 bytes of zero for Root Delay & Root Dispersion + packetBuffer[12] = 49; + packetBuffer[13] = 0x4E; + packetBuffer[14] = 49; + packetBuffer[15] = 52; + + // all NTP fields have been given values, now + // you can send a packet requesting a timestamp: + Udp.beginPacket(address, 123); //NTP requests are to port 123 + Udp.write(packetBuffer,NTP_PACKET_SIZE); + Udp.endPacket(); +} + + + + + + + + + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/WebClient/WebClient.ino b/libs/arduino-1.0/libraries/Ethernet/examples/WebClient/WebClient.ino new file mode 100644 index 0000000..5d5d7f2 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/WebClient/WebClient.ino @@ -0,0 +1,80 @@ +/* + Web client + + This sketch connects to a website (http://www.google.com) + using an Arduino Wiznet Ethernet shield. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 18 Dec 2009 + modified 9 Apr 2012 + by David A. Mellis + + */ + +#include +#include + +// Enter a MAC address for your controller below. +// Newer Ethernet shields have a MAC address printed on a sticker on the shield +byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; +IPAddress server(173,194,33,104); // Google + +// Initialize the Ethernet client library +// with the IP address and port of the server +// that you want to connect to (port 80 is default for HTTP): +EthernetClient client; + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // start the Ethernet connection: + if (Ethernet.begin(mac) == 0) { + Serial.println("Failed to configure Ethernet using DHCP"); + // no point in carrying on, so do nothing forevermore: + for(;;) + ; + } + // give the Ethernet shield a second to initialize: + delay(1000); + Serial.println("connecting..."); + + // if you get a connection, report back via serial: + if (client.connect(server, 80)) { + Serial.println("connected"); + // Make a HTTP request: + client.println("GET /search?q=arduino HTTP/1.0"); + client.println(); + } + else { + // kf you didn't get a connection to the server: + Serial.println("connection failed"); + } +} + +void loop() +{ + // if there are incoming bytes available + // from the server, read them and print them: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if the server's disconnected, stop the client: + if (!client.connected()) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + + // do nothing forevermore: + for(;;) + ; + } +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/WebClientRepeating/WebClientRepeating.ino b/libs/arduino-1.0/libraries/Ethernet/examples/WebClientRepeating/WebClientRepeating.ino new file mode 100644 index 0000000..e0f06c4 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/WebClientRepeating/WebClientRepeating.ino @@ -0,0 +1,110 @@ +/* + Repeating Web client + + This sketch connects to a a web server and makes a request + using a Wiznet Ethernet shield. You can use the Arduino Ethernet shield, or + the Adafruit Ethernet shield, either one will work, as long as it's got + a Wiznet Ethernet module on board. + + This example uses DNS, by assigning the Ethernet client with a MAC address, + IP address, and DNS address. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + + created 19 Apr 2012 + by Tom Igoe + + http://arduino.cc/en/Tutorial/WebClientRepeating + This code is in the public domain. + + */ + +#include +#include + +// assign a MAC address for the ethernet controller. +// fill in your address here: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED}; +// fill in an available IP address on your network here, +// for manual configuration: +IPAddress ip(10,0,0,20); + +// fill in your Domain Name Server address here: +IPAddress myDns(1,1,1,1); + +// initialize the library instance: +EthernetClient client; + +char server[] = "www.arduino.cc"; + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 60*1000; // delay between updates, in milliseconds + +void setup() { + // start serial port: + Serial.begin(9600); + // give the ethernet module time to boot up: + delay(1000); + // start the Ethernet connection using a fixed IP address and DNS server: + Ethernet.begin(mac, ip, myDns); + // print the Ethernet board/shield's IP address: + Serial.print("My IP address: "); + Serial.println(Ethernet.localIP()); +} + +void loop() { + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + if (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + httpRequest(); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void httpRequest() { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.println("GET /latest.txt HTTP/1.1"); + client.println("Host: www.arduino.cc"); + client.println("User-Agent: arduino-ethernet"); + client.println("Connection: close"); + client.println(); + + // note the time that the connection was made: + lastConnectionTime = millis(); + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println("disconnecting."); + client.stop(); + } +} + + + + diff --git a/libs/arduino-1.0/libraries/Ethernet/examples/WebServer/WebServer.ino b/libs/arduino-1.0/libraries/Ethernet/examples/WebServer/WebServer.ino new file mode 100644 index 0000000..ce8dbb1 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/examples/WebServer/WebServer.ino @@ -0,0 +1,101 @@ +/* + Web Server + + A simple web server that shows the value of the analog input pins. + using an Arduino Wiznet Ethernet shield. + + Circuit: + * Ethernet shield attached to pins 10, 11, 12, 13 + * Analog inputs attached to pins A0 through A5 (optional) + + created 18 Dec 2009 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe + + */ + +#include +#include + +// Enter a MAC address and IP address for your controller below. +// The IP address will be dependent on your local network: +byte mac[] = { + 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; +IPAddress ip(192,168,1, 177); + +// Initialize the Ethernet server library +// with the IP address and port you want to use +// (port 80 is default for HTTP): +EthernetServer server(80); + +void setup() { + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // start the Ethernet connection and the server: + Ethernet.begin(mac, ip); + server.begin(); + Serial.print("server is at "); + Serial.println(Ethernet.localIP()); +} + + +void loop() { + // listen for incoming clients + EthernetClient client = server.available(); + if (client) { + Serial.println("new client"); + // an http request ends with a blank line + boolean currentLineIsBlank = true; + while (client.connected()) { + if (client.available()) { + char c = client.read(); + Serial.write(c); + // if you've gotten to the end of the line (received a newline + // character) and the line is blank, the http request has ended, + // so you can send a reply + if (c == '\n' && currentLineIsBlank) { + // send a standard http response header + client.println("HTTP/1.1 200 OK"); + client.println("Content-Type: text/html"); + client.println("Connnection: close"); + client.println(); + client.println(""); + client.println(""); + // add a meta refresh tag, so the browser pulls again every 5 seconds: + client.println(""); + // output the value of each analog input pin + for (int analogChannel = 0; analogChannel < 6; analogChannel++) { + int sensorReading = analogRead(analogChannel); + client.print("analog input "); + client.print(analogChannel); + client.print(" is "); + client.print(sensorReading); + client.println("
"); + } + client.println(""); + break; + } + if (c == '\n') { + // you're starting a new line + currentLineIsBlank = true; + } + else if (c != '\r') { + // you've gotten a character on the current line + currentLineIsBlank = false; + } + } + } + // give the web browser time to receive the data + delay(1); + // close the connection: + client.stop(); + Serial.println("client disonnected"); + } +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/keywords.txt b/libs/arduino-1.0/libraries/Ethernet/keywords.txt new file mode 100644 index 0000000..6b37cbe --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/keywords.txt @@ -0,0 +1,37 @@ +####################################### +# Syntax Coloring Map For Ethernet +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +Ethernet KEYWORD1 +EthernetClient KEYWORD1 +EthernetServer KEYWORD1 +IPAddress KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +status KEYWORD2 +connect KEYWORD2 +write KEYWORD2 +available KEYWORD2 +read KEYWORD2 +peek KEYWORD2 +flush KEYWORD2 +stop KEYWORD2 +connected KEYWORD2 +begin KEYWORD2 +beginPacket KEYWORD2 +endPacket KEYWORD2 +parsePacket KEYWORD2 +remoteIP KEYWORD2 +remotePort KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/Ethernet/util.h b/libs/arduino-1.0/libraries/Ethernet/util.h new file mode 100644 index 0000000..5042e82 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/util.h @@ -0,0 +1,13 @@ +#ifndef UTIL_H +#define UTIL_H + +#define htons(x) ( ((x)<<8) | (((x)>>8)&0xFF) ) +#define ntohs(x) htons(x) + +#define htonl(x) ( ((x)<<24 & 0xFF000000UL) | \ + ((x)<< 8 & 0x00FF0000UL) | \ + ((x)>> 8 & 0x0000FF00UL) | \ + ((x)>>24 & 0x000000FFUL) ) +#define ntohl(x) htonl(x) + +#endif diff --git a/libs/arduino-1.0/libraries/Ethernet/utility/socket.cpp b/libs/arduino-1.0/libraries/Ethernet/utility/socket.cpp new file mode 100644 index 0000000..fd3e442 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/utility/socket.cpp @@ -0,0 +1,400 @@ +#include "w5100.h" +#include "socket.h" + +static uint16_t local_port; + +/** + * @brief This Socket function initialize the channel in perticular mode, and set the port and wait for W5100 done it. + * @return 1 for success else 0. + */ +uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag) +{ + if ((protocol == SnMR::TCP) || (protocol == SnMR::UDP) || (protocol == SnMR::IPRAW) || (protocol == SnMR::MACRAW) || (protocol == SnMR::PPPOE)) + { + close(s); + W5100.writeSnMR(s, protocol | flag); + if (port != 0) { + W5100.writeSnPORT(s, port); + } + else { + local_port++; // if don't set the source port, set local_port number. + W5100.writeSnPORT(s, local_port); + } + + W5100.execCmdSn(s, Sock_OPEN); + + return 1; + } + + return 0; +} + + +/** + * @brief This function close the socket and parameter is "s" which represent the socket number + */ +void close(SOCKET s) +{ + W5100.execCmdSn(s, Sock_CLOSE); + W5100.writeSnIR(s, 0xFF); +} + + +/** + * @brief This function established the connection for the channel in passive (server) mode. This function waits for the request from the peer. + * @return 1 for success else 0. + */ +uint8_t listen(SOCKET s) +{ + if (W5100.readSnSR(s) != SnSR::INIT) + return 0; + W5100.execCmdSn(s, Sock_LISTEN); + return 1; +} + + +/** + * @brief This function established the connection for the channel in Active (client) mode. + * This function waits for the untill the connection is established. + * + * @return 1 for success else 0. + */ +uint8_t connect(SOCKET s, uint8_t * addr, uint16_t port) +{ + if + ( + ((addr[0] == 0xFF) && (addr[1] == 0xFF) && (addr[2] == 0xFF) && (addr[3] == 0xFF)) || + ((addr[0] == 0x00) && (addr[1] == 0x00) && (addr[2] == 0x00) && (addr[3] == 0x00)) || + (port == 0x00) + ) + return 0; + + // set destination IP + W5100.writeSnDIPR(s, addr); + W5100.writeSnDPORT(s, port); + W5100.execCmdSn(s, Sock_CONNECT); + + return 1; +} + + + +/** + * @brief This function used for disconnect the socket and parameter is "s" which represent the socket number + * @return 1 for success else 0. + */ +void disconnect(SOCKET s) +{ + W5100.execCmdSn(s, Sock_DISCON); +} + + +/** + * @brief This function used to send the data in TCP mode + * @return 1 for success else 0. + */ +uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len) +{ + uint8_t status=0; + uint16_t ret=0; + uint16_t freesize=0; + + if (len > W5100.SSIZE) + ret = W5100.SSIZE; // check size not to exceed MAX size. + else + ret = len; + + // if freebuf is available, start. + do + { + freesize = W5100.getTXFreeSize(s); + status = W5100.readSnSR(s); + if ((status != SnSR::ESTABLISHED) && (status != SnSR::CLOSE_WAIT)) + { + ret = 0; + break; + } + } + while (freesize < ret); + + // copy data + W5100.send_data_processing(s, (uint8_t *)buf, ret); + W5100.execCmdSn(s, Sock_SEND); + + /* +2008.01 bj */ + while ( (W5100.readSnIR(s) & SnIR::SEND_OK) != SnIR::SEND_OK ) + { + /* m2008.01 [bj] : reduce code */ + if ( W5100.readSnSR(s) == SnSR::CLOSED ) + { + close(s); + return 0; + } + } + /* +2008.01 bj */ + W5100.writeSnIR(s, SnIR::SEND_OK); + return ret; +} + + +/** + * @brief This function is an application I/F function which is used to receive the data in TCP mode. + * It continues to wait for data as much as the application wants to receive. + * + * @return received data size for success else -1. + */ +int16_t recv(SOCKET s, uint8_t *buf, int16_t len) +{ + // Check how much data is available + int16_t ret = W5100.getRXReceivedSize(s); + if ( ret == 0 ) + { + // No data available. + uint8_t status = W5100.readSnSR(s); + if ( status == SnSR::LISTEN || status == SnSR::CLOSED || status == SnSR::CLOSE_WAIT ) + { + // The remote end has closed its side of the connection, so this is the eof state + ret = 0; + } + else + { + // The connection is still up, but there's no data waiting to be read + ret = -1; + } + } + else if (ret > len) + { + ret = len; + } + + if ( ret > 0 ) + { + W5100.recv_data_processing(s, buf, ret); + W5100.execCmdSn(s, Sock_RECV); + } + return ret; +} + + +/** + * @brief Returns the first byte in the receive queue (no checking) + * + * @return + */ +uint16_t peek(SOCKET s, uint8_t *buf) +{ + W5100.recv_data_processing(s, buf, 1, 1); + + return 1; +} + + +/** + * @brief This function is an application I/F function which is used to send the data for other then TCP mode. + * Unlike TCP transmission, The peer's destination address and the port is needed. + * + * @return This function return send data size for success else -1. + */ +uint16_t sendto(SOCKET s, const uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t port) +{ + uint16_t ret=0; + + if (len > W5100.SSIZE) ret = W5100.SSIZE; // check size not to exceed MAX size. + else ret = len; + + if + ( + ((addr[0] == 0x00) && (addr[1] == 0x00) && (addr[2] == 0x00) && (addr[3] == 0x00)) || + ((port == 0x00)) ||(ret == 0) + ) + { + /* +2008.01 [bj] : added return value */ + ret = 0; + } + else + { + W5100.writeSnDIPR(s, addr); + W5100.writeSnDPORT(s, port); + + // copy data + W5100.send_data_processing(s, (uint8_t *)buf, ret); + W5100.execCmdSn(s, Sock_SEND); + + /* +2008.01 bj */ + while ( (W5100.readSnIR(s) & SnIR::SEND_OK) != SnIR::SEND_OK ) + { + if (W5100.readSnIR(s) & SnIR::TIMEOUT) + { + /* +2008.01 [bj]: clear interrupt */ + W5100.writeSnIR(s, (SnIR::SEND_OK | SnIR::TIMEOUT)); /* clear SEND_OK & TIMEOUT */ + return 0; + } + } + + /* +2008.01 bj */ + W5100.writeSnIR(s, SnIR::SEND_OK); + } + return ret; +} + + +/** + * @brief This function is an application I/F function which is used to receive the data in other then + * TCP mode. This function is used to receive UDP, IP_RAW and MAC_RAW mode, and handle the header as well. + * + * @return This function return received data size for success else -1. + */ +uint16_t recvfrom(SOCKET s, uint8_t *buf, uint16_t len, uint8_t *addr, uint16_t *port) +{ + uint8_t head[8]; + uint16_t data_len=0; + uint16_t ptr=0; + + if ( len > 0 ) + { + ptr = W5100.readSnRX_RD(s); + switch (W5100.readSnMR(s) & 0x07) + { + case SnMR::UDP : + W5100.read_data(s, (uint8_t *)ptr, head, 0x08); + ptr += 8; + // read peer's IP address, port number. + addr[0] = head[0]; + addr[1] = head[1]; + addr[2] = head[2]; + addr[3] = head[3]; + *port = head[4]; + *port = (*port << 8) + head[5]; + data_len = head[6]; + data_len = (data_len << 8) + head[7]; + + W5100.read_data(s, (uint8_t *)ptr, buf, data_len); // data copy. + ptr += data_len; + + W5100.writeSnRX_RD(s, ptr); + break; + + case SnMR::IPRAW : + W5100.read_data(s, (uint8_t *)ptr, head, 0x06); + ptr += 6; + + addr[0] = head[0]; + addr[1] = head[1]; + addr[2] = head[2]; + addr[3] = head[3]; + data_len = head[4]; + data_len = (data_len << 8) + head[5]; + + W5100.read_data(s, (uint8_t *)ptr, buf, data_len); // data copy. + ptr += data_len; + + W5100.writeSnRX_RD(s, ptr); + break; + + case SnMR::MACRAW: + W5100.read_data(s,(uint8_t*)ptr,head,2); + ptr+=2; + data_len = head[0]; + data_len = (data_len<<8) + head[1] - 2; + + W5100.read_data(s,(uint8_t*) ptr,buf,data_len); + ptr += data_len; + W5100.writeSnRX_RD(s, ptr); + break; + + default : + break; + } + W5100.execCmdSn(s, Sock_RECV); + } + return data_len; +} + + +uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len) +{ + uint8_t status=0; + uint16_t ret=0; + + if (len > W5100.SSIZE) + ret = W5100.SSIZE; // check size not to exceed MAX size. + else + ret = len; + + if (ret == 0) + return 0; + + W5100.send_data_processing(s, (uint8_t *)buf, ret); + W5100.execCmdSn(s, Sock_SEND); + + while ( (W5100.readSnIR(s) & SnIR::SEND_OK) != SnIR::SEND_OK ) + { + status = W5100.readSnSR(s); + if (W5100.readSnIR(s) & SnIR::TIMEOUT) + { + /* in case of igmp, if send fails, then socket closed */ + /* if you want change, remove this code. */ + close(s); + return 0; + } + } + + W5100.writeSnIR(s, SnIR::SEND_OK); + return ret; +} + +uint16_t bufferData(SOCKET s, uint16_t offset, const uint8_t* buf, uint16_t len) +{ + uint16_t ret =0; + if (len > W5100.getTXFreeSize(s)) + { + ret = W5100.getTXFreeSize(s); // check size not to exceed MAX size. + } + else + { + ret = len; + } + W5100.send_data_processing_offset(s, offset, buf, ret); + return ret; +} + +int startUDP(SOCKET s, uint8_t* addr, uint16_t port) +{ + if + ( + ((addr[0] == 0x00) && (addr[1] == 0x00) && (addr[2] == 0x00) && (addr[3] == 0x00)) || + ((port == 0x00)) + ) + { + return 0; + } + else + { + W5100.writeSnDIPR(s, addr); + W5100.writeSnDPORT(s, port); + return 1; + } +} + +int sendUDP(SOCKET s) +{ + W5100.execCmdSn(s, Sock_SEND); + + /* +2008.01 bj */ + while ( (W5100.readSnIR(s) & SnIR::SEND_OK) != SnIR::SEND_OK ) + { + if (W5100.readSnIR(s) & SnIR::TIMEOUT) + { + /* +2008.01 [bj]: clear interrupt */ + W5100.writeSnIR(s, (SnIR::SEND_OK|SnIR::TIMEOUT)); + return 0; + } + } + + /* +2008.01 bj */ + W5100.writeSnIR(s, SnIR::SEND_OK); + + /* Sent ok */ + return 1; +} + diff --git a/libs/arduino-1.0/libraries/Ethernet/utility/socket.h b/libs/arduino-1.0/libraries/Ethernet/utility/socket.h new file mode 100644 index 0000000..45e0fb3 --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/utility/socket.h @@ -0,0 +1,41 @@ +#ifndef _SOCKET_H_ +#define _SOCKET_H_ + +#include "w5100.h" + +extern uint8_t socket(SOCKET s, uint8_t protocol, uint16_t port, uint8_t flag); // Opens a socket(TCP or UDP or IP_RAW mode) +extern void close(SOCKET s); // Close socket +extern uint8_t connect(SOCKET s, uint8_t * addr, uint16_t port); // Establish TCP connection (Active connection) +extern void disconnect(SOCKET s); // disconnect the connection +extern uint8_t listen(SOCKET s); // Establish TCP connection (Passive connection) +extern uint16_t send(SOCKET s, const uint8_t * buf, uint16_t len); // Send data (TCP) +extern int16_t recv(SOCKET s, uint8_t * buf, int16_t len); // Receive data (TCP) +extern uint16_t peek(SOCKET s, uint8_t *buf); +extern uint16_t sendto(SOCKET s, const uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t port); // Send data (UDP/IP RAW) +extern uint16_t recvfrom(SOCKET s, uint8_t * buf, uint16_t len, uint8_t * addr, uint16_t *port); // Receive data (UDP/IP RAW) + +extern uint16_t igmpsend(SOCKET s, const uint8_t * buf, uint16_t len); + +// Functions to allow buffered UDP send (i.e. where the UDP datagram is built up over a +// number of calls before being sent +/* + @brief This function sets up a UDP datagram, the data for which will be provided by one + or more calls to bufferData and then finally sent with sendUDP. + @return 1 if the datagram was successfully set up, or 0 if there was an error +*/ +extern int startUDP(SOCKET s, uint8_t* addr, uint16_t port); +/* + @brief This function copies up to len bytes of data from buf into a UDP datagram to be + sent later by sendUDP. Allows datagrams to be built up from a series of bufferData calls. + @return Number of bytes successfully buffered +*/ +uint16_t bufferData(SOCKET s, uint16_t offset, const uint8_t* buf, uint16_t len); +/* + @brief Send a UDP datagram built up from a sequence of startUDP followed by one or more + calls to bufferData. + @return 1 if the datagram was successfully sent, or 0 if there was an error +*/ +int sendUDP(SOCKET s); + +#endif +/* _SOCKET_H_ */ diff --git a/libs/arduino-1.0/libraries/Ethernet/utility/w5100.cpp b/libs/arduino-1.0/libraries/Ethernet/utility/w5100.cpp new file mode 100644 index 0000000..9c748fd --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/utility/w5100.cpp @@ -0,0 +1,188 @@ +/* + * Copyright (c) 2010 by Cristian Maglie + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#include +#include +#include + +#include "w5100.h" + +// W5100 controller instance +W5100Class W5100; + +#define TX_RX_MAX_BUF_SIZE 2048 +#define TX_BUF 0x1100 +#define RX_BUF (TX_BUF + TX_RX_MAX_BUF_SIZE) + +#define TXBUF_BASE 0x4000 +#define RXBUF_BASE 0x6000 + +void W5100Class::init(void) +{ + delay(300); + + SPI.begin(); + initSS(); + + writeMR(1< SSIZE) + { + // Wrap around circular buffer + uint16_t size = SSIZE - offset; + write(dstAddr, data, size); + write(SBASE[s], data + size, len - size); + } + else { + write(dstAddr, data, len); + } + + ptr += len; + writeSnTX_WR(s, ptr); +} + + +void W5100Class::recv_data_processing(SOCKET s, uint8_t *data, uint16_t len, uint8_t peek) +{ + uint16_t ptr; + ptr = readSnRX_RD(s); + read_data(s, (uint8_t *)ptr, data, len); + if (!peek) + { + ptr += len; + writeSnRX_RD(s, ptr); + } +} + +void W5100Class::read_data(SOCKET s, volatile uint8_t *src, volatile uint8_t *dst, uint16_t len) +{ + uint16_t size; + uint16_t src_mask; + uint16_t src_ptr; + + src_mask = (uint16_t)src & RMASK; + src_ptr = RBASE[s] + src_mask; + + if( (src_mask + len) > RSIZE ) + { + size = RSIZE - src_mask; + read(src_ptr, (uint8_t *)dst, size); + dst += size; + read(RBASE[s], (uint8_t *) dst, len - size); + } + else + read(src_ptr, (uint8_t *) dst, len); +} + + +uint8_t W5100Class::write(uint16_t _addr, uint8_t _data) +{ + setSS(); + SPI.transfer(0xF0); + SPI.transfer(_addr >> 8); + SPI.transfer(_addr & 0xFF); + SPI.transfer(_data); + resetSS(); + return 1; +} + +uint16_t W5100Class::write(uint16_t _addr, const uint8_t *_buf, uint16_t _len) +{ + for (uint16_t i=0; i<_len; i++) + { + setSS(); + SPI.transfer(0xF0); + SPI.transfer(_addr >> 8); + SPI.transfer(_addr & 0xFF); + _addr++; + SPI.transfer(_buf[i]); + resetSS(); + } + return _len; +} + +uint8_t W5100Class::read(uint16_t _addr) +{ + setSS(); + SPI.transfer(0x0F); + SPI.transfer(_addr >> 8); + SPI.transfer(_addr & 0xFF); + uint8_t _data = SPI.transfer(0); + resetSS(); + return _data; +} + +uint16_t W5100Class::read(uint16_t _addr, uint8_t *_buf, uint16_t _len) +{ + for (uint16_t i=0; i<_len; i++) + { + setSS(); + SPI.transfer(0x0F); + SPI.transfer(_addr >> 8); + SPI.transfer(_addr & 0xFF); + _addr++; + _buf[i] = SPI.transfer(0); + resetSS(); + } + return _len; +} + +void W5100Class::execCmdSn(SOCKET s, SockCMD _cmd) { + // Send command to socket + writeSnCR(s, _cmd); + // Wait for command to complete + while (readSnCR(s)) + ; +} diff --git a/libs/arduino-1.0/libraries/Ethernet/utility/w5100.h b/libs/arduino-1.0/libraries/Ethernet/utility/w5100.h new file mode 100644 index 0000000..8dccd9f --- /dev/null +++ b/libs/arduino-1.0/libraries/Ethernet/utility/w5100.h @@ -0,0 +1,404 @@ +/* + * Copyright (c) 2010 by Cristian Maglie + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef W5100_H_INCLUDED +#define W5100_H_INCLUDED + +#include +#include + +#define MAX_SOCK_NUM 4 + +typedef uint8_t SOCKET; + +#define IDM_OR 0x8000 +#define IDM_AR0 0x8001 +#define IDM_AR1 0x8002 +#define IDM_DR 0x8003 +/* +class MR { +public: + static const uint8_t RST = 0x80; + static const uint8_t PB = 0x10; + static const uint8_t PPPOE = 0x08; + static const uint8_t LB = 0x04; + static const uint8_t AI = 0x02; + static const uint8_t IND = 0x01; +}; +*/ +/* +class IR { +public: + static const uint8_t CONFLICT = 0x80; + static const uint8_t UNREACH = 0x40; + static const uint8_t PPPoE = 0x20; + static const uint8_t SOCK0 = 0x01; + static const uint8_t SOCK1 = 0x02; + static const uint8_t SOCK2 = 0x04; + static const uint8_t SOCK3 = 0x08; + static inline uint8_t SOCK(SOCKET ch) { return (0x01 << ch); }; +}; +*/ + +class SnMR { +public: + static const uint8_t CLOSE = 0x00; + static const uint8_t TCP = 0x01; + static const uint8_t UDP = 0x02; + static const uint8_t IPRAW = 0x03; + static const uint8_t MACRAW = 0x04; + static const uint8_t PPPOE = 0x05; + static const uint8_t ND = 0x20; + static const uint8_t MULTI = 0x80; +}; + +enum SockCMD { + Sock_OPEN = 0x01, + Sock_LISTEN = 0x02, + Sock_CONNECT = 0x04, + Sock_DISCON = 0x08, + Sock_CLOSE = 0x10, + Sock_SEND = 0x20, + Sock_SEND_MAC = 0x21, + Sock_SEND_KEEP = 0x22, + Sock_RECV = 0x40 +}; + +/*class SnCmd { +public: + static const uint8_t OPEN = 0x01; + static const uint8_t LISTEN = 0x02; + static const uint8_t CONNECT = 0x04; + static const uint8_t DISCON = 0x08; + static const uint8_t CLOSE = 0x10; + static const uint8_t SEND = 0x20; + static const uint8_t SEND_MAC = 0x21; + static const uint8_t SEND_KEEP = 0x22; + static const uint8_t RECV = 0x40; +}; +*/ + +class SnIR { +public: + static const uint8_t SEND_OK = 0x10; + static const uint8_t TIMEOUT = 0x08; + static const uint8_t RECV = 0x04; + static const uint8_t DISCON = 0x02; + static const uint8_t CON = 0x01; +}; + +class SnSR { +public: + static const uint8_t CLOSED = 0x00; + static const uint8_t INIT = 0x13; + static const uint8_t LISTEN = 0x14; + static const uint8_t SYNSENT = 0x15; + static const uint8_t SYNRECV = 0x16; + static const uint8_t ESTABLISHED = 0x17; + static const uint8_t FIN_WAIT = 0x18; + static const uint8_t CLOSING = 0x1A; + static const uint8_t TIME_WAIT = 0x1B; + static const uint8_t CLOSE_WAIT = 0x1C; + static const uint8_t LAST_ACK = 0x1D; + static const uint8_t UDP = 0x22; + static const uint8_t IPRAW = 0x32; + static const uint8_t MACRAW = 0x42; + static const uint8_t PPPOE = 0x5F; +}; + +class IPPROTO { +public: + static const uint8_t IP = 0; + static const uint8_t ICMP = 1; + static const uint8_t IGMP = 2; + static const uint8_t GGP = 3; + static const uint8_t TCP = 6; + static const uint8_t PUP = 12; + static const uint8_t UDP = 17; + static const uint8_t IDP = 22; + static const uint8_t ND = 77; + static const uint8_t RAW = 255; +}; + +class W5100Class { + +public: + void init(); + + /** + * @brief This function is being used for copy the data form Receive buffer of the chip to application buffer. + * + * It calculate the actual physical address where one has to read + * the data from Receive buffer. Here also take care of the condition while it exceed + * the Rx memory uper-bound of socket. + */ + void read_data(SOCKET s, volatile uint8_t * src, volatile uint8_t * dst, uint16_t len); + + /** + * @brief This function is being called by send() and sendto() function also. + * + * This function read the Tx write pointer register and after copy the data in buffer update the Tx write pointer + * register. User should read upper byte first and lower byte later to get proper value. + */ + void send_data_processing(SOCKET s, const uint8_t *data, uint16_t len); + /** + * @brief A copy of send_data_processing that uses the provided ptr for the + * write offset. Only needed for the "streaming" UDP API, where + * a single UDP packet is built up over a number of calls to + * send_data_processing_ptr, because TX_WR doesn't seem to get updated + * correctly in those scenarios + * @param ptr value to use in place of TX_WR. If 0, then the value is read + * in from TX_WR + * @return New value for ptr, to be used in the next call + */ +// FIXME Update documentation + void send_data_processing_offset(SOCKET s, uint16_t data_offset, const uint8_t *data, uint16_t len); + + /** + * @brief This function is being called by recv() also. + * + * This function read the Rx read pointer register + * and after copy the data from receive buffer update the Rx write pointer register. + * User should read upper byte first and lower byte later to get proper value. + */ + void recv_data_processing(SOCKET s, uint8_t *data, uint16_t len, uint8_t peek = 0); + + inline void setGatewayIp(uint8_t *_addr); + inline void getGatewayIp(uint8_t *_addr); + + inline void setSubnetMask(uint8_t *_addr); + inline void getSubnetMask(uint8_t *_addr); + + inline void setMACAddress(uint8_t * addr); + inline void getMACAddress(uint8_t * addr); + + inline void setIPAddress(uint8_t * addr); + inline void getIPAddress(uint8_t * addr); + + inline void setRetransmissionTime(uint16_t timeout); + inline void setRetransmissionCount(uint8_t _retry); + + void execCmdSn(SOCKET s, SockCMD _cmd); + + uint16_t getTXFreeSize(SOCKET s); + uint16_t getRXReceivedSize(SOCKET s); + + + // W5100 Registers + // --------------- +private: + static uint8_t write(uint16_t _addr, uint8_t _data); + static uint16_t write(uint16_t addr, const uint8_t *buf, uint16_t len); + static uint8_t read(uint16_t addr); + static uint16_t read(uint16_t addr, uint8_t *buf, uint16_t len); + +#define __GP_REGISTER8(name, address) \ + static inline void write##name(uint8_t _data) { \ + write(address, _data); \ + } \ + static inline uint8_t read##name() { \ + return read(address); \ + } +#define __GP_REGISTER16(name, address) \ + static void write##name(uint16_t _data) { \ + write(address, _data >> 8); \ + write(address+1, _data & 0xFF); \ + } \ + static uint16_t read##name() { \ + uint16_t res = read(address); \ + res = (res << 8) + read(address + 1); \ + return res; \ + } +#define __GP_REGISTER_N(name, address, size) \ + static uint16_t write##name(uint8_t *_buff) { \ + return write(address, _buff, size); \ + } \ + static uint16_t read##name(uint8_t *_buff) { \ + return read(address, _buff, size); \ + } + +public: + __GP_REGISTER8 (MR, 0x0000); // Mode + __GP_REGISTER_N(GAR, 0x0001, 4); // Gateway IP address + __GP_REGISTER_N(SUBR, 0x0005, 4); // Subnet mask address + __GP_REGISTER_N(SHAR, 0x0009, 6); // Source MAC address + __GP_REGISTER_N(SIPR, 0x000F, 4); // Source IP address + __GP_REGISTER8 (IR, 0x0015); // Interrupt + __GP_REGISTER8 (IMR, 0x0016); // Interrupt Mask + __GP_REGISTER16(RTR, 0x0017); // Timeout address + __GP_REGISTER8 (RCR, 0x0019); // Retry count + __GP_REGISTER8 (RMSR, 0x001A); // Receive memory size + __GP_REGISTER8 (TMSR, 0x001B); // Transmit memory size + __GP_REGISTER8 (PATR, 0x001C); // Authentication type address in PPPoE mode + __GP_REGISTER8 (PTIMER, 0x0028); // PPP LCP Request Timer + __GP_REGISTER8 (PMAGIC, 0x0029); // PPP LCP Magic Number + __GP_REGISTER_N(UIPR, 0x002A, 4); // Unreachable IP address in UDP mode + __GP_REGISTER16(UPORT, 0x002E); // Unreachable Port address in UDP mode + +#undef __GP_REGISTER8 +#undef __GP_REGISTER16 +#undef __GP_REGISTER_N + + // W5100 Socket registers + // ---------------------- +private: + static inline uint8_t readSn(SOCKET _s, uint16_t _addr); + static inline uint8_t writeSn(SOCKET _s, uint16_t _addr, uint8_t _data); + static inline uint16_t readSn(SOCKET _s, uint16_t _addr, uint8_t *_buf, uint16_t len); + static inline uint16_t writeSn(SOCKET _s, uint16_t _addr, uint8_t *_buf, uint16_t len); + + static const uint16_t CH_BASE = 0x0400; + static const uint16_t CH_SIZE = 0x0100; + +#define __SOCKET_REGISTER8(name, address) \ + static inline void write##name(SOCKET _s, uint8_t _data) { \ + writeSn(_s, address, _data); \ + } \ + static inline uint8_t read##name(SOCKET _s) { \ + return readSn(_s, address); \ + } +#define __SOCKET_REGISTER16(name, address) \ + static void write##name(SOCKET _s, uint16_t _data) { \ + writeSn(_s, address, _data >> 8); \ + writeSn(_s, address+1, _data & 0xFF); \ + } \ + static uint16_t read##name(SOCKET _s) { \ + uint16_t res = readSn(_s, address); \ + uint16_t res2 = readSn(_s,address + 1); \ + res = res << 8; \ + res2 = res2 & 0xFF; \ + res = res | res2; \ + return res; \ + } +#define __SOCKET_REGISTER_N(name, address, size) \ + static uint16_t write##name(SOCKET _s, uint8_t *_buff) { \ + return writeSn(_s, address, _buff, size); \ + } \ + static uint16_t read##name(SOCKET _s, uint8_t *_buff) { \ + return readSn(_s, address, _buff, size); \ + } + +public: + __SOCKET_REGISTER8(SnMR, 0x0000) // Mode + __SOCKET_REGISTER8(SnCR, 0x0001) // Command + __SOCKET_REGISTER8(SnIR, 0x0002) // Interrupt + __SOCKET_REGISTER8(SnSR, 0x0003) // Status + __SOCKET_REGISTER16(SnPORT, 0x0004) // Source Port + __SOCKET_REGISTER_N(SnDHAR, 0x0006, 6) // Destination Hardw Addr + __SOCKET_REGISTER_N(SnDIPR, 0x000C, 4) // Destination IP Addr + __SOCKET_REGISTER16(SnDPORT, 0x0010) // Destination Port + __SOCKET_REGISTER16(SnMSSR, 0x0012) // Max Segment Size + __SOCKET_REGISTER8(SnPROTO, 0x0014) // Protocol in IP RAW Mode + __SOCKET_REGISTER8(SnTOS, 0x0015) // IP TOS + __SOCKET_REGISTER8(SnTTL, 0x0016) // IP TTL + __SOCKET_REGISTER16(SnTX_FSR, 0x0020) // TX Free Size + __SOCKET_REGISTER16(SnTX_RD, 0x0022) // TX Read Pointer + __SOCKET_REGISTER16(SnTX_WR, 0x0024) // TX Write Pointer + __SOCKET_REGISTER16(SnRX_RSR, 0x0026) // RX Free Size + __SOCKET_REGISTER16(SnRX_RD, 0x0028) // RX Read Pointer + __SOCKET_REGISTER16(SnRX_WR, 0x002A) // RX Write Pointer (supported?) + +#undef __SOCKET_REGISTER8 +#undef __SOCKET_REGISTER16 +#undef __SOCKET_REGISTER_N + + +private: + static const uint8_t RST = 7; // Reset BIT + + static const int SOCKETS = 4; + static const uint16_t SMASK = 0x07FF; // Tx buffer MASK + static const uint16_t RMASK = 0x07FF; // Rx buffer MASK +public: + static const uint16_t SSIZE = 2048; // Max Tx buffer size +private: + static const uint16_t RSIZE = 2048; // Max Rx buffer size + uint16_t SBASE[SOCKETS]; // Tx buffer base address + uint16_t RBASE[SOCKETS]; // Rx buffer base address + +private: +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + inline static void initSS() { DDRB |= _BV(4); }; + inline static void setSS() { PORTB &= ~_BV(4); }; + inline static void resetSS() { PORTB |= _BV(4); }; +#elif defined(__AVR_ATmega32U4__) + inline static void initSS() { DDRB |= _BV(6); }; + inline static void setSS() { PORTB &= ~_BV(6); }; + inline static void resetSS() { PORTB |= _BV(6); }; +#elif defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB162__) + inline static void initSS() { DDRB |= _BV(0); }; + inline static void setSS() { PORTB &= ~_BV(0); }; + inline static void resetSS() { PORTB |= _BV(0); }; +#else + inline static void initSS() { DDRB |= _BV(2); }; + inline static void setSS() { PORTB &= ~_BV(2); }; + inline static void resetSS() { PORTB |= _BV(2); }; +#endif + +}; + +extern W5100Class W5100; + +uint8_t W5100Class::readSn(SOCKET _s, uint16_t _addr) { + return read(CH_BASE + _s * CH_SIZE + _addr); +} + +uint8_t W5100Class::writeSn(SOCKET _s, uint16_t _addr, uint8_t _data) { + return write(CH_BASE + _s * CH_SIZE + _addr, _data); +} + +uint16_t W5100Class::readSn(SOCKET _s, uint16_t _addr, uint8_t *_buf, uint16_t _len) { + return read(CH_BASE + _s * CH_SIZE + _addr, _buf, _len); +} + +uint16_t W5100Class::writeSn(SOCKET _s, uint16_t _addr, uint8_t *_buf, uint16_t _len) { + return write(CH_BASE + _s * CH_SIZE + _addr, _buf, _len); +} + +void W5100Class::getGatewayIp(uint8_t *_addr) { + readGAR(_addr); +} + +void W5100Class::setGatewayIp(uint8_t *_addr) { + writeGAR(_addr); +} + +void W5100Class::getSubnetMask(uint8_t *_addr) { + readSUBR(_addr); +} + +void W5100Class::setSubnetMask(uint8_t *_addr) { + writeSUBR(_addr); +} + +void W5100Class::getMACAddress(uint8_t *_addr) { + readSHAR(_addr); +} + +void W5100Class::setMACAddress(uint8_t *_addr) { + writeSHAR(_addr); +} + +void W5100Class::getIPAddress(uint8_t *_addr) { + readSIPR(_addr); +} + +void W5100Class::setIPAddress(uint8_t *_addr) { + writeSIPR(_addr); +} + +void W5100Class::setRetransmissionTime(uint16_t _timeout) { + writeRTR(_timeout); +} + +void W5100Class::setRetransmissionCount(uint8_t _retry) { + writeRCR(_retry); +} + +#endif diff --git a/libs/arduino-1.0/libraries/Firmata/Boards.h b/libs/arduino-1.0/libraries/Firmata/Boards.h new file mode 100644 index 0000000..06f69c6 --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/Boards.h @@ -0,0 +1,366 @@ +/* Boards.h - Hardware Abstraction Layer for Firmata library */ + +#ifndef Firmata_Boards_h +#define Firmata_Boards_h + +#include + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" // for digitalRead, digitalWrite, etc +#else +#include "WProgram.h" +#endif + +// Normally Servo.h must be included before Firmata.h (which then includes +// this file). If Servo.h wasn't included, this allows the code to still +// compile, but without support for any Servos. Hopefully that's what the +// user intended by not including Servo.h +#ifndef MAX_SERVOS +#define MAX_SERVOS 0 +#endif + +/* + Firmata Hardware Abstraction Layer + +Firmata is built on top of the hardware abstraction functions of Arduino, +specifically digitalWrite, digitalRead, analogWrite, analogRead, and +pinMode. While these functions offer simple integer pin numbers, Firmata +needs more information than is provided by Arduino. This file provides +all other hardware specific details. To make Firmata support a new board, +only this file should require editing. + +The key concept is every "pin" implemented by Firmata may be mapped to +any pin as implemented by Arduino. Usually a simple 1-to-1 mapping is +best, but such mapping should not be assumed. This hardware abstraction +layer allows Firmata to implement any number of pins which map onto the +Arduino implemented pins in almost any arbitrary way. + + +General Constants: + +These constants provide basic information Firmata requires. + +TOTAL_PINS: The total number of pins Firmata implemented by Firmata. + Usually this will match the number of pins the Arduino functions + implement, including any pins pins capable of analog or digital. + However, Firmata may implement any number of pins. For example, + on Arduino Mini with 8 analog inputs, 6 of these may be used + for digital functions, and 2 are analog only. On such boards, + Firmata can implement more pins than Arduino's pinMode() + function, in order to accommodate those special pins. The + Firmata protocol supports a maximum of 128 pins, so this + constant must not exceed 128. + +TOTAL_ANALOG_PINS: The total number of analog input pins implemented. + The Firmata protocol allows up to 16 analog inputs, accessed + using offsets 0 to 15. Because Firmata presents the analog + inputs using different offsets than the actual pin numbers + (a legacy of Arduino's analogRead function, and the way the + analog input capable pins are physically labeled on all + Arduino boards), the total number of analog input signals + must be specified. 16 is the maximum. + +VERSION_BLINK_PIN: When Firmata starts up, it will blink the version + number. This constant is the Arduino pin number where a + LED is connected. + + +Pin Mapping Macros: + +These macros provide the mapping between pins as implemented by +Firmata protocol and the actual pin numbers used by the Arduino +functions. Even though such mappings are often simple, pin +numbers received by Firmata protocol should always be used as +input to these macros, and the result of the macro should be +used with with any Arduino function. + +When Firmata is extended to support a new pin mode or feature, +a pair of macros should be added and used for all hardware +access. For simple 1:1 mapping, these macros add no actual +overhead, yet their consistent use allows source code which +uses them consistently to be easily adapted to all other boards +with different requirements. + +IS_PIN_XXXX(pin): The IS_PIN macros resolve to true or non-zero + if a pin as implemented by Firmata corresponds to a pin + that actually implements the named feature. + +PIN_TO_XXXX(pin): The PIN_TO macros translate pin numbers as + implemented by Firmata to the pin numbers needed as inputs + to the Arduino functions. The corresponding IS_PIN macro + should always be tested before using a PIN_TO macro, so + these macros only need to handle valid Firmata pin + numbers for the named feature. + + +Port Access Inline Funtions: + +For efficiency, Firmata protocol provides access to digital +input and output pins grouped by 8 bit ports. When these +groups of 8 correspond to actual 8 bit ports as implemented +by the hardware, these inline functions can provide high +speed direct port access. Otherwise, a default implementation +using 8 calls to digitalWrite or digitalRead is used. + +When porting Firmata to a new board, it is recommended to +use the default functions first and focus only on the constants +and macros above. When those are working, if optimized port +access is desired, these inline functions may be extended. +The recommended approach defines a symbol indicating which +optimization to use, and then conditional complication is +used within these functions. + +readPort(port, bitmask): Read an 8 bit port, returning the value. + port: The port number, Firmata pins port*8 to port*8+7 + bitmask: The actual pins to read, indicated by 1 bits. + +writePort(port, value, bitmask): Write an 8 bit port. + port: The port number, Firmata pins port*8 to port*8+7 + value: The 8 bit value to write + bitmask: The actual pins to write, indicated by 1 bits. +*/ + +/*============================================================================== + * Board Specific Configuration + *============================================================================*/ + +#ifndef digitalPinHasPWM +#define digitalPinHasPWM(p) IS_PIN_DIGITAL(p) +#endif + +// Arduino Duemilanove, Diecimila, and NG +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) +#if defined(NUM_ANALOG_INPUTS) && NUM_ANALOG_INPUTS == 6 +#define TOTAL_ANALOG_PINS 6 +#define TOTAL_PINS 20 // 14 digital + 6 analog +#else +#define TOTAL_ANALOG_PINS 8 +#define TOTAL_PINS 22 // 14 digital + 8 analog +#endif +#define VERSION_BLINK_PIN 13 +#define IS_PIN_DIGITAL(p) ((p) >= 2 && (p) <= 19) +#define IS_PIN_ANALOG(p) ((p) >= 14 && (p) < 14 + TOTAL_ANALOG_PINS) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 18 || (p) == 19) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 14) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) ((p) - 2) +#define ARDUINO_PINOUT_OPTIMIZE 1 + + +// Wiring (and board) +#elif defined(WIRING) +#define VERSION_BLINK_PIN WLED +#define IS_PIN_DIGITAL(p) ((p) >= 0 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= FIRST_ANALOG_PIN && (p) < (FIRST_ANALOG_PIN+TOTAL_ANALOG_PINS)) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == SDA || (p) == SCL) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - FIRST_ANALOG_PIN) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) (p) + + +// old Arduinos +#elif defined(__AVR_ATmega8__) +#define TOTAL_ANALOG_PINS 6 +#define TOTAL_PINS 20 // 14 digital + 6 analog +#define VERSION_BLINK_PIN 13 +#define IS_PIN_DIGITAL(p) ((p) >= 2 && (p) <= 19) +#define IS_PIN_ANALOG(p) ((p) >= 14 && (p) <= 19) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 18 || (p) == 19) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 14) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) ((p) - 2) +#define ARDUINO_PINOUT_OPTIMIZE 1 + + +// Arduino Mega +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define TOTAL_ANALOG_PINS 16 +#define TOTAL_PINS 70 // 54 digital + 16 analog +#define VERSION_BLINK_PIN 13 +#define IS_PIN_DIGITAL(p) ((p) >= 2 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= 54 && (p) < TOTAL_PINS) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 2 && (p) - 2 < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 20 || (p) == 21) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 54) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) ((p) - 2) + + +// Teensy 1.0 +#elif defined(__AVR_AT90USB162__) +#define TOTAL_ANALOG_PINS 0 +#define TOTAL_PINS 21 // 21 digital + no analog +#define VERSION_BLINK_PIN 6 +#define IS_PIN_DIGITAL(p) ((p) >= 0 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) (0) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) (0) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) (0) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) (p) + + +// Teensy 2.0 +#elif defined(__AVR_ATmega32U4__) +#define TOTAL_ANALOG_PINS 12 +#define TOTAL_PINS 25 // 11 digital + 12 analog +#define VERSION_BLINK_PIN 11 +#define IS_PIN_DIGITAL(p) ((p) >= 0 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= 11 && (p) <= 22) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 5 || (p) == 6) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) (((p)<22)?21-(p):11) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) (p) + + +// Teensy++ 1.0 and 2.0 +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) +#define TOTAL_ANALOG_PINS 8 +#define TOTAL_PINS 46 // 38 digital + 8 analog +#define VERSION_BLINK_PIN 6 +#define IS_PIN_DIGITAL(p) ((p) >= 0 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= 38 && (p) < TOTAL_PINS) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 0 || (p) == 1) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 38) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) (p) + + +// Sanguino +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) +#define TOTAL_ANALOG_PINS 8 +#define TOTAL_PINS 32 // 24 digital + 8 analog +#define VERSION_BLINK_PIN 0 +#define IS_PIN_DIGITAL(p) ((p) >= 2 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= 24 && (p) < TOTAL_PINS) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 16 || (p) == 17) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 24) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) ((p) - 2) + + +// Illuminato +#elif defined(__AVR_ATmega645__) +#define TOTAL_ANALOG_PINS 6 +#define TOTAL_PINS 42 // 36 digital + 6 analog +#define VERSION_BLINK_PIN 13 +#define IS_PIN_DIGITAL(p) ((p) >= 2 && (p) < TOTAL_PINS) +#define IS_PIN_ANALOG(p) ((p) >= 36 && (p) < TOTAL_PINS) +#define IS_PIN_PWM(p) digitalPinHasPWM(p) +#define IS_PIN_SERVO(p) ((p) >= 0 && (p) < MAX_SERVOS) +#define IS_PIN_I2C(p) ((p) == 4 || (p) == 5) +#define PIN_TO_DIGITAL(p) (p) +#define PIN_TO_ANALOG(p) ((p) - 36) +#define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) +#define PIN_TO_SERVO(p) ((p) - 2) + + +// anything else +#else +#error "Please edit Boards.h with a hardware abstraction for this board" +#endif + + +/*============================================================================== + * readPort() - Read an 8 bit port + *============================================================================*/ + +static inline unsigned char readPort(byte, byte) __attribute__((always_inline, unused)); +static inline unsigned char readPort(byte port, byte bitmask) +{ +#if defined(ARDUINO_PINOUT_OPTIMIZE) + if (port == 0) return (PIND & 0xFC) & bitmask; // ignore Rx/Tx 0/1 + if (port == 1) return ((PINB & 0x3F) | ((PINC & 0x03) << 6)) & bitmask; + if (port == 2) return ((PINC & 0x3C) >> 2) & bitmask; + return 0; +#else + unsigned char out=0, pin=port*8; + if (IS_PIN_DIGITAL(pin+0) && (bitmask & 0x01) && digitalRead(PIN_TO_DIGITAL(pin+0))) out |= 0x01; + if (IS_PIN_DIGITAL(pin+1) && (bitmask & 0x02) && digitalRead(PIN_TO_DIGITAL(pin+1))) out |= 0x02; + if (IS_PIN_DIGITAL(pin+2) && (bitmask & 0x04) && digitalRead(PIN_TO_DIGITAL(pin+2))) out |= 0x04; + if (IS_PIN_DIGITAL(pin+3) && (bitmask & 0x08) && digitalRead(PIN_TO_DIGITAL(pin+3))) out |= 0x08; + if (IS_PIN_DIGITAL(pin+4) && (bitmask & 0x10) && digitalRead(PIN_TO_DIGITAL(pin+4))) out |= 0x10; + if (IS_PIN_DIGITAL(pin+5) && (bitmask & 0x20) && digitalRead(PIN_TO_DIGITAL(pin+5))) out |= 0x20; + if (IS_PIN_DIGITAL(pin+6) && (bitmask & 0x40) && digitalRead(PIN_TO_DIGITAL(pin+6))) out |= 0x40; + if (IS_PIN_DIGITAL(pin+7) && (bitmask & 0x80) && digitalRead(PIN_TO_DIGITAL(pin+7))) out |= 0x80; + return out; +#endif +} + +/*============================================================================== + * writePort() - Write an 8 bit port, only touch pins specified by a bitmask + *============================================================================*/ + +static inline unsigned char writePort(byte, byte, byte) __attribute__((always_inline, unused)); +static inline unsigned char writePort(byte port, byte value, byte bitmask) +{ +#if defined(ARDUINO_PINOUT_OPTIMIZE) + if (port == 0) { + bitmask = bitmask & 0xFC; // do not touch Tx & Rx pins + byte valD = value & bitmask; + byte maskD = ~bitmask; + cli(); + PORTD = (PORTD & maskD) | valD; + sei(); + } else if (port == 1) { + byte valB = (value & bitmask) & 0x3F; + byte valC = (value & bitmask) >> 6; + byte maskB = ~(bitmask & 0x3F); + byte maskC = ~((bitmask & 0xC0) >> 6); + cli(); + PORTB = (PORTB & maskB) | valB; + PORTC = (PORTC & maskC) | valC; + sei(); + } else if (port == 2) { + bitmask = bitmask & 0x0F; + byte valC = (value & bitmask) << 2; + byte maskC = ~(bitmask << 2); + cli(); + PORTC = (PORTC & maskC) | valC; + sei(); + } +#else + byte pin=port*8; + if ((bitmask & 0x01)) digitalWrite(PIN_TO_DIGITAL(pin+0), (value & 0x01)); + if ((bitmask & 0x02)) digitalWrite(PIN_TO_DIGITAL(pin+1), (value & 0x02)); + if ((bitmask & 0x04)) digitalWrite(PIN_TO_DIGITAL(pin+2), (value & 0x04)); + if ((bitmask & 0x08)) digitalWrite(PIN_TO_DIGITAL(pin+3), (value & 0x08)); + if ((bitmask & 0x10)) digitalWrite(PIN_TO_DIGITAL(pin+4), (value & 0x10)); + if ((bitmask & 0x20)) digitalWrite(PIN_TO_DIGITAL(pin+5), (value & 0x20)); + if ((bitmask & 0x40)) digitalWrite(PIN_TO_DIGITAL(pin+6), (value & 0x40)); + if ((bitmask & 0x80)) digitalWrite(PIN_TO_DIGITAL(pin+7), (value & 0x80)); +#endif +} + + + + +#ifndef TOTAL_PORTS +#define TOTAL_PORTS ((TOTAL_PINS + 7) / 8) +#endif + + +#endif /* Firmata_Boards_h */ + diff --git a/libs/arduino-1.0/libraries/Firmata/Firmata.cpp b/libs/arduino-1.0/libraries/Firmata/Firmata.cpp new file mode 100644 index 0000000..e81c10b --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/Firmata.cpp @@ -0,0 +1,444 @@ +/* + Firmata.cpp - Firmata library + Copyright (C) 2006-2008 Hans-Christoph Steiner. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + See file LICENSE.txt for further informations on licensing terms. +*/ + +//****************************************************************************** +//* Includes +//****************************************************************************** + +#include "Firmata.h" +#include "HardwareSerial.h" + +extern "C" { +#include +#include +} + +//****************************************************************************** +//* Support Functions +//****************************************************************************** + +void FirmataClass::sendValueAsTwo7bitBytes(int value) +{ + FirmataSerial.write(value & B01111111); // LSB + FirmataSerial.write(value >> 7 & B01111111); // MSB +} + +void FirmataClass::startSysex(void) +{ + FirmataSerial.write(START_SYSEX); +} + +void FirmataClass::endSysex(void) +{ + FirmataSerial.write(END_SYSEX); +} + +//****************************************************************************** +//* Constructors +//****************************************************************************** + +FirmataClass::FirmataClass(Stream &s) : FirmataSerial(s) +{ + firmwareVersionCount = 0; + systemReset(); +} + +//****************************************************************************** +//* Public Methods +//****************************************************************************** + +/* begin method for overriding default serial bitrate */ +void FirmataClass::begin(void) +{ + begin(57600); +} + +/* begin method for overriding default serial bitrate */ +void FirmataClass::begin(long speed) +{ + Serial.begin(speed); + FirmataSerial = Serial; + blinkVersion(); + printVersion(); + printFirmwareVersion(); +} + +void FirmataClass::begin(Stream &s) +{ + FirmataSerial = s; + systemReset(); + printVersion(); + printFirmwareVersion(); +} + +// output the protocol version message to the serial port +void FirmataClass::printVersion(void) { + FirmataSerial.write(REPORT_VERSION); + FirmataSerial.write(FIRMATA_MAJOR_VERSION); + FirmataSerial.write(FIRMATA_MINOR_VERSION); +} + +void FirmataClass::blinkVersion(void) +{ + // flash the pin with the protocol version + pinMode(VERSION_BLINK_PIN,OUTPUT); + pin13strobe(FIRMATA_MAJOR_VERSION, 40, 210); + delay(250); + pin13strobe(FIRMATA_MINOR_VERSION, 40, 210); + delay(125); +} + +void FirmataClass::printFirmwareVersion(void) +{ + byte i; + + if(firmwareVersionCount) { // make sure that the name has been set before reporting + startSysex(); + FirmataSerial.write(REPORT_FIRMWARE); + FirmataSerial.write(firmwareVersionVector[0]); // major version number + FirmataSerial.write(firmwareVersionVector[1]); // minor version number + for(i=2; i 0) && (inputData < 128) ) { + waitForData--; + storedInputData[waitForData] = inputData; + if( (waitForData==0) && executeMultiByteCommand ) { // got the whole message + switch(executeMultiByteCommand) { + case ANALOG_MESSAGE: + if(currentAnalogCallback) { + (*currentAnalogCallback)(multiByteChannel, + (storedInputData[0] << 7) + + storedInputData[1]); + } + break; + case DIGITAL_MESSAGE: + if(currentDigitalCallback) { + (*currentDigitalCallback)(multiByteChannel, + (storedInputData[0] << 7) + + storedInputData[1]); + } + break; + case SET_PIN_MODE: + if(currentPinModeCallback) + (*currentPinModeCallback)(storedInputData[1], storedInputData[0]); + break; + case REPORT_ANALOG: + if(currentReportAnalogCallback) + (*currentReportAnalogCallback)(multiByteChannel,storedInputData[0]); + break; + case REPORT_DIGITAL: + if(currentReportDigitalCallback) + (*currentReportDigitalCallback)(multiByteChannel,storedInputData[0]); + break; + } + executeMultiByteCommand = 0; + } + } else { + // remove channel info from command byte if less than 0xF0 + if(inputData < 0xF0) { + command = inputData & 0xF0; + multiByteChannel = inputData & 0x0F; + } else { + command = inputData; + // commands in the 0xF* range don't use channel data + } + switch (command) { + case ANALOG_MESSAGE: + case DIGITAL_MESSAGE: + case SET_PIN_MODE: + waitForData = 2; // two data bytes needed + executeMultiByteCommand = command; + break; + case REPORT_ANALOG: + case REPORT_DIGITAL: + waitForData = 1; // two data bytes needed + executeMultiByteCommand = command; + break; + case START_SYSEX: + parsingSysex = true; + sysexBytesRead = 0; + break; + case SYSTEM_RESET: + systemReset(); + break; + case REPORT_VERSION: + Firmata.printVersion(); + break; + } + } +} + +//------------------------------------------------------------------------------ +// Serial Send Handling + +// send an analog message +void FirmataClass::sendAnalog(byte pin, int value) +{ + // pin can only be 0-15, so chop higher bits + FirmataSerial.write(ANALOG_MESSAGE | (pin & 0xF)); + sendValueAsTwo7bitBytes(value); +} + +// send a single digital pin in a digital message +void FirmataClass::sendDigital(byte pin, int value) +{ + /* TODO add single pin digital messages to the protocol, this needs to + * track the last digital data sent so that it can be sure to change just + * one bit in the packet. This is complicated by the fact that the + * numbering of the pins will probably differ on Arduino, Wiring, and + * other boards. The DIGITAL_MESSAGE sends 14 bits at a time, but it is + * probably easier to send 8 bit ports for any board with more than 14 + * digital pins. + */ + + // TODO: the digital message should not be sent on the serial port every + // time sendDigital() is called. Instead, it should add it to an int + // which will be sent on a schedule. If a pin changes more than once + // before the digital message is sent on the serial port, it should send a + // digital message for each change. + + // if(value == 0) + // sendDigitalPortPair(); +} + + +// send 14-bits in a single digital message (protocol v1) +// send an 8-bit port in a single digital message (protocol v2) +void FirmataClass::sendDigitalPort(byte portNumber, int portData) +{ + FirmataSerial.write(DIGITAL_MESSAGE | (portNumber & 0xF)); + FirmataSerial.write((byte)portData % 128); // Tx bits 0-6 + FirmataSerial.write(portData >> 7); // Tx bits 7-13 +} + + +void FirmataClass::sendSysex(byte command, byte bytec, byte* bytev) +{ + byte i; + startSysex(); + FirmataSerial.write(command); + for(i=0; i + +byte pin; + +int analogValue; +int previousAnalogValues[TOTAL_ANALOG_PINS]; + +byte portStatus[TOTAL_PORTS]; // each bit: 1=pin is digital input, 0=other/ignore +byte previousPINs[TOTAL_PORTS]; + +/* timer variables */ +unsigned long currentMillis; // store the current value from millis() +unsigned long previousMillis; // for comparison with currentMillis +/* make sure that the FTDI buffer doesn't go over 60 bytes, otherwise you + get long, random delays. So only read analogs every 20ms or so */ +int samplingInterval = 19; // how often to run the main loop (in ms) + +void sendPort(byte portNumber, byte portValue) +{ + portValue = portValue & portStatus[portNumber]; + if(previousPINs[portNumber] != portValue) { + Firmata.sendDigitalPort(portNumber, portValue); + previousPINs[portNumber] = portValue; + } +} + +void setup() +{ + byte i, port, status; + + Firmata.setFirmwareVersion(0, 1); + + for(pin = 0; pin < TOTAL_PINS; pin++) { + if IS_PIN_DIGITAL(pin) pinMode(PIN_TO_DIGITAL(pin), INPUT); + } + + for (port=0; port samplingInterval) { + previousMillis += samplingInterval; + while(Firmata.available()) { + Firmata.processInput(); + } + for(pin = 0; pin < TOTAL_ANALOG_PINS; pin++) { + analogValue = analogRead(pin); + if(analogValue != previousAnalogValues[pin]) { + Firmata.sendAnalog(pin, analogValue); + previousAnalogValues[pin] = analogValue; + } + } + } +} + + diff --git a/libs/arduino-1.0/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino b/libs/arduino-1.0/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino new file mode 100644 index 0000000..ff1d664 --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino @@ -0,0 +1,94 @@ +/* + * Firmata is a generic protocol for communicating with microcontrollers + * from software on a host computer. It is intended to work with + * any host computer software package. + * + * To download a host software package, please clink on the following link + * to open the download page in your default browser. + * + * http://firmata.org/wiki/Download + */ + +/* This firmware supports as many analog ports as possible, all analog inputs, + * four PWM outputs, and two with servo support. + * + * This example code is in the public domain. + */ +#include +#include + +/*============================================================================== + * GLOBAL VARIABLES + *============================================================================*/ + +/* servos */ +Servo servo9, servo10; // one instance per pin +/* analog inputs */ +int analogInputsToReport = 0; // bitwise array to store pin reporting +int analogPin = 0; // counter for reading analog pins +/* timer variables */ +unsigned long currentMillis; // store the current value from millis() +unsigned long previousMillis; // for comparison with currentMillis + + +/*============================================================================== + * FUNCTIONS + *============================================================================*/ + +void analogWriteCallback(byte pin, int value) +{ + switch(pin) { + case 9: servo9.write(value); break; + case 10: servo10.write(value); break; + case 3: + case 5: + case 6: + case 11: // PWM pins + analogWrite(pin, value); + break; + } +} +// ----------------------------------------------------------------------------- +// sets bits in a bit array (int) to toggle the reporting of the analogIns +void reportAnalogCallback(byte pin, int value) +{ + if(value == 0) { + analogInputsToReport = analogInputsToReport &~ (1 << pin); + } + else { // everything but 0 enables reporting of that pin + analogInputsToReport = analogInputsToReport | (1 << pin); + } + // TODO: save status to EEPROM here, if changed +} + +/*============================================================================== + * SETUP() + *============================================================================*/ +void setup() +{ + Firmata.setFirmwareVersion(0, 2); + Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); + Firmata.attach(REPORT_ANALOG, reportAnalogCallback); + + servo9.attach(9); + servo10.attach(10); + Firmata.begin(57600); +} + +/*============================================================================== + * LOOP() + *============================================================================*/ +void loop() +{ + while(Firmata.available()) + Firmata.processInput(); + currentMillis = millis(); + if(currentMillis - previousMillis > 20) { + previousMillis += 20; // run this every 20ms + for(analogPin=0;analogPin + +byte analogPin; + +void stringCallback(char *myString) +{ + Firmata.sendString(myString); +} + + +void sysexCallback(byte command, byte argc, byte*argv) +{ + Firmata.sendSysex(command, argc, argv); +} + +void setup() +{ + Firmata.setFirmwareVersion(0, 1); + Firmata.attach(STRING_DATA, stringCallback); + Firmata.attach(START_SYSEX, sysexCallback); + Firmata.begin(57600); +} + +void loop() +{ + while(Firmata.available()) { + Firmata.processInput(); + } +} + + diff --git a/libs/arduino-1.0/libraries/Firmata/examples/I2CFirmata/I2CFirmata.ino b/libs/arduino-1.0/libraries/Firmata/examples/I2CFirmata/I2CFirmata.ino new file mode 100644 index 0000000..1da8963 --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/examples/I2CFirmata/I2CFirmata.ino @@ -0,0 +1,228 @@ +/* + * Firmata is a generic protocol for communicating with microcontrollers + * from software on a host computer. It is intended to work with + * any host computer software package. + * + * To download a host software package, please clink on the following link + * to open the download page in your default browser. + * + * http://firmata.org/wiki/Download + */ + +/* + Copyright (C) 2009 Jeff Hoefs. All rights reserved. + Copyright (C) 2009 Shigeru Kobayashi. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + See file LICENSE.txt for further informations on licensing terms. + */ + +#include +#include + + +#define I2C_WRITE B00000000 +#define I2C_READ B00001000 +#define I2C_READ_CONTINUOUSLY B00010000 +#define I2C_STOP_READING B00011000 +#define I2C_READ_WRITE_MODE_MASK B00011000 + +#define MAX_QUERIES 8 + +unsigned long currentMillis; // store the current value from millis() +unsigned long previousMillis; // for comparison with currentMillis +unsigned int samplingInterval = 32; // default sampling interval is 33ms +unsigned int i2cReadDelayTime = 0; // default delay time between i2c read request and Wire.requestFrom() +unsigned int powerPinsEnabled = 0; // use as boolean to prevent enablePowerPins from being called more than once + +#define MINIMUM_SAMPLING_INTERVAL 10 + +#define REGISTER_NOT_SPECIFIED -1 + +struct i2c_device_info { + byte addr; + byte reg; + byte bytes; +}; + +i2c_device_info query[MAX_QUERIES]; + +byte i2cRxData[32]; +boolean readingContinuously = false; +byte queryIndex = 0; + +void readAndReportData(byte address, int theRegister, byte numBytes) +{ + if (theRegister != REGISTER_NOT_SPECIFIED) { + Wire.beginTransmission(address); + Wire.write((byte)theRegister); + Wire.endTransmission(); + delayMicroseconds(i2cReadDelayTime); // delay is necessary for some devices such as WiiNunchuck + } + else { + theRegister = 0; // fill the register with a dummy value + } + + Wire.requestFrom(address, numBytes); + + // check to be sure correct number of bytes were returned by slave + if(numBytes == Wire.available()) { + i2cRxData[0] = address; + i2cRxData[1] = theRegister; + for (int i = 0; i < numBytes; i++) { + i2cRxData[2 + i] = Wire.read(); + } + // send slave address, register and received bytes + Firmata.sendSysex(I2C_REPLY, numBytes + 2, i2cRxData); + } + else { + if(numBytes > Wire.available()) { + Firmata.sendString("I2C Read Error: Too many bytes received"); + } else { + Firmata.sendString("I2C Read Error: Too few bytes received"); + } + } + +} + +void sysexCallback(byte command, byte argc, byte *argv) +{ + byte mode; + byte slaveAddress; + byte slaveRegister; + byte data; + int delayTime; + + if (command == I2C_REQUEST) { + mode = argv[1] & I2C_READ_WRITE_MODE_MASK; + slaveAddress = argv[0]; + + switch(mode) { + case I2C_WRITE: + Wire.beginTransmission(slaveAddress); + for (byte i = 2; i < argc; i += 2) { + data = argv[i] + (argv[i + 1] << 7); + Wire.write(data); + } + Wire.endTransmission(); + delayMicroseconds(70); // TODO is this needed? + break; + case I2C_READ: + if (argc == 6) { + // a slave register is specified + slaveRegister = argv[2] + (argv[3] << 7); + data = argv[4] + (argv[5] << 7); // bytes to read + readAndReportData(slaveAddress, (int)slaveRegister, data); + } + else { + // a slave register is NOT specified + data = argv[2] + (argv[3] << 7); // bytes to read + readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data); + } + break; + case I2C_READ_CONTINUOUSLY: + if ((queryIndex + 1) >= MAX_QUERIES) { + // too many queries, just ignore + Firmata.sendString("too many queries"); + break; + } + query[queryIndex].addr = slaveAddress; + query[queryIndex].reg = argv[2] + (argv[3] << 7); + query[queryIndex].bytes = argv[4] + (argv[5] << 7); + readingContinuously = true; + queryIndex++; + break; + case I2C_STOP_READING: + readingContinuously = false; + queryIndex = 0; + break; + default: + break; + } + } + else if (command == SAMPLING_INTERVAL) { + samplingInterval = argv[0] + (argv[1] << 7); + + if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) { + samplingInterval = MINIMUM_SAMPLING_INTERVAL; + } + + samplingInterval -= 1; + Firmata.sendString("sampling interval"); + } + + else if (command == I2C_CONFIG) { + delayTime = (argv[4] + (argv[5] << 7)); // MSB + delayTime = (delayTime << 8) + (argv[2] + (argv[3] << 7)); // add LSB + + if((argv[0] + (argv[1] << 7)) > 0) { + enablePowerPins(PORTC3, PORTC2); + } + + if(delayTime > 0) { + i2cReadDelayTime = delayTime; + } + + if(argc > 6) { + // If you extend I2C_Config, handle your data here + } + + } +} + +void systemResetCallback() +{ + readingContinuously = false; + queryIndex = 0; +} + +/* reference: BlinkM_funcs.h by Tod E. Kurt, ThingM, http://thingm.com/ */ +// Enables Pins A2 and A3 to be used as GND and Power +// so that I2C devices can be plugged directly +// into Arduino header (pins A2 - A5) +static void enablePowerPins(byte pwrpin, byte gndpin) +{ + if(powerPinsEnabled == 0) { + DDRC |= _BV(pwrpin) | _BV(gndpin); + PORTC &=~ _BV(gndpin); + PORTC |= _BV(pwrpin); + powerPinsEnabled = 1; + Firmata.sendString("Power pins enabled"); + delay(100); + } +} + +void setup() +{ + Firmata.setFirmwareVersion(2, 0); + + Firmata.attach(START_SYSEX, sysexCallback); + Firmata.attach(SYSTEM_RESET, systemResetCallback); + + for (int i = 0; i < TOTAL_PINS; ++i) { + pinMode(i, OUTPUT); + } + + Firmata.begin(57600); + Wire.begin(); +} + +void loop() +{ + while (Firmata.available()) { + Firmata.processInput(); + } + + currentMillis = millis(); + if (currentMillis - previousMillis > samplingInterval) { + previousMillis += samplingInterval; + + for (byte i = 0; i < queryIndex; i++) { + readAndReportData(query[i].addr, query[i].reg, query[i].bytes); + } + } +} diff --git a/libs/arduino-1.0/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino b/libs/arduino-1.0/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino new file mode 100644 index 0000000..d306c70 --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino @@ -0,0 +1,239 @@ +/* + * Firmata is a generic protocol for communicating with microcontrollers + * from software on a host computer. It is intended to work with + * any host computer software package. + * + * To download a host software package, please clink on the following link + * to open the download page in your default browser. + * + * http://firmata.org/wiki/Download + */ + +/* + Copyright (C) 2006-2008 Hans-Christoph Steiner. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + See file LICENSE.txt for further informations on licensing terms. + */ + +/* + * This is an old version of StandardFirmata (v2.0). It is kept here because + * its the last version that works on an ATMEGA8 chip. Also, it can be used + * for host software that has not been updated to a newer version of the + * protocol. It also uses the old baud rate of 115200 rather than 57600. + */ + +#include +#include + +/*============================================================================== + * GLOBAL VARIABLES + *============================================================================*/ + +/* analog inputs */ +int analogInputsToReport = 0; // bitwise array to store pin reporting +int analogPin = 0; // counter for reading analog pins + +/* digital pins */ +byte reportPINs[TOTAL_PORTS]; // PIN == input port +byte previousPINs[TOTAL_PORTS]; // PIN == input port +byte pinStatus[TOTAL_PINS]; // store pin status, default OUTPUT +byte portStatus[TOTAL_PORTS]; + +/* timer variables */ +unsigned long currentMillis; // store the current value from millis() +unsigned long previousMillis; // for comparison with currentMillis + + +/*============================================================================== + * FUNCTIONS + *============================================================================*/ + +void outputPort(byte portNumber, byte portValue) +{ + portValue = portValue &~ portStatus[portNumber]; + if(previousPINs[portNumber] != portValue) { + Firmata.sendDigitalPort(portNumber, portValue); + previousPINs[portNumber] = portValue; + Firmata.sendDigitalPort(portNumber, portValue); + } +} + +/* ----------------------------------------------------------------------------- + * check all the active digital inputs for change of state, then add any events + * to the Serial output queue using Serial.print() */ +void checkDigitalInputs(void) +{ + byte i, tmp; + for(i=0; i < TOTAL_PORTS; i++) { + if(reportPINs[i]) { + switch(i) { + case 0: outputPort(0, PIND &~ B00000011); break; // ignore Rx/Tx 0/1 + case 1: outputPort(1, PINB); break; + case 2: outputPort(2, PINC); break; + } + } + } +} + +// ----------------------------------------------------------------------------- +/* sets the pin mode to the correct state and sets the relevant bits in the + * two bit-arrays that track Digital I/O and PWM status + */ +void setPinModeCallback(byte pin, int mode) { + byte port = 0; + byte offset = 0; + + if (pin < 8) { + port = 0; + offset = 0; + } else if (pin < 14) { + port = 1; + offset = 8; + } else if (pin < 22) { + port = 2; + offset = 14; + } + + if(pin > 1) { // ignore RxTx (pins 0 and 1) + pinStatus[pin] = mode; + switch(mode) { + case INPUT: + pinMode(pin, INPUT); + portStatus[port] = portStatus[port] &~ (1 << (pin - offset)); + break; + case OUTPUT: + digitalWrite(pin, LOW); // disable PWM + case PWM: + pinMode(pin, OUTPUT); + portStatus[port] = portStatus[port] | (1 << (pin - offset)); + break; + //case ANALOG: // TODO figure this out + default: + Firmata.sendString(""); + } + // TODO: save status to EEPROM here, if changed + } +} + +void analogWriteCallback(byte pin, int value) +{ + setPinModeCallback(pin,PWM); + analogWrite(pin, value); +} + +void digitalWriteCallback(byte port, int value) +{ + switch(port) { + case 0: // pins 2-7 (don't change Rx/Tx, pins 0 and 1) + // 0xFF03 == B1111111100000011 0x03 == B00000011 + PORTD = (value &~ 0xFF03) | (PORTD & 0x03); + break; + case 1: // pins 8-13 (14,15 are disabled for the crystal) + PORTB = (byte)value; + break; + case 2: // analog pins used as digital + PORTC = (byte)value; + break; + } +} + +// ----------------------------------------------------------------------------- +/* sets bits in a bit array (int) to toggle the reporting of the analogIns + */ +//void FirmataClass::setAnalogPinReporting(byte pin, byte state) { +//} +void reportAnalogCallback(byte pin, int value) +{ + if(value == 0) { + analogInputsToReport = analogInputsToReport &~ (1 << pin); + } + else { // everything but 0 enables reporting of that pin + analogInputsToReport = analogInputsToReport | (1 << pin); + } + // TODO: save status to EEPROM here, if changed +} + +void reportDigitalCallback(byte port, int value) +{ + reportPINs[port] = (byte)value; + if(port == 2) // turn off analog reporting when used as digital + analogInputsToReport = 0; +} + +/*============================================================================== + * SETUP() + *============================================================================*/ +void setup() +{ + byte i; + + Firmata.setFirmwareVersion(2, 0); + + Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); + Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback); + Firmata.attach(REPORT_ANALOG, reportAnalogCallback); + Firmata.attach(REPORT_DIGITAL, reportDigitalCallback); + Firmata.attach(SET_PIN_MODE, setPinModeCallback); + + portStatus[0] = B00000011; // ignore Tx/RX pins + portStatus[1] = B11000000; // ignore 14/15 pins + portStatus[2] = B00000000; + +// for(i=0; i 20) { + previousMillis += 20; // run this every 20ms + /* SERIALREAD - Serial.read() uses a 128 byte circular buffer, so handle + * all serialReads at once, i.e. empty the buffer */ + while(Firmata.available()) + Firmata.processInput(); + /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over + * 60 bytes. use a timer to sending an event character every 4 ms to + * trigger the buffer to dump. */ + + /* ANALOGREAD - right after the event character, do all of the + * analogReads(). These only need to be done every 4ms. */ + for(analogPin=0;analogPin +#include + +Servo servos[MAX_SERVOS]; + +void analogWriteCallback(byte pin, int value) +{ + if (IS_PIN_SERVO(pin)) { + servos[PIN_TO_SERVO(pin)].write(value); + } +} + +void setup() +{ + byte pin; + + Firmata.setFirmwareVersion(0, 2); + Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); + + for (pin=0; pin < TOTAL_PINS; pin++) { + if (IS_PIN_SERVO(pin)) { + servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin)); + } + } + + Firmata.begin(57600); +} + +void loop() +{ + while(Firmata.available()) + Firmata.processInput(); +} + diff --git a/libs/arduino-1.0/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino b/libs/arduino-1.0/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino new file mode 100644 index 0000000..44ea91e --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino @@ -0,0 +1,46 @@ +/* + * Firmata is a generic protocol for communicating with microcontrollers + * from software on a host computer. It is intended to work with + * any host computer software package. + * + * To download a host software package, please clink on the following link + * to open the download page in your default browser. + * + * http://firmata.org/wiki/Download + */ + +/* Supports as many analog inputs and analog PWM outputs as possible. + * + * This example code is in the public domain. + */ +#include + +byte analogPin = 0; + +void analogWriteCallback(byte pin, int value) +{ + if (IS_PIN_PWM(pin)) { + pinMode(PIN_TO_DIGITAL(pin), OUTPUT); + analogWrite(PIN_TO_PWM(pin), value); + } +} + +void setup() +{ + Firmata.setFirmwareVersion(0, 1); + Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); + Firmata.begin(57600); +} + +void loop() +{ + while(Firmata.available()) { + Firmata.processInput(); + } + // do one analogRead per loop, so if PC is sending a lot of + // analog write messages, we will only delay 1 analogRead + Firmata.sendAnalog(analogPin, analogRead(analogPin)); + analogPin = analogPin + 1; + if (analogPin >= TOTAL_ANALOG_PINS) analogPin = 0; +} + diff --git a/libs/arduino-1.0/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino b/libs/arduino-1.0/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino new file mode 100644 index 0000000..a0d764f --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino @@ -0,0 +1,72 @@ +/* + * Firmata is a generic protocol for communicating with microcontrollers + * from software on a host computer. It is intended to work with + * any host computer software package. + * + * To download a host software package, please clink on the following link + * to open the download page in your default browser. + * + * http://firmata.org/wiki/Download + */ + +/* Supports as many digital inputs and outputs as possible. + * + * This example code is in the public domain. + */ +#include + +byte previousPIN[TOTAL_PORTS]; // PIN means PORT for input +byte previousPORT[TOTAL_PORTS]; + +void outputPort(byte portNumber, byte portValue) +{ + // only send the data when it changes, otherwise you get too many messages! + if (previousPIN[portNumber] != portValue) { + Firmata.sendDigitalPort(portNumber, portValue); + previousPIN[portNumber] = portValue; + } +} + +void setPinModeCallback(byte pin, int mode) { + if (IS_PIN_DIGITAL(pin)) { + pinMode(PIN_TO_DIGITAL(pin), mode); + } +} + +void digitalWriteCallback(byte port, int value) +{ + byte i; + byte currentPinValue, previousPinValue; + + if (port < TOTAL_PORTS && value != previousPORT[port]) { + for(i=0; i<8; i++) { + currentPinValue = (byte) value & (1 << i); + previousPinValue = previousPORT[port] & (1 << i); + if(currentPinValue != previousPinValue) { + digitalWrite(i + (port*8), currentPinValue); + } + } + previousPORT[port] = value; + } +} + +void setup() +{ + Firmata.setFirmwareVersion(0, 1); + Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback); + Firmata.attach(SET_PIN_MODE, setPinModeCallback); + Firmata.begin(57600); +} + +void loop() +{ + byte i; + + for (i=0; i +#include +#include + +// move the following defines to Firmata.h? +#define I2C_WRITE B00000000 +#define I2C_READ B00001000 +#define I2C_READ_CONTINUOUSLY B00010000 +#define I2C_STOP_READING B00011000 +#define I2C_READ_WRITE_MODE_MASK B00011000 +#define I2C_10BIT_ADDRESS_MODE_MASK B00100000 + +#define MAX_QUERIES 8 +#define MINIMUM_SAMPLING_INTERVAL 10 + +#define REGISTER_NOT_SPECIFIED -1 + +/*============================================================================== + * GLOBAL VARIABLES + *============================================================================*/ + +/* analog inputs */ +int analogInputsToReport = 0; // bitwise array to store pin reporting + +/* digital input ports */ +byte reportPINs[TOTAL_PORTS]; // 1 = report this port, 0 = silence +byte previousPINs[TOTAL_PORTS]; // previous 8 bits sent + +/* pins configuration */ +byte pinConfig[TOTAL_PINS]; // configuration of every pin +byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else +int pinState[TOTAL_PINS]; // any value that has been written + +/* timer variables */ +unsigned long currentMillis; // store the current value from millis() +unsigned long previousMillis; // for comparison with currentMillis +int samplingInterval = 19; // how often to run the main loop (in ms) + +/* i2c data */ +struct i2c_device_info { + byte addr; + byte reg; + byte bytes; +}; + +/* for i2c read continuous more */ +i2c_device_info query[MAX_QUERIES]; + +byte i2cRxData[32]; +boolean isI2CEnabled = false; +signed char queryIndex = -1; +unsigned int i2cReadDelayTime = 0; // default delay time between i2c read request and Wire.requestFrom() + +Servo servos[MAX_SERVOS]; +/*============================================================================== + * FUNCTIONS + *============================================================================*/ + +void readAndReportData(byte address, int theRegister, byte numBytes) { + // allow I2C requests that don't require a register read + // for example, some devices using an interrupt pin to signify new data available + // do not always require the register read so upon interrupt you call Wire.requestFrom() + if (theRegister != REGISTER_NOT_SPECIFIED) { + Wire.beginTransmission(address); + #if ARDUINO >= 100 + Wire.write((byte)theRegister); + #else + Wire.send((byte)theRegister); + #endif + Wire.endTransmission(); + delayMicroseconds(i2cReadDelayTime); // delay is necessary for some devices such as WiiNunchuck + } else { + theRegister = 0; // fill the register with a dummy value + } + + Wire.requestFrom(address, numBytes); // all bytes are returned in requestFrom + + // check to be sure correct number of bytes were returned by slave + if(numBytes == Wire.available()) { + i2cRxData[0] = address; + i2cRxData[1] = theRegister; + for (int i = 0; i < numBytes; i++) { + #if ARDUINO >= 100 + i2cRxData[2 + i] = Wire.read(); + #else + i2cRxData[2 + i] = Wire.receive(); + #endif + } + } + else { + if(numBytes > Wire.available()) { + Firmata.sendString("I2C Read Error: Too many bytes received"); + } else { + Firmata.sendString("I2C Read Error: Too few bytes received"); + } + } + + // send slave address, register and received bytes + Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData); +} + +void outputPort(byte portNumber, byte portValue, byte forceSend) +{ + // pins not configured as INPUT are cleared to zeros + portValue = portValue & portConfigInputs[portNumber]; + // only send if the value is different than previously sent + if(forceSend || previousPINs[portNumber] != portValue) { + Firmata.sendDigitalPort(portNumber, portValue); + previousPINs[portNumber] = portValue; + } +} + +/* ----------------------------------------------------------------------------- + * check all the active digital inputs for change of state, then add any events + * to the Serial output queue using Serial.print() */ +void checkDigitalInputs(void) +{ + /* Using non-looping code allows constants to be given to readPort(). + * The compiler will apply substantial optimizations if the inputs + * to readPort() are compile-time constants. */ + if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false); + if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false); + if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false); + if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false); + if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false); + if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false); + if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false); + if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false); + if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false); + if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false); + if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false); + if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false); + if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false); + if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false); + if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false); + if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false); +} + +// ----------------------------------------------------------------------------- +/* sets the pin mode to the correct state and sets the relevant bits in the + * two bit-arrays that track Digital I/O and PWM status + */ +void setPinModeCallback(byte pin, int mode) +{ + if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) { + // disable i2c so pins can be used for other functions + // the following if statements should reconfigure the pins properly + disableI2CPins(); + } + if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached()) { + servos[PIN_TO_SERVO(pin)].detach(); + } + if (IS_PIN_ANALOG(pin)) { + reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting + } + if (IS_PIN_DIGITAL(pin)) { + if (mode == INPUT) { + portConfigInputs[pin/8] |= (1 << (pin & 7)); + } else { + portConfigInputs[pin/8] &= ~(1 << (pin & 7)); + } + } + pinState[pin] = 0; + switch(mode) { + case ANALOG: + if (IS_PIN_ANALOG(pin)) { + if (IS_PIN_DIGITAL(pin)) { + pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver + digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups + } + pinConfig[pin] = ANALOG; + } + break; + case INPUT: + if (IS_PIN_DIGITAL(pin)) { + pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver + digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups + pinConfig[pin] = INPUT; + } + break; + case OUTPUT: + if (IS_PIN_DIGITAL(pin)) { + digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM + pinMode(PIN_TO_DIGITAL(pin), OUTPUT); + pinConfig[pin] = OUTPUT; + } + break; + case PWM: + if (IS_PIN_PWM(pin)) { + pinMode(PIN_TO_PWM(pin), OUTPUT); + analogWrite(PIN_TO_PWM(pin), 0); + pinConfig[pin] = PWM; + } + break; + case SERVO: + if (IS_PIN_SERVO(pin)) { + pinConfig[pin] = SERVO; + if (!servos[PIN_TO_SERVO(pin)].attached()) { + servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin)); + } + } + break; + case I2C: + if (IS_PIN_I2C(pin)) { + // mark the pin as i2c + // the user must call I2C_CONFIG to enable I2C for a device + pinConfig[pin] = I2C; + } + break; + default: + Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM + } + // TODO: save status to EEPROM here, if changed +} + +void analogWriteCallback(byte pin, int value) +{ + if (pin < TOTAL_PINS) { + switch(pinConfig[pin]) { + case SERVO: + if (IS_PIN_SERVO(pin)) + servos[PIN_TO_SERVO(pin)].write(value); + pinState[pin] = value; + break; + case PWM: + if (IS_PIN_PWM(pin)) + analogWrite(PIN_TO_PWM(pin), value); + pinState[pin] = value; + break; + } + } +} + +void digitalWriteCallback(byte port, int value) +{ + byte pin, lastPin, mask=1, pinWriteMask=0; + + if (port < TOTAL_PORTS) { + // create a mask of the pins on this port that are writable. + lastPin = port*8+8; + if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS; + for (pin=port*8; pin < lastPin; pin++) { + // do not disturb non-digital pins (eg, Rx & Tx) + if (IS_PIN_DIGITAL(pin)) { + // only write to OUTPUT and INPUT (enables pullup) + // do not touch pins in PWM, ANALOG, SERVO or other modes + if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) { + pinWriteMask |= mask; + pinState[pin] = ((byte)value & mask) ? 1 : 0; + } + } + mask = mask << 1; + } + writePort(port, (byte)value, pinWriteMask); + } +} + + +// ----------------------------------------------------------------------------- +/* sets bits in a bit array (int) to toggle the reporting of the analogIns + */ +//void FirmataClass::setAnalogPinReporting(byte pin, byte state) { +//} +void reportAnalogCallback(byte analogPin, int value) +{ + if (analogPin < TOTAL_ANALOG_PINS) { + if(value == 0) { + analogInputsToReport = analogInputsToReport &~ (1 << analogPin); + } else { + analogInputsToReport = analogInputsToReport | (1 << analogPin); + } + } + // TODO: save status to EEPROM here, if changed +} + +void reportDigitalCallback(byte port, int value) +{ + if (port < TOTAL_PORTS) { + reportPINs[port] = (byte)value; + } + // do not disable analog reporting on these 8 pins, to allow some + // pins used for digital, others analog. Instead, allow both types + // of reporting to be enabled, but check if the pin is configured + // as analog when sampling the analog inputs. Likewise, while + // scanning digital pins, portConfigInputs will mask off values from any + // pins configured as analog +} + +/*============================================================================== + * SYSEX-BASED commands + *============================================================================*/ + +void sysexCallback(byte command, byte argc, byte *argv) +{ + byte mode; + byte slaveAddress; + byte slaveRegister; + byte data; + unsigned int delayTime; + + switch(command) { + case I2C_REQUEST: + mode = argv[1] & I2C_READ_WRITE_MODE_MASK; + if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) { + Firmata.sendString("10-bit addressing mode is not yet supported"); + return; + } + else { + slaveAddress = argv[0]; + } + + switch(mode) { + case I2C_WRITE: + Wire.beginTransmission(slaveAddress); + for (byte i = 2; i < argc; i += 2) { + data = argv[i] + (argv[i + 1] << 7); + #if ARDUINO >= 100 + Wire.write(data); + #else + Wire.send(data); + #endif + } + Wire.endTransmission(); + delayMicroseconds(70); + break; + case I2C_READ: + if (argc == 6) { + // a slave register is specified + slaveRegister = argv[2] + (argv[3] << 7); + data = argv[4] + (argv[5] << 7); // bytes to read + readAndReportData(slaveAddress, (int)slaveRegister, data); + } + else { + // a slave register is NOT specified + data = argv[2] + (argv[3] << 7); // bytes to read + readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data); + } + break; + case I2C_READ_CONTINUOUSLY: + if ((queryIndex + 1) >= MAX_QUERIES) { + // too many queries, just ignore + Firmata.sendString("too many queries"); + break; + } + queryIndex++; + query[queryIndex].addr = slaveAddress; + query[queryIndex].reg = argv[2] + (argv[3] << 7); + query[queryIndex].bytes = argv[4] + (argv[5] << 7); + break; + case I2C_STOP_READING: + byte queryIndexToSkip; + // if read continuous mode is enabled for only 1 i2c device, disable + // read continuous reporting for that device + if (queryIndex <= 0) { + queryIndex = -1; + } else { + // if read continuous mode is enabled for multiple devices, + // determine which device to stop reading and remove it's data from + // the array, shifiting other array data to fill the space + for (byte i = 0; i < queryIndex + 1; i++) { + if (query[i].addr = slaveAddress) { + queryIndexToSkip = i; + break; + } + } + + for (byte i = queryIndexToSkip; i 0) { + i2cReadDelayTime = delayTime; + } + + if (!isI2CEnabled) { + enableI2CPins(); + } + + break; + case SERVO_CONFIG: + if(argc > 4) { + // these vars are here for clarity, they'll optimized away by the compiler + byte pin = argv[0]; + int minPulse = argv[1] + (argv[2] << 7); + int maxPulse = argv[3] + (argv[4] << 7); + + if (IS_PIN_SERVO(pin)) { + if (servos[PIN_TO_SERVO(pin)].attached()) + servos[PIN_TO_SERVO(pin)].detach(); + servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse); + setPinModeCallback(pin, SERVO); + } + } + break; + case SAMPLING_INTERVAL: + if (argc > 1) { + samplingInterval = argv[0] + (argv[1] << 7); + if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) { + samplingInterval = MINIMUM_SAMPLING_INTERVAL; + } + } else { + //Firmata.sendString("Not enough data"); + } + break; + case EXTENDED_ANALOG: + if (argc > 1) { + int val = argv[1]; + if (argc > 2) val |= (argv[2] << 7); + if (argc > 3) val |= (argv[3] << 14); + analogWriteCallback(argv[0], val); + } + break; + case CAPABILITY_QUERY: + Serial.write(START_SYSEX); + Serial.write(CAPABILITY_RESPONSE); + for (byte pin=0; pin < TOTAL_PINS; pin++) { + if (IS_PIN_DIGITAL(pin)) { + Serial.write((byte)INPUT); + Serial.write(1); + Serial.write((byte)OUTPUT); + Serial.write(1); + } + if (IS_PIN_ANALOG(pin)) { + Serial.write(ANALOG); + Serial.write(10); + } + if (IS_PIN_PWM(pin)) { + Serial.write(PWM); + Serial.write(8); + } + if (IS_PIN_SERVO(pin)) { + Serial.write(SERVO); + Serial.write(14); + } + if (IS_PIN_I2C(pin)) { + Serial.write(I2C); + Serial.write(1); // to do: determine appropriate value + } + Serial.write(127); + } + Serial.write(END_SYSEX); + break; + case PIN_STATE_QUERY: + if (argc > 0) { + byte pin=argv[0]; + Serial.write(START_SYSEX); + Serial.write(PIN_STATE_RESPONSE); + Serial.write(pin); + if (pin < TOTAL_PINS) { + Serial.write((byte)pinConfig[pin]); + Serial.write((byte)pinState[pin] & 0x7F); + if (pinState[pin] & 0xFF80) Serial.write((byte)(pinState[pin] >> 7) & 0x7F); + if (pinState[pin] & 0xC000) Serial.write((byte)(pinState[pin] >> 14) & 0x7F); + } + Serial.write(END_SYSEX); + } + break; + case ANALOG_MAPPING_QUERY: + Serial.write(START_SYSEX); + Serial.write(ANALOG_MAPPING_RESPONSE); + for (byte pin=0; pin < TOTAL_PINS; pin++) { + Serial.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127); + } + Serial.write(END_SYSEX); + break; + } +} + +void enableI2CPins() +{ + byte i; + // is there a faster way to do this? would probaby require importing + // Arduino.h to get SCL and SDA pins + for (i=0; i < TOTAL_PINS; i++) { + if(IS_PIN_I2C(i)) { + // mark pins as i2c so they are ignore in non i2c data requests + setPinModeCallback(i, I2C); + } + } + + isI2CEnabled = true; + + // is there enough time before the first I2C request to call this here? + Wire.begin(); +} + +/* disable the i2c pins so they can be used for other functions */ +void disableI2CPins() { + isI2CEnabled = false; + // disable read continuous mode for all devices + queryIndex = -1; + // uncomment the following if or when the end() method is added to Wire library + // Wire.end(); +} + +/*============================================================================== + * SETUP() + *============================================================================*/ + +void systemResetCallback() +{ + // initialize a defalt state + // TODO: option to load config from EEPROM instead of default + if (isI2CEnabled) { + disableI2CPins(); + } + for (byte i=0; i < TOTAL_PORTS; i++) { + reportPINs[i] = false; // by default, reporting off + portConfigInputs[i] = 0; // until activated + previousPINs[i] = 0; + } + // pins with analog capability default to analog input + // otherwise, pins default to digital output + for (byte i=0; i < TOTAL_PINS; i++) { + if (IS_PIN_ANALOG(i)) { + // turns off pullup, configures everything + setPinModeCallback(i, ANALOG); + } else { + // sets the output to 0, configures portConfigInputs + setPinModeCallback(i, OUTPUT); + } + } + // by default, do not report any analog inputs + analogInputsToReport = 0; + + /* send digital inputs to set the initial state on the host computer, + * since once in the loop(), this firmware will only send on change */ + /* + TODO: this can never execute, since no pins default to digital input + but it will be needed when/if we support EEPROM stored config + for (byte i=0; i < TOTAL_PORTS; i++) { + outputPort(i, readPort(i, portConfigInputs[i]), true); + } + */ +} + +void setup() +{ + Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION); + + Firmata.attach(ANALOG_MESSAGE, analogWriteCallback); + Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback); + Firmata.attach(REPORT_ANALOG, reportAnalogCallback); + Firmata.attach(REPORT_DIGITAL, reportDigitalCallback); + Firmata.attach(SET_PIN_MODE, setPinModeCallback); + Firmata.attach(START_SYSEX, sysexCallback); + Firmata.attach(SYSTEM_RESET, systemResetCallback); + + Firmata.begin(57600); + systemResetCallback(); // reset to default config +} + +/*============================================================================== + * LOOP() + *============================================================================*/ +void loop() +{ + byte pin, analogPin; + + /* DIGITALREAD - as fast as possible, check for changes and output them to the + * FTDI buffer using Serial.print() */ + checkDigitalInputs(); + + /* SERIALREAD - processing incoming messagse as soon as possible, while still + * checking digital inputs. */ + while(Firmata.available()) + Firmata.processInput(); + + /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over + * 60 bytes. use a timer to sending an event character every 4 ms to + * trigger the buffer to dump. */ + + currentMillis = millis(); + if (currentMillis - previousMillis > samplingInterval) { + previousMillis += samplingInterval; + /* ANALOGREAD - do all analogReads() at the configured sampling interval */ + for(pin=0; pin -1) { + for (byte i = 0; i < queryIndex + 1; i++) { + readAndReportData(query[i].addr, query[i].reg, query[i].bytes); + } + } + } +} diff --git a/libs/arduino-1.0/libraries/Firmata/keywords.txt b/libs/arduino-1.0/libraries/Firmata/keywords.txt new file mode 100644 index 0000000..52e0a9c --- /dev/null +++ b/libs/arduino-1.0/libraries/Firmata/keywords.txt @@ -0,0 +1,62 @@ +####################################### +# Syntax Coloring Map For Firmata +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +Firmata KEYWORD1 +callbackFunction KEYWORD1 +systemResetCallbackFunction KEYWORD1 +stringCallbackFunction KEYWORD1 +sysexCallbackFunction KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +begin KEYWORD2 +printVersion KEYWORD2 +blinkVersion KEYWORD2 +printFirmwareVersion KEYWORD2 +setFirmwareVersion KEYWORD2 +setFirmwareNameAndVersion KEYWORD2 +available KEYWORD2 +processInput KEYWORD2 +sendAnalog KEYWORD2 +sendDigital KEYWORD2 +sendDigitalPortPair KEYWORD2 +sendDigitalPort KEYWORD2 +sendString KEYWORD2 +sendString KEYWORD2 +sendSysex KEYWORD2 +attach KEYWORD2 +detach KEYWORD2 +flush KEYWORD2 + + +####################################### +# Constants (LITERAL1) +####################################### + +MAX_DATA_BYTES LITERAL1 + +DIGITAL_MESSAGE LITERAL1 +ANALOG_MESSAGE LITERAL1 +REPORT_ANALOG LITERAL1 +REPORT_DIGITAL LITERAL1 +REPORT_VERSION LITERAL1 +SET_PIN_MODE LITERAL1 +SYSTEM_RESET LITERAL1 + +START_SYSEX LITERAL1 +END_SYSEX LITERAL1 + +PWM LITERAL1 + +TOTAL_ANALOG_PINS LITERAL1 +TOTAL_DIGITAL_PINS LITERAL1 +TOTAL_PORTS LITERAL1 +ANALOG_PORT LITERAL1 diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.cpp b/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.cpp new file mode 100644 index 0000000..0653487 --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.cpp @@ -0,0 +1,310 @@ +#include "LiquidCrystal.h" + +#include +#include +#include +#include "Arduino.h" + +// When the display powers up, it is configured as follows: +// +// 1. Display clear +// 2. Function set: +// DL = 1; 8-bit interface data +// N = 0; 1-line display +// F = 0; 5x8 dot character font +// 3. Display on/off control: +// D = 0; Display off +// C = 0; Cursor off +// B = 0; Blinking off +// 4. Entry mode set: +// I/D = 1; Increment by 1 +// S = 0; No shift +// +// Note, however, that resetting the Arduino doesn't reset the LCD, so we +// can't assume that its in that state when a sketch starts (and the +// LiquidCrystal constructor is called). + +LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) +{ + init(0, rs, rw, enable, d0, d1, d2, d3, d4, d5, d6, d7); +} + +LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) +{ + init(0, rs, 255, enable, d0, d1, d2, d3, d4, d5, d6, d7); +} + +LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3) +{ + init(1, rs, rw, enable, d0, d1, d2, d3, 0, 0, 0, 0); +} + +LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3) +{ + init(1, rs, 255, enable, d0, d1, d2, d3, 0, 0, 0, 0); +} + +void LiquidCrystal::init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) +{ + _rs_pin = rs; + _rw_pin = rw; + _enable_pin = enable; + + _data_pins[0] = d0; + _data_pins[1] = d1; + _data_pins[2] = d2; + _data_pins[3] = d3; + _data_pins[4] = d4; + _data_pins[5] = d5; + _data_pins[6] = d6; + _data_pins[7] = d7; + + pinMode(_rs_pin, OUTPUT); + // we can save 1 pin by not using RW. Indicate by passing 255 instead of pin# + if (_rw_pin != 255) { + pinMode(_rw_pin, OUTPUT); + } + pinMode(_enable_pin, OUTPUT); + + if (fourbitmode) + _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS; + else + _displayfunction = LCD_8BITMODE | LCD_1LINE | LCD_5x8DOTS; + + begin(16, 1); +} + +void LiquidCrystal::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) { + if (lines > 1) { + _displayfunction |= LCD_2LINE; + } + _numlines = lines; + _currline = 0; + + // for some 1 line displays you can select a 10 pixel high font + if ((dotsize != 0) && (lines == 1)) { + _displayfunction |= LCD_5x10DOTS; + } + + // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION! + // according to datasheet, we need at least 40ms after power rises above 2.7V + // before sending commands. Arduino can turn on way befer 4.5V so we'll wait 50 + delayMicroseconds(50000); + // Now we pull both RS and R/W low to begin commands + digitalWrite(_rs_pin, LOW); + digitalWrite(_enable_pin, LOW); + if (_rw_pin != 255) { + digitalWrite(_rw_pin, LOW); + } + + //put the LCD into 4 bit or 8 bit mode + if (! (_displayfunction & LCD_8BITMODE)) { + // this is according to the hitachi HD44780 datasheet + // figure 24, pg 46 + + // we start in 8bit mode, try to set 4 bit mode + write4bits(0x03); + delayMicroseconds(4500); // wait min 4.1ms + + // second try + write4bits(0x03); + delayMicroseconds(4500); // wait min 4.1ms + + // third go! + write4bits(0x03); + delayMicroseconds(150); + + // finally, set to 4-bit interface + write4bits(0x02); + } else { + // this is according to the hitachi HD44780 datasheet + // page 45 figure 23 + + // Send function set command sequence + command(LCD_FUNCTIONSET | _displayfunction); + delayMicroseconds(4500); // wait more than 4.1ms + + // second try + command(LCD_FUNCTIONSET | _displayfunction); + delayMicroseconds(150); + + // third go + command(LCD_FUNCTIONSET | _displayfunction); + } + + // finally, set # lines, font size, etc. + command(LCD_FUNCTIONSET | _displayfunction); + + // turn the display on with no cursor or blinking default + _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF; + display(); + + // clear it off + clear(); + + // Initialize to default text direction (for romance languages) + _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT; + // set the entry mode + command(LCD_ENTRYMODESET | _displaymode); + +} + +/********** high level commands, for the user! */ +void LiquidCrystal::clear() +{ + command(LCD_CLEARDISPLAY); // clear display, set cursor position to zero + delayMicroseconds(2000); // this command takes a long time! +} + +void LiquidCrystal::home() +{ + command(LCD_RETURNHOME); // set cursor position to zero + delayMicroseconds(2000); // this command takes a long time! +} + +void LiquidCrystal::setCursor(uint8_t col, uint8_t row) +{ + int row_offsets[] = { 0x00, 0x40, 0x14, 0x54 }; + if ( row >= _numlines ) { + row = _numlines-1; // we count rows starting w/0 + } + + command(LCD_SETDDRAMADDR | (col + row_offsets[row])); +} + +// Turn the display on/off (quickly) +void LiquidCrystal::noDisplay() { + _displaycontrol &= ~LCD_DISPLAYON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal::display() { + _displaycontrol |= LCD_DISPLAYON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// Turns the underline cursor on/off +void LiquidCrystal::noCursor() { + _displaycontrol &= ~LCD_CURSORON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal::cursor() { + _displaycontrol |= LCD_CURSORON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// Turn on and off the blinking cursor +void LiquidCrystal::noBlink() { + _displaycontrol &= ~LCD_BLINKON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} +void LiquidCrystal::blink() { + _displaycontrol |= LCD_BLINKON; + command(LCD_DISPLAYCONTROL | _displaycontrol); +} + +// These commands scroll the display without changing the RAM +void LiquidCrystal::scrollDisplayLeft(void) { + command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT); +} +void LiquidCrystal::scrollDisplayRight(void) { + command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT); +} + +// This is for text that flows Left to Right +void LiquidCrystal::leftToRight(void) { + _displaymode |= LCD_ENTRYLEFT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This is for text that flows Right to Left +void LiquidCrystal::rightToLeft(void) { + _displaymode &= ~LCD_ENTRYLEFT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This will 'right justify' text from the cursor +void LiquidCrystal::autoscroll(void) { + _displaymode |= LCD_ENTRYSHIFTINCREMENT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// This will 'left justify' text from the cursor +void LiquidCrystal::noAutoscroll(void) { + _displaymode &= ~LCD_ENTRYSHIFTINCREMENT; + command(LCD_ENTRYMODESET | _displaymode); +} + +// Allows us to fill the first 8 CGRAM locations +// with custom characters +void LiquidCrystal::createChar(uint8_t location, uint8_t charmap[]) { + location &= 0x7; // we only have 8 locations 0-7 + command(LCD_SETCGRAMADDR | (location << 3)); + for (int i=0; i<8; i++) { + write(charmap[i]); + } +} + +/*********** mid level commands, for sending data/cmds */ + +inline void LiquidCrystal::command(uint8_t value) { + send(value, LOW); +} + +inline size_t LiquidCrystal::write(uint8_t value) { + send(value, HIGH); + return 1; // assume sucess +} + +/************ low level data pushing commands **********/ + +// write either command or data, with automatic 4/8-bit selection +void LiquidCrystal::send(uint8_t value, uint8_t mode) { + digitalWrite(_rs_pin, mode); + + // if there is a RW pin indicated, set it low to Write + if (_rw_pin != 255) { + digitalWrite(_rw_pin, LOW); + } + + if (_displayfunction & LCD_8BITMODE) { + write8bits(value); + } else { + write4bits(value>>4); + write4bits(value); + } +} + +void LiquidCrystal::pulseEnable(void) { + digitalWrite(_enable_pin, LOW); + delayMicroseconds(1); + digitalWrite(_enable_pin, HIGH); + delayMicroseconds(1); // enable pulse must be >450ns + digitalWrite(_enable_pin, LOW); + delayMicroseconds(100); // commands need > 37us to settle +} + +void LiquidCrystal::write4bits(uint8_t value) { + for (int i = 0; i < 4; i++) { + pinMode(_data_pins[i], OUTPUT); + digitalWrite(_data_pins[i], (value >> i) & 0x01); + } + + pulseEnable(); +} + +void LiquidCrystal::write8bits(uint8_t value) { + for (int i = 0; i < 8; i++) { + pinMode(_data_pins[i], OUTPUT); + digitalWrite(_data_pins[i], (value >> i) & 0x01); + } + + pulseEnable(); +} diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.h b/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.h new file mode 100644 index 0000000..24ec5af --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/LiquidCrystal.h @@ -0,0 +1,106 @@ +#ifndef LiquidCrystal_h +#define LiquidCrystal_h + +#include +#include "Print.h" + +// commands +#define LCD_CLEARDISPLAY 0x01 +#define LCD_RETURNHOME 0x02 +#define LCD_ENTRYMODESET 0x04 +#define LCD_DISPLAYCONTROL 0x08 +#define LCD_CURSORSHIFT 0x10 +#define LCD_FUNCTIONSET 0x20 +#define LCD_SETCGRAMADDR 0x40 +#define LCD_SETDDRAMADDR 0x80 + +// flags for display entry mode +#define LCD_ENTRYRIGHT 0x00 +#define LCD_ENTRYLEFT 0x02 +#define LCD_ENTRYSHIFTINCREMENT 0x01 +#define LCD_ENTRYSHIFTDECREMENT 0x00 + +// flags for display on/off control +#define LCD_DISPLAYON 0x04 +#define LCD_DISPLAYOFF 0x00 +#define LCD_CURSORON 0x02 +#define LCD_CURSOROFF 0x00 +#define LCD_BLINKON 0x01 +#define LCD_BLINKOFF 0x00 + +// flags for display/cursor shift +#define LCD_DISPLAYMOVE 0x08 +#define LCD_CURSORMOVE 0x00 +#define LCD_MOVERIGHT 0x04 +#define LCD_MOVELEFT 0x00 + +// flags for function set +#define LCD_8BITMODE 0x10 +#define LCD_4BITMODE 0x00 +#define LCD_2LINE 0x08 +#define LCD_1LINE 0x00 +#define LCD_5x10DOTS 0x04 +#define LCD_5x8DOTS 0x00 + +class LiquidCrystal : public Print { +public: + LiquidCrystal(uint8_t rs, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); + LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); + LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3); + LiquidCrystal(uint8_t rs, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3); + + void init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable, + uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, + uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7); + + void begin(uint8_t cols, uint8_t rows, uint8_t charsize = LCD_5x8DOTS); + + void clear(); + void home(); + + void noDisplay(); + void display(); + void noBlink(); + void blink(); + void noCursor(); + void cursor(); + void scrollDisplayLeft(); + void scrollDisplayRight(); + void leftToRight(); + void rightToLeft(); + void autoscroll(); + void noAutoscroll(); + + void createChar(uint8_t, uint8_t[]); + void setCursor(uint8_t, uint8_t); + virtual size_t write(uint8_t); + void command(uint8_t); + + using Print::write; +private: + void send(uint8_t, uint8_t); + void write4bits(uint8_t); + void write8bits(uint8_t); + void pulseEnable(); + + uint8_t _rs_pin; // LOW: command. HIGH: character. + uint8_t _rw_pin; // LOW: write to LCD. HIGH: read from LCD. + uint8_t _enable_pin; // activated by a HIGH pulse. + uint8_t _data_pins[8]; + + uint8_t _displayfunction; + uint8_t _displaycontrol; + uint8_t _displaymode; + + uint8_t _initialized; + + uint8_t _numlines,_currline; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.ino new file mode 100644 index 0000000..1127d8f --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.ino @@ -0,0 +1,74 @@ +/* + LiquidCrystal Library - Autoscroll + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch demonstrates the use of the autoscroll() + and noAutoscroll() functions to make new text scroll or not. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalAutoscroll + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16,2); +} + +void loop() { + // set the cursor to (0,0): + lcd.setCursor(0, 0); + // print from 0 to 9: + for (int thisChar = 0; thisChar < 10; thisChar++) { + lcd.print(thisChar); + delay(500); + } + + // set the cursor to (16,1): + lcd.setCursor(16,1); + // set the display to automatically scroll: + lcd.autoscroll(); + // print from 0 to 9: + for (int thisChar = 0; thisChar < 10; thisChar++) { + lcd.print(thisChar); + delay(500); + } + // turn off automatic scrolling + lcd.noAutoscroll(); + + // clear screen for the next loop: + lcd.clear(); +} + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/Blink/Blink.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Blink/Blink.ino new file mode 100644 index 0000000..9667b5d --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Blink/Blink.ino @@ -0,0 +1,61 @@ +/* + LiquidCrystal Library - Blink + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints "Hello World!" to the LCD and makes the + cursor block blink. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalBlink + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the LCD. + lcd.print("hello, world!"); +} + +void loop() { + // Turn off the blinking cursor: + lcd.noBlink(); + delay(3000); + // Turn on the blinking cursor: + lcd.blink(); + delay(3000); +} + + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/Cursor/Cursor.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Cursor/Cursor.ino new file mode 100644 index 0000000..05862a4 --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Cursor/Cursor.ino @@ -0,0 +1,61 @@ +/* + LiquidCrystal Library - Cursor + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints "Hello World!" to the LCD and + uses the cursor() and noCursor() methods to turn + on and off the cursor. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalCursor + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the LCD. + lcd.print("hello, world!"); +} + +void loop() { + // Turn off the cursor: + lcd.noCursor(); + delay(500); + // Turn on the cursor: + lcd.cursor(); + delay(500); +} + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/CustomCharacter/CustomCharacter.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/CustomCharacter/CustomCharacter.ino new file mode 100644 index 0000000..d3ce479 --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/CustomCharacter/CustomCharacter.ino @@ -0,0 +1,138 @@ +/* + LiquidCrystal Library - Custom Characters + + Demonstrates how to add custom characters on an LCD display. + The LiquidCrystal library works with all LCD displays that are + compatible with the Hitachi HD44780 driver. There are many of + them out there, and you can usually tell them by the 16-pin interface. + + This sketch prints "I Arduino!" and a little dancing man + to the LCD. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K potentiometer: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + * 10K poterntiometer on pin A0 + + created21 Mar 2011 + by Tom Igoe + Based on Adafruit's example at + https://github.com/adafruit/SPI_VFD/blob/master/examples/createChar/createChar.pde + + This example code is in the public domain. + http://www.arduino.cc/en/Tutorial/LiquidCrystal + + Also useful: + http://icontexto.com/charactercreator/ + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +// make some custom characters: +byte heart[8] = { + 0b00000, + 0b01010, + 0b11111, + 0b11111, + 0b11111, + 0b01110, + 0b00100, + 0b00000 +}; + +byte smiley[8] = { + 0b00000, + 0b00000, + 0b01010, + 0b00000, + 0b00000, + 0b10001, + 0b01110, + 0b00000 +}; + +byte frownie[8] = { + 0b00000, + 0b00000, + 0b01010, + 0b00000, + 0b00000, + 0b00000, + 0b01110, + 0b10001 +}; + +byte armsDown[8] = { + 0b00100, + 0b01010, + 0b00100, + 0b00100, + 0b01110, + 0b10101, + 0b00100, + 0b01010 +}; + +byte armsUp[8] = { + 0b00100, + 0b01010, + 0b00100, + 0b10101, + 0b01110, + 0b00100, + 0b00100, + 0b01010 +}; +void setup() { + // create a new character + lcd.createChar(0, heart); + // create a new character + lcd.createChar(1, smiley); + // create a new character + lcd.createChar(2, frownie); + // create a new character + lcd.createChar(3, armsDown); + // create a new character + lcd.createChar(4, armsUp); + + // set up the lcd's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the lcd. + lcd.print("I "); + lcd.write(0); + lcd.print(" Arduino! "); + lcd.write(1); + +} + +void loop() { + // read the potentiometer on A0: + int sensorReading = analogRead(A0); + // map the result to 200 - 1000: + int delayTime = map(sensorReading, 0, 1023, 200, 1000); + // set the cursor to the bottom row, 5th position: + lcd.setCursor(4, 1); + // draw the little man, arms down: + lcd.write(3); + delay(delayTime); + lcd.setCursor(4, 1); + // draw him arms up: + lcd.write(4); + delay(delayTime); +} + + + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/Display/Display.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Display/Display.ino new file mode 100644 index 0000000..a85effb --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Display/Display.ino @@ -0,0 +1,61 @@ +/* + LiquidCrystal Library - display() and noDisplay() + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints "Hello World!" to the LCD and uses the + display() and noDisplay() functions to turn on and off + the display. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalDisplay + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the LCD. + lcd.print("hello, world!"); +} + +void loop() { + // Turn off the display: + lcd.noDisplay(); + delay(500); + // Turn on the display: + lcd.display(); + delay(500); +} + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.ino new file mode 100644 index 0000000..e99957d --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.ino @@ -0,0 +1,58 @@ +/* + LiquidCrystal Library - Hello World + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints "Hello World!" to the LCD + and shows the time. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://www.arduino.cc/en/Tutorial/LiquidCrystal + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the LCD. + lcd.print("hello, world!"); +} + +void loop() { + // set the cursor to column 0, line 1 + // (note: line 1 is the second row, since counting begins with 0): + lcd.setCursor(0, 1); + // print the number of seconds since reset: + lcd.print(millis()/1000); +} + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/Scroll/Scroll.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Scroll/Scroll.ino new file mode 100644 index 0000000..0d6d8dc --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/Scroll/Scroll.ino @@ -0,0 +1,86 @@ +/* + LiquidCrystal Library - scrollDisplayLeft() and scrollDisplayRight() + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints "Hello World!" to the LCD and uses the + scrollDisplayLeft() and scrollDisplayRight() methods to scroll + the text. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalScroll + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // Print a message to the LCD. + lcd.print("hello, world!"); + delay(1000); +} + +void loop() { + // scroll 13 positions (string length) to the left + // to move it offscreen left: + for (int positionCounter = 0; positionCounter < 13; positionCounter++) { + // scroll one position left: + lcd.scrollDisplayLeft(); + // wait a bit: + delay(150); + } + + // scroll 29 positions (string length + display length) to the right + // to move it offscreen right: + for (int positionCounter = 0; positionCounter < 29; positionCounter++) { + // scroll one position right: + lcd.scrollDisplayRight(); + // wait a bit: + delay(150); + } + + // scroll 16 positions (display length + string length) to the left + // to move it back to center: + for (int positionCounter = 0; positionCounter < 16; positionCounter++) { + // scroll one position left: + lcd.scrollDisplayLeft(); + // wait a bit: + delay(150); + } + + // delay at the end of the full loop: + delay(1000); + +} + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.ino new file mode 100644 index 0000000..a6f8f40 --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.ino @@ -0,0 +1,65 @@ +/* + LiquidCrystal Library - Serial Input + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch displays text sent over the serial port + (e.g. from the Serial Monitor) on an attached LCD. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalSerial + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup(){ + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // initialize the serial communications: + Serial.begin(9600); +} + +void loop() +{ + // when characters arrive over the serial port... + if (Serial.available()) { + // wait a bit for the entire message to arrive + delay(100); + // clear the screen + lcd.clear(); + // read all the available characters + while (Serial.available() > 0) { + // display each character to the LCD + lcd.write(Serial.read()); + } + } +} diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/TextDirection/TextDirection.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/TextDirection/TextDirection.ino new file mode 100644 index 0000000..cabd8ea --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/TextDirection/TextDirection.ino @@ -0,0 +1,86 @@ + /* + LiquidCrystal Library - TextDirection + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch demonstrates how to use leftToRight() and rightToLeft() + to move the cursor. + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalTextDirection + + */ + +// include the library code: +#include + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +int thisChar = 'a'; + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(16, 2); + // turn on the cursor: + lcd.cursor(); +} + +void loop() { + // reverse directions at 'm': + if (thisChar == 'm') { + // go right for the next letter + lcd.rightToLeft(); + } + // reverse again at 's': + if (thisChar == 's') { + // go left for the next letter + lcd.leftToRight(); + } + // reset at 'z': + if (thisChar > 'z') { + // go to (0,0): + lcd.home(); + // start again at 0 + thisChar = 'a'; + } + // print the character + lcd.write(thisChar); + // wait a second: + delay(1000); + // increment the letter: + thisChar++; +} + + + + + + + + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/examples/setCursor/setCursor.ino b/libs/arduino-1.0/libraries/LiquidCrystal/examples/setCursor/setCursor.ino new file mode 100644 index 0000000..e45c491 --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/examples/setCursor/setCursor.ino @@ -0,0 +1,72 @@ +/* + LiquidCrystal Library - setCursor + + Demonstrates the use a 16x2 LCD display. The LiquidCrystal + library works with all LCD displays that are compatible with the + Hitachi HD44780 driver. There are many of them out there, and you + can usually tell them by the 16-pin interface. + + This sketch prints to all the positions of the LCD using the + setCursor(0 method: + + The circuit: + * LCD RS pin to digital pin 12 + * LCD Enable pin to digital pin 11 + * LCD D4 pin to digital pin 5 + * LCD D5 pin to digital pin 4 + * LCD D6 pin to digital pin 3 + * LCD D7 pin to digital pin 2 + * LCD R/W pin to ground + * 10K resistor: + * ends to +5V and ground + * wiper to LCD VO pin (pin 3) + + Library originally added 18 Apr 2008 + by David A. Mellis + library modified 5 Jul 2009 + by Limor Fried (http://www.ladyada.net) + example added 9 Jul 2009 + by Tom Igoe + modified 22 Nov 2010 + by Tom Igoe + + This example code is in the public domain. + + http://arduino.cc/en/Tutorial/LiquidCrystalSetCursor + + */ + +// include the library code: +#include + +// these constants won't change. But you can change the size of +// your LCD using them: +const int numRows = 2; +const int numCols = 16; + +// initialize the library with the numbers of the interface pins +LiquidCrystal lcd(12, 11, 5, 4, 3, 2); + +void setup() { + // set up the LCD's number of columns and rows: + lcd.begin(numCols,numRows); +} + +void loop() { + // loop from ASCII 'a' to ASCII 'z': + for (int thisLetter = 'a'; thisLetter <= 'z'; thisLetter++) { + // loop over the columns: + for (int thisCol = 0; thisCol < numRows; thisCol++) { + // loop over the rows: + for (int thisRow = 0; thisRow < numCols; thisRow++) { + // set the cursor position: + lcd.setCursor(thisRow,thisCol); + // print the letter: + lcd.write(thisLetter); + delay(200); + } + } + } +} + + diff --git a/libs/arduino-1.0/libraries/LiquidCrystal/keywords.txt b/libs/arduino-1.0/libraries/LiquidCrystal/keywords.txt new file mode 100644 index 0000000..132845c --- /dev/null +++ b/libs/arduino-1.0/libraries/LiquidCrystal/keywords.txt @@ -0,0 +1,37 @@ +####################################### +# Syntax Coloring Map For LiquidCrystal +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +LiquidCrystal KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +clear KEYWORD2 +home KEYWORD2 +print KEYWORD2 +setCursor KEYWORD2 +cursor KEYWORD2 +noCursor KEYWORD2 +blink KEYWORD2 +noBlink KEYWORD2 +display KEYWORD2 +noDisplay KEYWORD2 +autoscroll KEYWORD2 +noAutoscroll KEYWORD2 +leftToRight KEYWORD2 +rightToLeft KEYWORD2 +scrollDisplayLeft KEYWORD2 +scrollDisplayRight KEYWORD2 +createChar KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/SD/File.cpp b/libs/arduino-1.0/libraries/SD/File.cpp new file mode 100644 index 0000000..88d9e9a --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/File.cpp @@ -0,0 +1,150 @@ +/* + + SD - a slightly more friendly wrapper for sdfatlib + + This library aims to expose a subset of SD card functionality + in the form of a higher level "wrapper" object. + + License: GNU General Public License V3 + (Because sdfatlib is licensed with this.) + + (C) Copyright 2010 SparkFun Electronics + + */ + +#include + +/* for debugging file open/close leaks + uint8_t nfilecount=0; +*/ + +File::File(SdFile f, const char *n) { + // oh man you are kidding me, new() doesnt exist? Ok we do it by hand! + _file = (SdFile *)malloc(sizeof(SdFile)); + if (_file) { + memcpy(_file, &f, sizeof(SdFile)); + + strncpy(_name, n, 12); + _name[12] = 0; + + /* for debugging file open/close leaks + nfilecount++; + Serial.print("Created \""); + Serial.print(n); + Serial.print("\": "); + Serial.println(nfilecount, DEC); + */ + } +} + +File::File(void) { + _file = 0; + _name[0] = 0; + //Serial.print("Created empty file object"); +} + +File::~File(void) { + // Serial.print("Deleted file object"); +} + +// returns a pointer to the file name +char *File::name(void) { + return _name; +} + +// a directory is a special type of file +boolean File::isDirectory(void) { + return (_file && _file->isDir()); +} + + +size_t File::write(uint8_t val) { + return write(&val, 1); +} + +size_t File::write(const uint8_t *buf, size_t size) { + size_t t; + if (!_file) { + setWriteError(); + return 0; + } + _file->clearWriteError(); + t = _file->write(buf, size); + if (_file->getWriteError()) { + setWriteError(); + return 0; + } + return t; +} + +int File::peek() { + if (! _file) + return 0; + + int c = _file->read(); + if (c != -1) _file->seekCur(-1); + return c; +} + +int File::read() { + if (_file) + return _file->read(); + return -1; +} + +// buffered read for more efficient, high speed reading +int File::read(void *buf, uint16_t nbyte) { + if (_file) + return _file->read(buf, nbyte); + return 0; +} + +int File::available() { + if (! _file) return 0; + + uint32_t n = size() - position(); + + return n > 0X7FFF ? 0X7FFF : n; +} + +void File::flush() { + if (_file) + _file->sync(); +} + +boolean File::seek(uint32_t pos) { + if (! _file) return false; + + return _file->seekSet(pos); +} + +uint32_t File::position() { + if (! _file) return -1; + return _file->curPosition(); +} + +uint32_t File::size() { + if (! _file) return 0; + return _file->fileSize(); +} + +void File::close() { + if (_file) { + _file->close(); + free(_file); + _file = 0; + + /* for debugging file open/close leaks + nfilecount--; + Serial.print("Deleted "); + Serial.println(nfilecount, DEC); + */ + } +} + +File::operator bool() { + if (_file) + return _file->isOpen(); + return false; +} + diff --git a/libs/arduino-1.0/libraries/SD/README.txt b/libs/arduino-1.0/libraries/SD/README.txt new file mode 100644 index 0000000..495ea4c --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/README.txt @@ -0,0 +1,13 @@ + +** SD - a slightly more friendly wrapper for sdfatlib ** + +This library aims to expose a subset of SD card functionality in the +form of a higher level "wrapper" object. + +License: GNU General Public License V3 + (Because sdfatlib is licensed with this.) + +(C) Copyright 2010 SparkFun Electronics + +Now better than ever with optimization, multiple file support, directory handling, etc - ladyada! + diff --git a/libs/arduino-1.0/libraries/SD/SD.cpp b/libs/arduino-1.0/libraries/SD/SD.cpp new file mode 100644 index 0000000..c746809 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/SD.cpp @@ -0,0 +1,616 @@ +/* + + SD - a slightly more friendly wrapper for sdfatlib + + This library aims to expose a subset of SD card functionality + in the form of a higher level "wrapper" object. + + License: GNU General Public License V3 + (Because sdfatlib is licensed with this.) + + (C) Copyright 2010 SparkFun Electronics + + + This library provides four key benefits: + + * Including `SD.h` automatically creates a global + `SD` object which can be interacted with in a similar + manner to other standard global objects like `Serial` and `Ethernet`. + + * Boilerplate initialisation code is contained in one method named + `begin` and no further objects need to be created in order to access + the SD card. + + * Calls to `open` can supply a full path name including parent + directories which simplifies interacting with files in subdirectories. + + * Utility methods are provided to determine whether a file exists + and to create a directory heirarchy. + + + Note however that not all functionality provided by the underlying + sdfatlib library is exposed. + + */ + +/* + + Implementation Notes + + In order to handle multi-directory path traversal, functionality that + requires this ability is implemented as callback functions. + + Individual methods call the `walkPath` function which performs the actual + directory traversal (swapping between two different directory/file handles + along the way) and at each level calls the supplied callback function. + + Some types of functionality will take an action at each level (e.g. exists + or make directory) which others will only take an action at the bottom + level (e.g. open). + + */ + +#include "SD.h" + +// Used by `getNextPathComponent` +#define MAX_COMPONENT_LEN 12 // What is max length? +#define PATH_COMPONENT_BUFFER_LEN MAX_COMPONENT_LEN+1 + +bool getNextPathComponent(char *path, unsigned int *p_offset, + char *buffer) { + /* + + Parse individual path components from a path. + + e.g. after repeated calls '/foo/bar/baz' will be split + into 'foo', 'bar', 'baz'. + + This is similar to `strtok()` but copies the component into the + supplied buffer rather than modifying the original string. + + + `buffer` needs to be PATH_COMPONENT_BUFFER_LEN in size. + + `p_offset` needs to point to an integer of the offset at + which the previous path component finished. + + Returns `true` if more components remain. + + Returns `false` if this is the last component. + (This means path ended with 'foo' or 'foo/'.) + + */ + + // TODO: Have buffer local to this function, so we know it's the + // correct length? + + int bufferOffset = 0; + + int offset = *p_offset; + + // Skip root or other separator + if (path[offset] == '/') { + offset++; + } + + // Copy the next next path segment + while (bufferOffset < MAX_COMPONENT_LEN + && (path[offset] != '/') + && (path[offset] != '\0')) { + buffer[bufferOffset++] = path[offset++]; + } + + buffer[bufferOffset] = '\0'; + + // Skip trailing separator so we can determine if this + // is the last component in the path or not. + if (path[offset] == '/') { + offset++; + } + + *p_offset = offset; + + return (path[offset] != '\0'); +} + + + +boolean walkPath(char *filepath, SdFile& parentDir, + boolean (*callback)(SdFile& parentDir, + char *filePathComponent, + boolean isLastComponent, + void *object), + void *object = NULL) { + /* + + When given a file path (and parent directory--normally root), + this function traverses the directories in the path and at each + level calls the supplied callback function while also providing + the supplied object for context if required. + + e.g. given the path '/foo/bar/baz' + the callback would be called at the equivalent of + '/foo', '/foo/bar' and '/foo/bar/baz'. + + The implementation swaps between two different directory/file + handles as it traverses the directories and does not use recursion + in an attempt to use memory efficiently. + + If a callback wishes to stop the directory traversal it should + return false--in this case the function will stop the traversal, + tidy up and return false. + + If a directory path doesn't exist at some point this function will + also return false and not subsequently call the callback. + + If a directory path specified is complete, valid and the callback + did not indicate the traversal should be interrupted then this + function will return true. + + */ + + + SdFile subfile1; + SdFile subfile2; + + char buffer[PATH_COMPONENT_BUFFER_LEN]; + + unsigned int offset = 0; + + SdFile *p_parent; + SdFile *p_child; + + SdFile *p_tmp_sdfile; + + p_child = &subfile1; + + p_parent = &parentDir; + + while (true) { + + boolean moreComponents = getNextPathComponent(filepath, &offset, buffer); + + boolean shouldContinue = callback((*p_parent), buffer, !moreComponents, object); + + if (!shouldContinue) { + // TODO: Don't repeat this code? + // If it's one we've created then we + // don't need the parent handle anymore. + if (p_parent != &parentDir) { + (*p_parent).close(); + } + return false; + } + + if (!moreComponents) { + break; + } + + boolean exists = (*p_child).open(*p_parent, buffer, O_RDONLY); + + // If it's one we've created then we + // don't need the parent handle anymore. + if (p_parent != &parentDir) { + (*p_parent).close(); + } + + // Handle case when it doesn't exist and we can't continue... + if (exists) { + // We alternate between two file handles as we go down + // the path. + if (p_parent == &parentDir) { + p_parent = &subfile2; + } + + p_tmp_sdfile = p_parent; + p_parent = p_child; + p_child = p_tmp_sdfile; + } else { + return false; + } + } + + if (p_parent != &parentDir) { + (*p_parent).close(); // TODO: Return/ handle different? + } + + return true; +} + + + +/* + + The callbacks used to implement various functionality follow. + + Each callback is supplied with a parent directory handle, + character string with the name of the current file path component, + a flag indicating if this component is the last in the path and + a pointer to an arbitrary object used for context. + + */ + +boolean callback_pathExists(SdFile& parentDir, char *filePathComponent, + boolean isLastComponent, void *object) { + /* + + Callback used to determine if a file/directory exists in parent + directory. + + Returns true if file path exists. + + */ + SdFile child; + + boolean exists = child.open(parentDir, filePathComponent, O_RDONLY); + + if (exists) { + child.close(); + } + + return exists; +} + + + +boolean callback_makeDirPath(SdFile& parentDir, char *filePathComponent, + boolean isLastComponent, void *object) { + /* + + Callback used to create a directory in the parent directory if + it does not already exist. + + Returns true if a directory was created or it already existed. + + */ + boolean result = false; + SdFile child; + + result = callback_pathExists(parentDir, filePathComponent, isLastComponent, object); + if (!result) { + result = child.makeDir(parentDir, filePathComponent); + } + + return result; +} + + + /* + +boolean callback_openPath(SdFile& parentDir, char *filePathComponent, + boolean isLastComponent, void *object) { + + Callback used to open a file specified by a filepath that may + specify one or more directories above it. + + Expects the context object to be an instance of `SDClass` and + will use the `file` property of the instance to open the requested + file/directory with the associated file open mode property. + + Always returns true if the directory traversal hasn't reached the + bottom of the directory heirarchy. + + Returns false once the file has been opened--to prevent the traversal + from descending further. (This may be unnecessary.) + + if (isLastComponent) { + SDClass *p_SD = static_cast(object); + p_SD->file.open(parentDir, filePathComponent, p_SD->fileOpenMode); + if (p_SD->fileOpenMode == FILE_WRITE) { + p_SD->file.seekSet(p_SD->file.fileSize()); + } + // TODO: Return file open result? + return false; + } + return true; +} + */ + + + +boolean callback_remove(SdFile& parentDir, char *filePathComponent, + boolean isLastComponent, void *object) { + if (isLastComponent) { + return SdFile::remove(parentDir, filePathComponent); + } + return true; +} + +boolean callback_rmdir(SdFile& parentDir, char *filePathComponent, + boolean isLastComponent, void *object) { + if (isLastComponent) { + SdFile f; + if (!f.open(parentDir, filePathComponent, O_READ)) return false; + return f.rmDir(); + } + return true; +} + + + +/* Implementation of class used to create `SDCard` object. */ + + + +boolean SDClass::begin(uint8_t csPin) { + /* + + Performs the initialisation required by the sdfatlib library. + + Return true if initialization succeeds, false otherwise. + + */ + return card.init(SPI_HALF_SPEED, csPin) && + volume.init(card) && + root.openRoot(volume); +} + + + +// this little helper is used to traverse paths +SdFile SDClass::getParentDir(const char *filepath, int *index) { + // get parent directory + SdFile d1 = root; // start with the mostparent, root! + SdFile d2; + + // we'll use the pointers to swap between the two objects + SdFile *parent = &d1; + SdFile *subdir = &d2; + + const char *origpath = filepath; + + while (strchr(filepath, '/')) { + + // get rid of leading /'s + if (filepath[0] == '/') { + filepath++; + continue; + } + + if (! strchr(filepath, '/')) { + // it was in the root directory, so leave now + break; + } + + // extract just the name of the next subdirectory + uint8_t idx = strchr(filepath, '/') - filepath; + if (idx > 12) + idx = 12; // dont let them specify long names + char subdirname[13]; + strncpy(subdirname, filepath, idx); + subdirname[idx] = 0; + + // close the subdir (we reuse them) if open + subdir->close(); + if (! subdir->open(parent, subdirname, O_READ)) { + // failed to open one of the subdirectories + return SdFile(); + } + // move forward to the next subdirectory + filepath += idx; + + // we reuse the objects, close it. + parent->close(); + + // swap the pointers + SdFile *t = parent; + parent = subdir; + subdir = t; + } + + *index = (int)(filepath - origpath); + // parent is now the parent diretory of the file! + return *parent; +} + + +File SDClass::open(const char *filepath, uint8_t mode) { + /* + + Open the supplied file path for reading or writing. + + The file content can be accessed via the `file` property of + the `SDClass` object--this property is currently + a standard `SdFile` object from `sdfatlib`. + + Defaults to read only. + + If `write` is true, default action (when `append` is true) is to + append data to the end of the file. + + If `append` is false then the file will be truncated first. + + If the file does not exist and it is opened for writing the file + will be created. + + An attempt to open a file for reading that does not exist is an + error. + + */ + + int pathidx; + + // do the interative search + SdFile parentdir = getParentDir(filepath, &pathidx); + // no more subdirs! + + filepath += pathidx; + + if (! filepath[0]) { + // it was the directory itself! + return File(parentdir, "/"); + } + + // Open the file itself + SdFile file; + + // failed to open a subdir! + if (!parentdir.isOpen()) + return File(); + + // there is a special case for the Root directory since its a static dir + if (parentdir.isRoot()) { + if ( ! file.open(SD.root, filepath, mode)) { + // failed to open the file :( + return File(); + } + // dont close the root! + } else { + if ( ! file.open(parentdir, filepath, mode)) { + return File(); + } + // close the parent + parentdir.close(); + } + + if (mode & (O_APPEND | O_WRITE)) + file.seekSet(file.fileSize()); + return File(file, filepath); +} + + +/* +File SDClass::open(char *filepath, uint8_t mode) { + // + + Open the supplied file path for reading or writing. + + The file content can be accessed via the `file` property of + the `SDClass` object--this property is currently + a standard `SdFile` object from `sdfatlib`. + + Defaults to read only. + + If `write` is true, default action (when `append` is true) is to + append data to the end of the file. + + If `append` is false then the file will be truncated first. + + If the file does not exist and it is opened for writing the file + will be created. + + An attempt to open a file for reading that does not exist is an + error. + + // + + // TODO: Allow for read&write? (Possibly not, as it requires seek.) + + fileOpenMode = mode; + walkPath(filepath, root, callback_openPath, this); + + return File(); + +} +*/ + + +//boolean SDClass::close() { +// /* +// +// Closes the file opened by the `open` method. +// +// */ +// file.close(); +//} + + +boolean SDClass::exists(char *filepath) { + /* + + Returns true if the supplied file path exists. + + */ + return walkPath(filepath, root, callback_pathExists); +} + + +//boolean SDClass::exists(char *filepath, SdFile& parentDir) { +// /* +// +// Returns true if the supplied file path rooted at `parentDir` +// exists. +// +// */ +// return walkPath(filepath, parentDir, callback_pathExists); +//} + + +boolean SDClass::mkdir(char *filepath) { + /* + + Makes a single directory or a heirarchy of directories. + + A rough equivalent to `mkdir -p`. + + */ + return walkPath(filepath, root, callback_makeDirPath); +} + +boolean SDClass::rmdir(char *filepath) { + /* + + Makes a single directory or a heirarchy of directories. + + A rough equivalent to `mkdir -p`. + + */ + return walkPath(filepath, root, callback_rmdir); +} + +boolean SDClass::remove(char *filepath) { + return walkPath(filepath, root, callback_remove); +} + + +// allows you to recurse into a directory +File File::openNextFile(uint8_t mode) { + dir_t p; + + //Serial.print("\t\treading dir..."); + while (_file->readDir(&p) > 0) { + + // done if past last used entry + if (p.name[0] == DIR_NAME_FREE) { + //Serial.println("end"); + return File(); + } + + // skip deleted entry and entries for . and .. + if (p.name[0] == DIR_NAME_DELETED || p.name[0] == '.') { + //Serial.println("dots"); + continue; + } + + // only list subdirectories and files + if (!DIR_IS_FILE_OR_SUBDIR(&p)) { + //Serial.println("notafile"); + continue; + } + + // print file name with possible blank fill + SdFile f; + char name[13]; + _file->dirName(p, name); + //Serial.print("try to open file "); + //Serial.println(name); + + if (f.open(_file, name, mode)) { + //Serial.println("OK!"); + return File(f, name); + } else { + //Serial.println("ugh"); + return File(); + } + } + + //Serial.println("nothing"); + return File(); +} + +void File::rewindDirectory(void) { + if (isDirectory()) + _file->rewind(); +} + +SDClass SD; diff --git a/libs/arduino-1.0/libraries/SD/SD.h b/libs/arduino-1.0/libraries/SD/SD.h new file mode 100644 index 0000000..f21ec0f --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/SD.h @@ -0,0 +1,103 @@ +/* + + SD - a slightly more friendly wrapper for sdfatlib + + This library aims to expose a subset of SD card functionality + in the form of a higher level "wrapper" object. + + License: GNU General Public License V3 + (Because sdfatlib is licensed with this.) + + (C) Copyright 2010 SparkFun Electronics + + */ + +#ifndef __SD_H__ +#define __SD_H__ + +#include + +#include +#include + +#define FILE_READ O_READ +#define FILE_WRITE (O_READ | O_WRITE | O_CREAT) + +class File : public Stream { + private: + char _name[13]; // our name + SdFile *_file; // underlying file pointer + +public: + File(SdFile f, const char *name); // wraps an underlying SdFile + File(void); // 'empty' constructor + ~File(void); // destructor + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *buf, size_t size); + virtual int read(); + virtual int peek(); + virtual int available(); + virtual void flush(); + int read(void *buf, uint16_t nbyte); + boolean seek(uint32_t pos); + uint32_t position(); + uint32_t size(); + void close(); + operator bool(); + char * name(); + + boolean isDirectory(void); + File openNextFile(uint8_t mode = O_RDONLY); + void rewindDirectory(void); + + using Print::write; +}; + +class SDClass { + +private: + // These are required for initialisation and use of sdfatlib + Sd2Card card; + SdVolume volume; + SdFile root; + + // my quick&dirty iterator, should be replaced + SdFile getParentDir(const char *filepath, int *indx); +public: + // This needs to be called to set up the connection to the SD card + // before other methods are used. + boolean begin(uint8_t csPin = SD_CHIP_SELECT_PIN); + + // Open the specified file/directory with the supplied mode (e.g. read or + // write, etc). Returns a File object for interacting with the file. + // Note that currently only one file can be open at a time. + File open(const char *filename, uint8_t mode = FILE_READ); + + // Methods to determine if the requested file path exists. + boolean exists(char *filepath); + + // Create the requested directory heirarchy--if intermediate directories + // do not exist they will be created. + boolean mkdir(char *filepath); + + // Delete the file. + boolean remove(char *filepath); + + boolean rmdir(char *filepath); + +private: + + // This is used to determine the mode used to open a file + // it's here because it's the easiest place to pass the + // information through the directory walking function. But + // it's probably not the best place for it. + // It shouldn't be set directly--it is set via the parameters to `open`. + int fileOpenMode; + + friend class File; + friend boolean callback_openPath(SdFile&, char *, boolean, void *); +}; + +extern SDClass SD; + +#endif diff --git a/libs/arduino-1.0/libraries/SD/examples/CardInfo/CardInfo.ino b/libs/arduino-1.0/libraries/SD/examples/CardInfo/CardInfo.ino new file mode 100644 index 0000000..0c2dfc5 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/CardInfo/CardInfo.ino @@ -0,0 +1,117 @@ +/* + SD card test + + This example shows how use the utility libraries on which the' + SD library is based in order to get info about your SD card. + Very useful for testing a card when you're not sure whether its working or not. + + The circuit: + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 on Arduino Uno/Duemilanove/Diecimila + ** MISO - pin 12 on Arduino Uno/Duemilanove/Diecimila + ** CLK - pin 13 on Arduino Uno/Duemilanove/Diecimila + ** CS - depends on your SD card shield or module. + Pin 4 used here for consistency with other Arduino examples + + + created 28 Mar 2011 + by Limor Fried + modified 9 Apr 2012 + by Tom Igoe + */ + // include the SD library: +#include + +// set up variables using the SD utility library functions: +Sd2Card card; +SdVolume volume; +SdFile root; + +// change this to match your SD shield or module; +// Arduino Ethernet shield: pin 4 +// Adafruit SD shields and modules: pin 10 +// Sparkfun SD shield: pin 8 +const int chipSelect = 4; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("\nInitializing SD card..."); + // On the Ethernet Shield, CS is pin 4. It's set as an output by default. + // Note that even if it's not used as the CS pin, the hardware SS pin + // (10 on most Arduino boards, 53 on the Mega) must be left as an output + // or the SD library functions will not work. + pinMode(10, OUTPUT); // change this to 53 on a mega + + + // we'll use the initialization code from the utility libraries + // since we're just testing if the card is working! + if (!card.init(SPI_HALF_SPEED, chipSelect)) { + Serial.println("initialization failed. Things to check:"); + Serial.println("* is a card is inserted?"); + Serial.println("* Is your wiring correct?"); + Serial.println("* did you change the chipSelect pin to match your shield or module?"); + return; + } else { + Serial.println("Wiring is correct and a card is present."); + } + + // print the type of card + Serial.print("\nCard type: "); + switch(card.type()) { + case SD_CARD_TYPE_SD1: + Serial.println("SD1"); + break; + case SD_CARD_TYPE_SD2: + Serial.println("SD2"); + break; + case SD_CARD_TYPE_SDHC: + Serial.println("SDHC"); + break; + default: + Serial.println("Unknown"); + } + + // Now we will try to open the 'volume'/'partition' - it should be FAT16 or FAT32 + if (!volume.init(card)) { + Serial.println("Could not find FAT16/FAT32 partition.\nMake sure you've formatted the card"); + return; + } + + + // print the type and size of the first FAT-type volume + uint32_t volumesize; + Serial.print("\nVolume type is FAT"); + Serial.println(volume.fatType(), DEC); + Serial.println(); + + volumesize = volume.blocksPerCluster(); // clusters are collections of blocks + volumesize *= volume.clusterCount(); // we'll have a lot of clusters + volumesize *= 512; // SD card blocks are always 512 bytes + Serial.print("Volume size (bytes): "); + Serial.println(volumesize); + Serial.print("Volume size (Kbytes): "); + volumesize /= 1024; + Serial.println(volumesize); + Serial.print("Volume size (Mbytes): "); + volumesize /= 1024; + Serial.println(volumesize); + + + Serial.println("\nFiles found on the card (name, date and size in bytes): "); + root.openRoot(volume); + + // list all files in the card with date and size + root.ls(LS_R | LS_DATE | LS_SIZE); +} + + +void loop(void) { + +} diff --git a/libs/arduino-1.0/libraries/SD/examples/Datalogger/Datalogger.ino b/libs/arduino-1.0/libraries/SD/examples/Datalogger/Datalogger.ino new file mode 100644 index 0000000..a7f85ee --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/Datalogger/Datalogger.ino @@ -0,0 +1,92 @@ +/* + SD card datalogger + + This example shows how to log data from three analog sensors + to an SD card using the SD library. + + The circuit: + * analog sensors on analog ins 0, 1, and 2 + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 + ** MISO - pin 12 + ** CLK - pin 13 + ** CS - pin 4 + + created 24 Nov 2010 + modified 9 Apr 2012 + by Tom Igoe + + This example code is in the public domain. + + */ + +#include + +// On the Ethernet Shield, CS is pin 4. Note that even if it's not +// used as the CS pin, the hardware CS pin (10 on most Arduino boards, +// 53 on the Mega) must be left as an output or the SD library +// functions will not work. +const int chipSelect = 4; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Initializing SD card..."); + // make sure that the default chip select pin is set to + // output, even if you don't use it: + pinMode(10, OUTPUT); + + // see if the card is present and can be initialized: + if (!SD.begin(chipSelect)) { + Serial.println("Card failed, or not present"); + // don't do anything more: + return; + } + Serial.println("card initialized."); +} + +void loop() +{ + // make a string for assembling the data to log: + String dataString = ""; + + // read three sensors and append to the string: + for (int analogPin = 0; analogPin < 3; analogPin++) { + int sensor = analogRead(analogPin); + dataString += String(sensor); + if (analogPin < 2) { + dataString += ","; + } + } + + // open the file. note that only one file can be open at a time, + // so you have to close this one before opening another. + File dataFile = SD.open("datalog.txt", FILE_WRITE); + + // if the file is available, write to it: + if (dataFile) { + dataFile.println(dataString); + dataFile.close(); + // print to the serial port too: + Serial.println(dataString); + } + // if the file isn't open, pop up an error: + else { + Serial.println("error opening datalog.txt"); + } +} + + + + + + + + + diff --git a/libs/arduino-1.0/libraries/SD/examples/DumpFile/DumpFile.ino b/libs/arduino-1.0/libraries/SD/examples/DumpFile/DumpFile.ino new file mode 100644 index 0000000..d83089a --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/DumpFile/DumpFile.ino @@ -0,0 +1,73 @@ +/* + SD card file dump + + This example shows how to read a file from the SD card using the + SD library and send it over the serial port. + + The circuit: + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 + ** MISO - pin 12 + ** CLK - pin 13 + ** CS - pin 4 + + created 22 December 2010 + by Limor Fried + modified 9 Apr 2012 + by Tom Igoe + + This example code is in the public domain. + + */ + +#include + +// On the Ethernet Shield, CS is pin 4. Note that even if it's not +// used as the CS pin, the hardware CS pin (10 on most Arduino boards, +// 53 on the Mega) must be left as an output or the SD library +// functions will not work. +const int chipSelect = 4; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Initializing SD card..."); + // make sure that the default chip select pin is set to + // output, even if you don't use it: + pinMode(10, OUTPUT); + + // see if the card is present and can be initialized: + if (!SD.begin(chipSelect)) { + Serial.println("Card failed, or not present"); + // don't do anything more: + return; + } + Serial.println("card initialized."); + + // open the file. note that only one file can be open at a time, + // so you have to close this one before opening another. + File dataFile = SD.open("datalog.txt"); + + // if the file is available, write to it: + if (dataFile) { + while (dataFile.available()) { + Serial.write(dataFile.read()); + } + dataFile.close(); + } + // if the file isn't open, pop up an error: + else { + Serial.println("error opening datalog.txt"); + } +} + +void loop() +{ +} + diff --git a/libs/arduino-1.0/libraries/SD/examples/Files/Files.ino b/libs/arduino-1.0/libraries/SD/examples/Files/Files.ino new file mode 100644 index 0000000..a15b862 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/Files/Files.ino @@ -0,0 +1,84 @@ +/* + SD card basic file example + + This example shows how to create and destroy an SD card file + The circuit: + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 + ** MISO - pin 12 + ** CLK - pin 13 + ** CS - pin 4 + + created Nov 2010 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe + + This example code is in the public domain. + + */ +#include + +File myFile; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Initializing SD card..."); + // On the Ethernet Shield, CS is pin 4. It's set as an output by default. + // Note that even if it's not used as the CS pin, the hardware SS pin + // (10 on most Arduino boards, 53 on the Mega) must be left as an output + // or the SD library functions will not work. + pinMode(10, OUTPUT); + + if (!SD.begin(4)) { + Serial.println("initialization failed!"); + return; + } + Serial.println("initialization done."); + + if (SD.exists("example.txt")) { + Serial.println("example.txt exists."); + } + else { + Serial.println("example.txt doesn't exist."); + } + + // open a new file and immediately close it: + Serial.println("Creating example.txt..."); + myFile = SD.open("example.txt", FILE_WRITE); + myFile.close(); + + // Check to see if the file exists: + if (SD.exists("example.txt")) { + Serial.println("example.txt exists."); + } + else { + Serial.println("example.txt doesn't exist."); + } + + // delete the file: + Serial.println("Removing example.txt..."); + SD.remove("example.txt"); + + if (SD.exists("example.txt")){ + Serial.println("example.txt exists."); + } + else { + Serial.println("example.txt doesn't exist."); + } +} + +void loop() +{ + // nothing happens after setup finishes. +} + + + diff --git a/libs/arduino-1.0/libraries/SD/examples/ReadWrite/ReadWrite.ino b/libs/arduino-1.0/libraries/SD/examples/ReadWrite/ReadWrite.ino new file mode 100644 index 0000000..5805fc8 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/ReadWrite/ReadWrite.ino @@ -0,0 +1,85 @@ +/* + SD card read/write + + This example shows how to read and write data to and from an SD card file + The circuit: + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 + ** MISO - pin 12 + ** CLK - pin 13 + ** CS - pin 4 + + created Nov 2010 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe + + This example code is in the public domain. + + */ + +#include + +File myFile; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Initializing SD card..."); + // On the Ethernet Shield, CS is pin 4. It's set as an output by default. + // Note that even if it's not used as the CS pin, the hardware SS pin + // (10 on most Arduino boards, 53 on the Mega) must be left as an output + // or the SD library functions will not work. + pinMode(10, OUTPUT); + + if (!SD.begin(4)) { + Serial.println("initialization failed!"); + return; + } + Serial.println("initialization done."); + + // open the file. note that only one file can be open at a time, + // so you have to close this one before opening another. + myFile = SD.open("test.txt", FILE_WRITE); + + // if the file opened okay, write to it: + if (myFile) { + Serial.print("Writing to test.txt..."); + myFile.println("testing 1, 2, 3."); + // close the file: + myFile.close(); + Serial.println("done."); + } else { + // if the file didn't open, print an error: + Serial.println("error opening test.txt"); + } + + // re-open the file for reading: + myFile = SD.open("test.txt"); + if (myFile) { + Serial.println("test.txt:"); + + // read from the file until there's nothing else in it: + while (myFile.available()) { + Serial.write(myFile.read()); + } + // close the file: + myFile.close(); + } else { + // if the file didn't open, print an error: + Serial.println("error opening test.txt"); + } +} + +void loop() +{ + // nothing happens after setup +} + + diff --git a/libs/arduino-1.0/libraries/SD/examples/listfiles/listfiles.ino b/libs/arduino-1.0/libraries/SD/examples/listfiles/listfiles.ino new file mode 100644 index 0000000..876c3f8 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/examples/listfiles/listfiles.ino @@ -0,0 +1,83 @@ +/* + SD card basic file example + + This example shows how to create and destroy an SD card file + The circuit: + * SD card attached to SPI bus as follows: + ** MOSI - pin 11 + ** MISO - pin 12 + ** CLK - pin 13 + ** CS - pin 4 + + created Nov 2010 + by David A. Mellis + modified 9 Apr 2012 + by Tom Igoe + + This example code is in the public domain. + + */ +#include + +File root; + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.print("Initializing SD card..."); + // On the Ethernet Shield, CS is pin 4. It's set as an output by default. + // Note that even if it's not used as the CS pin, the hardware SS pin + // (10 on most Arduino boards, 53 on the Mega) must be left as an output + // or the SD library functions will not work. + pinMode(10, OUTPUT); + + if (!SD.begin(10)) { + Serial.println("initialization failed!"); + return; + } + Serial.println("initialization done."); + + root = SD.open("/"); + + printDirectory(root, 0); + + Serial.println("done!"); +} + +void loop() +{ + // nothing happens after setup finishes. +} + +void printDirectory(File dir, int numTabs) { + while(true) { + + File entry = dir.openNextFile(); + if (! entry) { + // no more files + //Serial.println("**nomorefiles**"); + break; + } + for (uint8_t i=0; i. + */ +#ifndef FatStructs_h +#define FatStructs_h +/** + * \file + * FAT file structures + */ +/* + * mostly from Microsoft document fatgen103.doc + * http://www.microsoft.com/whdc/system/platform/firmware/fatgen.mspx + */ +//------------------------------------------------------------------------------ +/** Value for byte 510 of boot block or MBR */ +uint8_t const BOOTSIG0 = 0X55; +/** Value for byte 511 of boot block or MBR */ +uint8_t const BOOTSIG1 = 0XAA; +//------------------------------------------------------------------------------ +/** + * \struct partitionTable + * \brief MBR partition table entry + * + * A partition table entry for a MBR formatted storage device. + * The MBR partition table has four entries. + */ +struct partitionTable { + /** + * Boot Indicator . Indicates whether the volume is the active + * partition. Legal values include: 0X00. Do not use for booting. + * 0X80 Active partition. + */ + uint8_t boot; + /** + * Head part of Cylinder-head-sector address of the first block in + * the partition. Legal values are 0-255. Only used in old PC BIOS. + */ + uint8_t beginHead; + /** + * Sector part of Cylinder-head-sector address of the first block in + * the partition. Legal values are 1-63. Only used in old PC BIOS. + */ + unsigned beginSector : 6; + /** High bits cylinder for first block in partition. */ + unsigned beginCylinderHigh : 2; + /** + * Combine beginCylinderLow with beginCylinderHigh. Legal values + * are 0-1023. Only used in old PC BIOS. + */ + uint8_t beginCylinderLow; + /** + * Partition type. See defines that begin with PART_TYPE_ for + * some Microsoft partition types. + */ + uint8_t type; + /** + * head part of cylinder-head-sector address of the last sector in the + * partition. Legal values are 0-255. Only used in old PC BIOS. + */ + uint8_t endHead; + /** + * Sector part of cylinder-head-sector address of the last sector in + * the partition. Legal values are 1-63. Only used in old PC BIOS. + */ + unsigned endSector : 6; + /** High bits of end cylinder */ + unsigned endCylinderHigh : 2; + /** + * Combine endCylinderLow with endCylinderHigh. Legal values + * are 0-1023. Only used in old PC BIOS. + */ + uint8_t endCylinderLow; + /** Logical block address of the first block in the partition. */ + uint32_t firstSector; + /** Length of the partition, in blocks. */ + uint32_t totalSectors; +}; +/** Type name for partitionTable */ +typedef struct partitionTable part_t; +//------------------------------------------------------------------------------ +/** + * \struct masterBootRecord + * + * \brief Master Boot Record + * + * The first block of a storage device that is formatted with a MBR. + */ +struct masterBootRecord { + /** Code Area for master boot program. */ + uint8_t codeArea[440]; + /** Optional WindowsNT disk signature. May contain more boot code. */ + uint32_t diskSignature; + /** Usually zero but may be more boot code. */ + uint16_t usuallyZero; + /** Partition tables. */ + part_t part[4]; + /** First MBR signature byte. Must be 0X55 */ + uint8_t mbrSig0; + /** Second MBR signature byte. Must be 0XAA */ + uint8_t mbrSig1; +}; +/** Type name for masterBootRecord */ +typedef struct masterBootRecord mbr_t; +//------------------------------------------------------------------------------ +/** + * \struct biosParmBlock + * + * \brief BIOS parameter block + * + * The BIOS parameter block describes the physical layout of a FAT volume. + */ +struct biosParmBlock { + /** + * Count of bytes per sector. This value may take on only the + * following values: 512, 1024, 2048 or 4096 + */ + uint16_t bytesPerSector; + /** + * Number of sectors per allocation unit. This value must be a + * power of 2 that is greater than 0. The legal values are + * 1, 2, 4, 8, 16, 32, 64, and 128. + */ + uint8_t sectorsPerCluster; + /** + * Number of sectors before the first FAT. + * This value must not be zero. + */ + uint16_t reservedSectorCount; + /** The count of FAT data structures on the volume. This field should + * always contain the value 2 for any FAT volume of any type. + */ + uint8_t fatCount; + /** + * For FAT12 and FAT16 volumes, this field contains the count of + * 32-byte directory entries in the root directory. For FAT32 volumes, + * this field must be set to 0. For FAT12 and FAT16 volumes, this + * value should always specify a count that when multiplied by 32 + * results in a multiple of bytesPerSector. FAT16 volumes should + * use the value 512. + */ + uint16_t rootDirEntryCount; + /** + * This field is the old 16-bit total count of sectors on the volume. + * This count includes the count of all sectors in all four regions + * of the volume. This field can be 0; if it is 0, then totalSectors32 + * must be non-zero. For FAT32 volumes, this field must be 0. For + * FAT12 and FAT16 volumes, this field contains the sector count, and + * totalSectors32 is 0 if the total sector count fits + * (is less than 0x10000). + */ + uint16_t totalSectors16; + /** + * This dates back to the old MS-DOS 1.x media determination and is + * no longer usually used for anything. 0xF8 is the standard value + * for fixed (non-removable) media. For removable media, 0xF0 is + * frequently used. Legal values are 0xF0 or 0xF8-0xFF. + */ + uint8_t mediaType; + /** + * Count of sectors occupied by one FAT on FAT12/FAT16 volumes. + * On FAT32 volumes this field must be 0, and sectorsPerFat32 + * contains the FAT size count. + */ + uint16_t sectorsPerFat16; + /** Sectors per track for interrupt 0x13. Not used otherwise. */ + uint16_t sectorsPerTrtack; + /** Number of heads for interrupt 0x13. Not used otherwise. */ + uint16_t headCount; + /** + * Count of hidden sectors preceding the partition that contains this + * FAT volume. This field is generally only relevant for media + * visible on interrupt 0x13. + */ + uint32_t hidddenSectors; + /** + * This field is the new 32-bit total count of sectors on the volume. + * This count includes the count of all sectors in all four regions + * of the volume. This field can be 0; if it is 0, then + * totalSectors16 must be non-zero. + */ + uint32_t totalSectors32; + /** + * Count of sectors occupied by one FAT on FAT32 volumes. + */ + uint32_t sectorsPerFat32; + /** + * This field is only defined for FAT32 media and does not exist on + * FAT12 and FAT16 media. + * Bits 0-3 -- Zero-based number of active FAT. + * Only valid if mirroring is disabled. + * Bits 4-6 -- Reserved. + * Bit 7 -- 0 means the FAT is mirrored at runtime into all FATs. + * -- 1 means only one FAT is active; it is the one referenced in bits 0-3. + * Bits 8-15 -- Reserved. + */ + uint16_t fat32Flags; + /** + * FAT32 version. High byte is major revision number. + * Low byte is minor revision number. Only 0.0 define. + */ + uint16_t fat32Version; + /** + * Cluster number of the first cluster of the root directory for FAT32. + * This usually 2 but not required to be 2. + */ + uint32_t fat32RootCluster; + /** + * Sector number of FSINFO structure in the reserved area of the + * FAT32 volume. Usually 1. + */ + uint16_t fat32FSInfo; + /** + * If non-zero, indicates the sector number in the reserved area + * of the volume of a copy of the boot record. Usually 6. + * No value other than 6 is recommended. + */ + uint16_t fat32BackBootBlock; + /** + * Reserved for future expansion. Code that formats FAT32 volumes + * should always set all of the bytes of this field to 0. + */ + uint8_t fat32Reserved[12]; +}; +/** Type name for biosParmBlock */ +typedef struct biosParmBlock bpb_t; +//------------------------------------------------------------------------------ +/** + * \struct fat32BootSector + * + * \brief Boot sector for a FAT16 or FAT32 volume. + * + */ +struct fat32BootSector { + /** X86 jmp to boot program */ + uint8_t jmpToBootCode[3]; + /** informational only - don't depend on it */ + char oemName[8]; + /** BIOS Parameter Block */ + bpb_t bpb; + /** for int0x13 use value 0X80 for hard drive */ + uint8_t driveNumber; + /** used by Windows NT - should be zero for FAT */ + uint8_t reserved1; + /** 0X29 if next three fields are valid */ + uint8_t bootSignature; + /** usually generated by combining date and time */ + uint32_t volumeSerialNumber; + /** should match volume label in root dir */ + char volumeLabel[11]; + /** informational only - don't depend on it */ + char fileSystemType[8]; + /** X86 boot code */ + uint8_t bootCode[420]; + /** must be 0X55 */ + uint8_t bootSectorSig0; + /** must be 0XAA */ + uint8_t bootSectorSig1; +}; +//------------------------------------------------------------------------------ +// End Of Chain values for FAT entries +/** FAT16 end of chain value used by Microsoft. */ +uint16_t const FAT16EOC = 0XFFFF; +/** Minimum value for FAT16 EOC. Use to test for EOC. */ +uint16_t const FAT16EOC_MIN = 0XFFF8; +/** FAT32 end of chain value used by Microsoft. */ +uint32_t const FAT32EOC = 0X0FFFFFFF; +/** Minimum value for FAT32 EOC. Use to test for EOC. */ +uint32_t const FAT32EOC_MIN = 0X0FFFFFF8; +/** Mask a for FAT32 entry. Entries are 28 bits. */ +uint32_t const FAT32MASK = 0X0FFFFFFF; + +/** Type name for fat32BootSector */ +typedef struct fat32BootSector fbs_t; +//------------------------------------------------------------------------------ +/** + * \struct directoryEntry + * \brief FAT short directory entry + * + * Short means short 8.3 name, not the entry size. + * + * Date Format. A FAT directory entry date stamp is a 16-bit field that is + * basically a date relative to the MS-DOS epoch of 01/01/1980. Here is the + * format (bit 0 is the LSB of the 16-bit word, bit 15 is the MSB of the + * 16-bit word): + * + * Bits 9-15: Count of years from 1980, valid value range 0-127 + * inclusive (1980-2107). + * + * Bits 5-8: Month of year, 1 = January, valid value range 1-12 inclusive. + * + * Bits 0-4: Day of month, valid value range 1-31 inclusive. + * + * Time Format. A FAT directory entry time stamp is a 16-bit field that has + * a granularity of 2 seconds. Here is the format (bit 0 is the LSB of the + * 16-bit word, bit 15 is the MSB of the 16-bit word). + * + * Bits 11-15: Hours, valid value range 0-23 inclusive. + * + * Bits 5-10: Minutes, valid value range 0-59 inclusive. + * + * Bits 0-4: 2-second count, valid value range 0-29 inclusive (0 - 58 seconds). + * + * The valid time range is from Midnight 00:00:00 to 23:59:58. + */ +struct directoryEntry { + /** + * Short 8.3 name. + * The first eight bytes contain the file name with blank fill. + * The last three bytes contain the file extension with blank fill. + */ + uint8_t name[11]; + /** Entry attributes. + * + * The upper two bits of the attribute byte are reserved and should + * always be set to 0 when a file is created and never modified or + * looked at after that. See defines that begin with DIR_ATT_. + */ + uint8_t attributes; + /** + * Reserved for use by Windows NT. Set value to 0 when a file is + * created and never modify or look at it after that. + */ + uint8_t reservedNT; + /** + * The granularity of the seconds part of creationTime is 2 seconds + * so this field is a count of tenths of a second and its valid + * value range is 0-199 inclusive. (WHG note - seems to be hundredths) + */ + uint8_t creationTimeTenths; + /** Time file was created. */ + uint16_t creationTime; + /** Date file was created. */ + uint16_t creationDate; + /** + * Last access date. Note that there is no last access time, only + * a date. This is the date of last read or write. In the case of + * a write, this should be set to the same date as lastWriteDate. + */ + uint16_t lastAccessDate; + /** + * High word of this entry's first cluster number (always 0 for a + * FAT12 or FAT16 volume). + */ + uint16_t firstClusterHigh; + /** Time of last write. File creation is considered a write. */ + uint16_t lastWriteTime; + /** Date of last write. File creation is considered a write. */ + uint16_t lastWriteDate; + /** Low word of this entry's first cluster number. */ + uint16_t firstClusterLow; + /** 32-bit unsigned holding this file's size in bytes. */ + uint32_t fileSize; +}; +//------------------------------------------------------------------------------ +// Definitions for directory entries +// +/** Type name for directoryEntry */ +typedef struct directoryEntry dir_t; +/** escape for name[0] = 0XE5 */ +uint8_t const DIR_NAME_0XE5 = 0X05; +/** name[0] value for entry that is free after being "deleted" */ +uint8_t const DIR_NAME_DELETED = 0XE5; +/** name[0] value for entry that is free and no allocated entries follow */ +uint8_t const DIR_NAME_FREE = 0X00; +/** file is read-only */ +uint8_t const DIR_ATT_READ_ONLY = 0X01; +/** File should hidden in directory listings */ +uint8_t const DIR_ATT_HIDDEN = 0X02; +/** Entry is for a system file */ +uint8_t const DIR_ATT_SYSTEM = 0X04; +/** Directory entry contains the volume label */ +uint8_t const DIR_ATT_VOLUME_ID = 0X08; +/** Entry is for a directory */ +uint8_t const DIR_ATT_DIRECTORY = 0X10; +/** Old DOS archive bit for backup support */ +uint8_t const DIR_ATT_ARCHIVE = 0X20; +/** Test value for long name entry. Test is + (d->attributes & DIR_ATT_LONG_NAME_MASK) == DIR_ATT_LONG_NAME. */ +uint8_t const DIR_ATT_LONG_NAME = 0X0F; +/** Test mask for long name entry */ +uint8_t const DIR_ATT_LONG_NAME_MASK = 0X3F; +/** defined attribute bits */ +uint8_t const DIR_ATT_DEFINED_BITS = 0X3F; +/** Directory entry is part of a long name */ +static inline uint8_t DIR_IS_LONG_NAME(const dir_t* dir) { + return (dir->attributes & DIR_ATT_LONG_NAME_MASK) == DIR_ATT_LONG_NAME; +} +/** Mask for file/subdirectory tests */ +uint8_t const DIR_ATT_FILE_TYPE_MASK = (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY); +/** Directory entry is for a file */ +static inline uint8_t DIR_IS_FILE(const dir_t* dir) { + return (dir->attributes & DIR_ATT_FILE_TYPE_MASK) == 0; +} +/** Directory entry is for a subdirectory */ +static inline uint8_t DIR_IS_SUBDIR(const dir_t* dir) { + return (dir->attributes & DIR_ATT_FILE_TYPE_MASK) == DIR_ATT_DIRECTORY; +} +/** Directory entry is for a file or subdirectory */ +static inline uint8_t DIR_IS_FILE_OR_SUBDIR(const dir_t* dir) { + return (dir->attributes & DIR_ATT_VOLUME_ID) == 0; +} +#endif // FatStructs_h diff --git a/libs/arduino-1.0/libraries/SD/utility/Sd2Card.cpp b/libs/arduino-1.0/libraries/SD/utility/Sd2Card.cpp new file mode 100644 index 0000000..8d272d8 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/Sd2Card.cpp @@ -0,0 +1,644 @@ +/* Arduino Sd2Card Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino Sd2Card Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino Sd2Card Library. If not, see + * . + */ +#include +#include "Sd2Card.h" +//------------------------------------------------------------------------------ +#ifndef SOFTWARE_SPI +// functions for hardware SPI +/** Send a byte to the card */ +static void spiSend(uint8_t b) { + SPDR = b; + while (!(SPSR & (1 << SPIF))); +} +/** Receive a byte from the card */ +static uint8_t spiRec(void) { + spiSend(0XFF); + return SPDR; +} +#else // SOFTWARE_SPI +//------------------------------------------------------------------------------ +/** nop to tune soft SPI timing */ +#define nop asm volatile ("nop\n\t") +//------------------------------------------------------------------------------ +/** Soft SPI receive */ +uint8_t spiRec(void) { + uint8_t data = 0; + // no interrupts during byte receive - about 8 us + cli(); + // output pin high - like sending 0XFF + fastDigitalWrite(SPI_MOSI_PIN, HIGH); + + for (uint8_t i = 0; i < 8; i++) { + fastDigitalWrite(SPI_SCK_PIN, HIGH); + + // adjust so SCK is nice + nop; + nop; + + data <<= 1; + + if (fastDigitalRead(SPI_MISO_PIN)) data |= 1; + + fastDigitalWrite(SPI_SCK_PIN, LOW); + } + // enable interrupts + sei(); + return data; +} +//------------------------------------------------------------------------------ +/** Soft SPI send */ +void spiSend(uint8_t data) { + // no interrupts during byte send - about 8 us + cli(); + for (uint8_t i = 0; i < 8; i++) { + fastDigitalWrite(SPI_SCK_PIN, LOW); + + fastDigitalWrite(SPI_MOSI_PIN, data & 0X80); + + data <<= 1; + + fastDigitalWrite(SPI_SCK_PIN, HIGH); + } + // hold SCK high for a few ns + nop; + nop; + nop; + nop; + + fastDigitalWrite(SPI_SCK_PIN, LOW); + // enable interrupts + sei(); +} +#endif // SOFTWARE_SPI +//------------------------------------------------------------------------------ +// send command and return error code. Return zero for OK +uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) { + // end read if in partialBlockRead mode + readEnd(); + + // select card + chipSelectLow(); + + // wait up to 300 ms if busy + waitNotBusy(300); + + // send command + spiSend(cmd | 0x40); + + // send argument + for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s); + + // send CRC + uint8_t crc = 0XFF; + if (cmd == CMD0) crc = 0X95; // correct crc for CMD0 with arg 0 + if (cmd == CMD8) crc = 0X87; // correct crc for CMD8 with arg 0X1AA + spiSend(crc); + + // wait for response + for (uint8_t i = 0; ((status_ = spiRec()) & 0X80) && i != 0XFF; i++); + return status_; +} +//------------------------------------------------------------------------------ +/** + * Determine the size of an SD flash memory card. + * + * \return The number of 512 byte data blocks in the card + * or zero if an error occurs. + */ +uint32_t Sd2Card::cardSize(void) { + csd_t csd; + if (!readCSD(&csd)) return 0; + if (csd.v1.csd_ver == 0) { + uint8_t read_bl_len = csd.v1.read_bl_len; + uint16_t c_size = (csd.v1.c_size_high << 10) + | (csd.v1.c_size_mid << 2) | csd.v1.c_size_low; + uint8_t c_size_mult = (csd.v1.c_size_mult_high << 1) + | csd.v1.c_size_mult_low; + return (uint32_t)(c_size + 1) << (c_size_mult + read_bl_len - 7); + } else if (csd.v2.csd_ver == 1) { + uint32_t c_size = ((uint32_t)csd.v2.c_size_high << 16) + | (csd.v2.c_size_mid << 8) | csd.v2.c_size_low; + return (c_size + 1) << 10; + } else { + error(SD_CARD_ERROR_BAD_CSD); + return 0; + } +} +//------------------------------------------------------------------------------ +void Sd2Card::chipSelectHigh(void) { + digitalWrite(chipSelectPin_, HIGH); +} +//------------------------------------------------------------------------------ +void Sd2Card::chipSelectLow(void) { + digitalWrite(chipSelectPin_, LOW); +} +//------------------------------------------------------------------------------ +/** Erase a range of blocks. + * + * \param[in] firstBlock The address of the first block in the range. + * \param[in] lastBlock The address of the last block in the range. + * + * \note This function requests the SD card to do a flash erase for a + * range of blocks. The data on the card after an erase operation is + * either 0 or 1, depends on the card vendor. The card must support + * single block erase. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::erase(uint32_t firstBlock, uint32_t lastBlock) { + if (!eraseSingleBlockEnable()) { + error(SD_CARD_ERROR_ERASE_SINGLE_BLOCK); + goto fail; + } + if (type_ != SD_CARD_TYPE_SDHC) { + firstBlock <<= 9; + lastBlock <<= 9; + } + if (cardCommand(CMD32, firstBlock) + || cardCommand(CMD33, lastBlock) + || cardCommand(CMD38, 0)) { + error(SD_CARD_ERROR_ERASE); + goto fail; + } + if (!waitNotBusy(SD_ERASE_TIMEOUT)) { + error(SD_CARD_ERROR_ERASE_TIMEOUT); + goto fail; + } + chipSelectHigh(); + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** Determine if card supports single block erase. + * + * \return The value one, true, is returned if single block erase is supported. + * The value zero, false, is returned if single block erase is not supported. + */ +uint8_t Sd2Card::eraseSingleBlockEnable(void) { + csd_t csd; + return readCSD(&csd) ? csd.v1.erase_blk_en : 0; +} +//------------------------------------------------------------------------------ +/** + * Initialize an SD flash memory card. + * + * \param[in] sckRateID SPI clock rate selector. See setSckRate(). + * \param[in] chipSelectPin SD chip select pin number. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. The reason for failure + * can be determined by calling errorCode() and errorData(). + */ +uint8_t Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) { + errorCode_ = inBlock_ = partialBlockRead_ = type_ = 0; + chipSelectPin_ = chipSelectPin; + // 16-bit init start time allows over a minute + uint16_t t0 = (uint16_t)millis(); + uint32_t arg; + + // set pin modes + pinMode(chipSelectPin_, OUTPUT); + chipSelectHigh(); + pinMode(SPI_MISO_PIN, INPUT); + pinMode(SPI_MOSI_PIN, OUTPUT); + pinMode(SPI_SCK_PIN, OUTPUT); + +#ifndef SOFTWARE_SPI + // SS must be in output mode even it is not chip select + pinMode(SS_PIN, OUTPUT); + digitalWrite(SS_PIN, HIGH); // disable any SPI device using hardware SS pin + // Enable SPI, Master, clock rate f_osc/128 + SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (1 << SPR0); + // clear double speed + SPSR &= ~(1 << SPI2X); +#endif // SOFTWARE_SPI + + // must supply min of 74 clock cycles with CS high. + for (uint8_t i = 0; i < 10; i++) spiSend(0XFF); + + chipSelectLow(); + + // command to go idle in SPI mode + while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) { + if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) { + error(SD_CARD_ERROR_CMD0); + goto fail; + } + } + // check SD version + if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) { + type(SD_CARD_TYPE_SD1); + } else { + // only need last byte of r7 response + for (uint8_t i = 0; i < 4; i++) status_ = spiRec(); + if (status_ != 0XAA) { + error(SD_CARD_ERROR_CMD8); + goto fail; + } + type(SD_CARD_TYPE_SD2); + } + // initialize card and send host supports SDHC if SD2 + arg = type() == SD_CARD_TYPE_SD2 ? 0X40000000 : 0; + + while ((status_ = cardAcmd(ACMD41, arg)) != R1_READY_STATE) { + // check for timeout + if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) { + error(SD_CARD_ERROR_ACMD41); + goto fail; + } + } + // if SD2 read OCR register to check for SDHC card + if (type() == SD_CARD_TYPE_SD2) { + if (cardCommand(CMD58, 0)) { + error(SD_CARD_ERROR_CMD58); + goto fail; + } + if ((spiRec() & 0XC0) == 0XC0) type(SD_CARD_TYPE_SDHC); + // discard rest of ocr - contains allowed voltage range + for (uint8_t i = 0; i < 3; i++) spiRec(); + } + chipSelectHigh(); + +#ifndef SOFTWARE_SPI + return setSckRate(sckRateID); +#else // SOFTWARE_SPI + return true; +#endif // SOFTWARE_SPI + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** + * Enable or disable partial block reads. + * + * Enabling partial block reads improves performance by allowing a block + * to be read over the SPI bus as several sub-blocks. Errors may occur + * if the time between reads is too long since the SD card may timeout. + * The SPI SS line will be held low until the entire block is read or + * readEnd() is called. + * + * Use this for applications like the Adafruit Wave Shield. + * + * \param[in] value The value TRUE (non-zero) or FALSE (zero).) + */ +void Sd2Card::partialBlockRead(uint8_t value) { + readEnd(); + partialBlockRead_ = value; +} +//------------------------------------------------------------------------------ +/** + * Read a 512 byte block from an SD card device. + * + * \param[in] block Logical block to be read. + * \param[out] dst Pointer to the location that will receive the data. + + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::readBlock(uint32_t block, uint8_t* dst) { + return readData(block, 0, 512, dst); +} +//------------------------------------------------------------------------------ +/** + * Read part of a 512 byte block from an SD card. + * + * \param[in] block Logical block to be read. + * \param[in] offset Number of bytes to skip at start of block + * \param[out] dst Pointer to the location that will receive the data. + * \param[in] count Number of bytes to read + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::readData(uint32_t block, + uint16_t offset, uint16_t count, uint8_t* dst) { + uint16_t n; + if (count == 0) return true; + if ((count + offset) > 512) { + goto fail; + } + if (!inBlock_ || block != block_ || offset < offset_) { + block_ = block; + // use address if not SDHC card + if (type()!= SD_CARD_TYPE_SDHC) block <<= 9; + if (cardCommand(CMD17, block)) { + error(SD_CARD_ERROR_CMD17); + goto fail; + } + if (!waitStartBlock()) { + goto fail; + } + offset_ = 0; + inBlock_ = 1; + } + +#ifdef OPTIMIZE_HARDWARE_SPI + // start first spi transfer + SPDR = 0XFF; + + // skip data before offset + for (;offset_ < offset; offset_++) { + while (!(SPSR & (1 << SPIF))); + SPDR = 0XFF; + } + // transfer data + n = count - 1; + for (uint16_t i = 0; i < n; i++) { + while (!(SPSR & (1 << SPIF))); + dst[i] = SPDR; + SPDR = 0XFF; + } + // wait for last byte + while (!(SPSR & (1 << SPIF))); + dst[n] = SPDR; + +#else // OPTIMIZE_HARDWARE_SPI + + // skip data before offset + for (;offset_ < offset; offset_++) { + spiRec(); + } + // transfer data + for (uint16_t i = 0; i < count; i++) { + dst[i] = spiRec(); + } +#endif // OPTIMIZE_HARDWARE_SPI + + offset_ += count; + if (!partialBlockRead_ || offset_ >= 512) { + // read rest of data, checksum and set chip select high + readEnd(); + } + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** Skip remaining data in a block when in partial block read mode. */ +void Sd2Card::readEnd(void) { + if (inBlock_) { + // skip data and crc +#ifdef OPTIMIZE_HARDWARE_SPI + // optimize skip for hardware + SPDR = 0XFF; + while (offset_++ < 513) { + while (!(SPSR & (1 << SPIF))); + SPDR = 0XFF; + } + // wait for last crc byte + while (!(SPSR & (1 << SPIF))); +#else // OPTIMIZE_HARDWARE_SPI + while (offset_++ < 514) spiRec(); +#endif // OPTIMIZE_HARDWARE_SPI + chipSelectHigh(); + inBlock_ = 0; + } +} +//------------------------------------------------------------------------------ +/** read CID or CSR register */ +uint8_t Sd2Card::readRegister(uint8_t cmd, void* buf) { + uint8_t* dst = reinterpret_cast(buf); + if (cardCommand(cmd, 0)) { + error(SD_CARD_ERROR_READ_REG); + goto fail; + } + if (!waitStartBlock()) goto fail; + // transfer data + for (uint16_t i = 0; i < 16; i++) dst[i] = spiRec(); + spiRec(); // get first crc byte + spiRec(); // get second crc byte + chipSelectHigh(); + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** + * Set the SPI clock rate. + * + * \param[in] sckRateID A value in the range [0, 6]. + * + * The SPI clock will be set to F_CPU/pow(2, 1 + sckRateID). The maximum + * SPI rate is F_CPU/2 for \a sckRateID = 0 and the minimum rate is F_CPU/128 + * for \a scsRateID = 6. + * + * \return The value one, true, is returned for success and the value zero, + * false, is returned for an invalid value of \a sckRateID. + */ +uint8_t Sd2Card::setSckRate(uint8_t sckRateID) { + if (sckRateID > 6) { + error(SD_CARD_ERROR_SCK_RATE); + return false; + } + // see avr processor datasheet for SPI register bit definitions + if ((sckRateID & 1) || sckRateID == 6) { + SPSR &= ~(1 << SPI2X); + } else { + SPSR |= (1 << SPI2X); + } + SPCR &= ~((1 < SD_READ_TIMEOUT) { + error(SD_CARD_ERROR_READ_TIMEOUT); + goto fail; + } + } + if (status_ != DATA_START_BLOCK) { + error(SD_CARD_ERROR_READ); + goto fail; + } + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** + * Writes a 512 byte block to an SD card. + * + * \param[in] blockNumber Logical block to be written. + * \param[in] src Pointer to the location of the data to be written. + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::writeBlock(uint32_t blockNumber, const uint8_t* src) { +#if SD_PROTECT_BLOCK_ZERO + // don't allow write to first block + if (blockNumber == 0) { + error(SD_CARD_ERROR_WRITE_BLOCK_ZERO); + goto fail; + } +#endif // SD_PROTECT_BLOCK_ZERO + + // use address if not SDHC card + if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9; + if (cardCommand(CMD24, blockNumber)) { + error(SD_CARD_ERROR_CMD24); + goto fail; + } + if (!writeData(DATA_START_BLOCK, src)) goto fail; + + // wait for flash programming to complete + if (!waitNotBusy(SD_WRITE_TIMEOUT)) { + error(SD_CARD_ERROR_WRITE_TIMEOUT); + goto fail; + } + // response is r2 so get and check two bytes for nonzero + if (cardCommand(CMD13, 0) || spiRec()) { + error(SD_CARD_ERROR_WRITE_PROGRAMMING); + goto fail; + } + chipSelectHigh(); + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** Write one data block in a multiple block write sequence */ +uint8_t Sd2Card::writeData(const uint8_t* src) { + // wait for previous write to finish + if (!waitNotBusy(SD_WRITE_TIMEOUT)) { + error(SD_CARD_ERROR_WRITE_MULTIPLE); + chipSelectHigh(); + return false; + } + return writeData(WRITE_MULTIPLE_TOKEN, src); +} +//------------------------------------------------------------------------------ +// send one block of data for write block or write multiple blocks +uint8_t Sd2Card::writeData(uint8_t token, const uint8_t* src) { +#ifdef OPTIMIZE_HARDWARE_SPI + + // send data - optimized loop + SPDR = token; + + // send two byte per iteration + for (uint16_t i = 0; i < 512; i += 2) { + while (!(SPSR & (1 << SPIF))); + SPDR = src[i]; + while (!(SPSR & (1 << SPIF))); + SPDR = src[i+1]; + } + + // wait for last data byte + while (!(SPSR & (1 << SPIF))); + +#else // OPTIMIZE_HARDWARE_SPI + spiSend(token); + for (uint16_t i = 0; i < 512; i++) { + spiSend(src[i]); + } +#endif // OPTIMIZE_HARDWARE_SPI + spiSend(0xff); // dummy crc + spiSend(0xff); // dummy crc + + status_ = spiRec(); + if ((status_ & DATA_RES_MASK) != DATA_RES_ACCEPTED) { + error(SD_CARD_ERROR_WRITE); + chipSelectHigh(); + return false; + } + return true; +} +//------------------------------------------------------------------------------ +/** Start a write multiple blocks sequence. + * + * \param[in] blockNumber Address of first block in sequence. + * \param[in] eraseCount The number of blocks to be pre-erased. + * + * \note This function is used with writeData() and writeStop() + * for optimized multiple block writes. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::writeStart(uint32_t blockNumber, uint32_t eraseCount) { +#if SD_PROTECT_BLOCK_ZERO + // don't allow write to first block + if (blockNumber == 0) { + error(SD_CARD_ERROR_WRITE_BLOCK_ZERO); + goto fail; + } +#endif // SD_PROTECT_BLOCK_ZERO + // send pre-erase count + if (cardAcmd(ACMD23, eraseCount)) { + error(SD_CARD_ERROR_ACMD23); + goto fail; + } + // use address if not SDHC card + if (type() != SD_CARD_TYPE_SDHC) blockNumber <<= 9; + if (cardCommand(CMD25, blockNumber)) { + error(SD_CARD_ERROR_CMD25); + goto fail; + } + return true; + + fail: + chipSelectHigh(); + return false; +} +//------------------------------------------------------------------------------ +/** End a write multiple blocks sequence. + * +* \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t Sd2Card::writeStop(void) { + if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail; + spiSend(STOP_TRAN_TOKEN); + if (!waitNotBusy(SD_WRITE_TIMEOUT)) goto fail; + chipSelectHigh(); + return true; + + fail: + error(SD_CARD_ERROR_STOP_TRAN); + chipSelectHigh(); + return false; +} diff --git a/libs/arduino-1.0/libraries/SD/utility/Sd2Card.h b/libs/arduino-1.0/libraries/SD/utility/Sd2Card.h new file mode 100644 index 0000000..9160c3d --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/Sd2Card.h @@ -0,0 +1,233 @@ +/* Arduino Sd2Card Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino Sd2Card Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino Sd2Card Library. If not, see + * . + */ +#ifndef Sd2Card_h +#define Sd2Card_h +/** + * \file + * Sd2Card class + */ +#include "Sd2PinMap.h" +#include "SdInfo.h" +/** Set SCK to max rate of F_CPU/2. See Sd2Card::setSckRate(). */ +uint8_t const SPI_FULL_SPEED = 0; +/** Set SCK rate to F_CPU/4. See Sd2Card::setSckRate(). */ +uint8_t const SPI_HALF_SPEED = 1; +/** Set SCK rate to F_CPU/8. Sd2Card::setSckRate(). */ +uint8_t const SPI_QUARTER_SPEED = 2; +/** + * Define MEGA_SOFT_SPI non-zero to use software SPI on Mega Arduinos. + * Pins used are SS 10, MOSI 11, MISO 12, and SCK 13. + * + * MEGA_SOFT_SPI allows an unmodified Adafruit GPS Shield to be used + * on Mega Arduinos. Software SPI works well with GPS Shield V1.1 + * but many SD cards will fail with GPS Shield V1.0. + */ +#define MEGA_SOFT_SPI 0 +//------------------------------------------------------------------------------ +#if MEGA_SOFT_SPI && (defined(__AVR_ATmega1280__)||defined(__AVR_ATmega2560__)) +#define SOFTWARE_SPI +#endif // MEGA_SOFT_SPI +//------------------------------------------------------------------------------ +// SPI pin definitions +// +#ifndef SOFTWARE_SPI +// hardware pin defs +/** + * SD Chip Select pin + * + * Warning if this pin is redefined the hardware SS will pin will be enabled + * as an output by init(). An avr processor will not function as an SPI + * master unless SS is set to output mode. + */ +/** The default chip select pin for the SD card is SS. */ +uint8_t const SD_CHIP_SELECT_PIN = SS_PIN; +// The following three pins must not be redefined for hardware SPI. +/** SPI Master Out Slave In pin */ +uint8_t const SPI_MOSI_PIN = MOSI_PIN; +/** SPI Master In Slave Out pin */ +uint8_t const SPI_MISO_PIN = MISO_PIN; +/** SPI Clock pin */ +uint8_t const SPI_SCK_PIN = SCK_PIN; +/** optimize loops for hardware SPI */ +#define OPTIMIZE_HARDWARE_SPI + +#else // SOFTWARE_SPI +// define software SPI pins so Mega can use unmodified GPS Shield +/** SPI chip select pin */ +uint8_t const SD_CHIP_SELECT_PIN = 10; +/** SPI Master Out Slave In pin */ +uint8_t const SPI_MOSI_PIN = 11; +/** SPI Master In Slave Out pin */ +uint8_t const SPI_MISO_PIN = 12; +/** SPI Clock pin */ +uint8_t const SPI_SCK_PIN = 13; +#endif // SOFTWARE_SPI +//------------------------------------------------------------------------------ +/** Protect block zero from write if nonzero */ +#define SD_PROTECT_BLOCK_ZERO 1 +/** init timeout ms */ +uint16_t const SD_INIT_TIMEOUT = 2000; +/** erase timeout ms */ +uint16_t const SD_ERASE_TIMEOUT = 10000; +/** read timeout ms */ +uint16_t const SD_READ_TIMEOUT = 300; +/** write time out ms */ +uint16_t const SD_WRITE_TIMEOUT = 600; +//------------------------------------------------------------------------------ +// SD card errors +/** timeout error for command CMD0 */ +uint8_t const SD_CARD_ERROR_CMD0 = 0X1; +/** CMD8 was not accepted - not a valid SD card*/ +uint8_t const SD_CARD_ERROR_CMD8 = 0X2; +/** card returned an error response for CMD17 (read block) */ +uint8_t const SD_CARD_ERROR_CMD17 = 0X3; +/** card returned an error response for CMD24 (write block) */ +uint8_t const SD_CARD_ERROR_CMD24 = 0X4; +/** WRITE_MULTIPLE_BLOCKS command failed */ +uint8_t const SD_CARD_ERROR_CMD25 = 0X05; +/** card returned an error response for CMD58 (read OCR) */ +uint8_t const SD_CARD_ERROR_CMD58 = 0X06; +/** SET_WR_BLK_ERASE_COUNT failed */ +uint8_t const SD_CARD_ERROR_ACMD23 = 0X07; +/** card's ACMD41 initialization process timeout */ +uint8_t const SD_CARD_ERROR_ACMD41 = 0X08; +/** card returned a bad CSR version field */ +uint8_t const SD_CARD_ERROR_BAD_CSD = 0X09; +/** erase block group command failed */ +uint8_t const SD_CARD_ERROR_ERASE = 0X0A; +/** card not capable of single block erase */ +uint8_t const SD_CARD_ERROR_ERASE_SINGLE_BLOCK = 0X0B; +/** Erase sequence timed out */ +uint8_t const SD_CARD_ERROR_ERASE_TIMEOUT = 0X0C; +/** card returned an error token instead of read data */ +uint8_t const SD_CARD_ERROR_READ = 0X0D; +/** read CID or CSD failed */ +uint8_t const SD_CARD_ERROR_READ_REG = 0X0E; +/** timeout while waiting for start of read data */ +uint8_t const SD_CARD_ERROR_READ_TIMEOUT = 0X0F; +/** card did not accept STOP_TRAN_TOKEN */ +uint8_t const SD_CARD_ERROR_STOP_TRAN = 0X10; +/** card returned an error token as a response to a write operation */ +uint8_t const SD_CARD_ERROR_WRITE = 0X11; +/** attempt to write protected block zero */ +uint8_t const SD_CARD_ERROR_WRITE_BLOCK_ZERO = 0X12; +/** card did not go ready for a multiple block write */ +uint8_t const SD_CARD_ERROR_WRITE_MULTIPLE = 0X13; +/** card returned an error to a CMD13 status check after a write */ +uint8_t const SD_CARD_ERROR_WRITE_PROGRAMMING = 0X14; +/** timeout occurred during write programming */ +uint8_t const SD_CARD_ERROR_WRITE_TIMEOUT = 0X15; +/** incorrect rate selected */ +uint8_t const SD_CARD_ERROR_SCK_RATE = 0X16; +//------------------------------------------------------------------------------ +// card types +/** Standard capacity V1 SD card */ +uint8_t const SD_CARD_TYPE_SD1 = 1; +/** Standard capacity V2 SD card */ +uint8_t const SD_CARD_TYPE_SD2 = 2; +/** High Capacity SD card */ +uint8_t const SD_CARD_TYPE_SDHC = 3; +//------------------------------------------------------------------------------ +/** + * \class Sd2Card + * \brief Raw access to SD and SDHC flash memory cards. + */ +class Sd2Card { + public: + /** Construct an instance of Sd2Card. */ + Sd2Card(void) : errorCode_(0), inBlock_(0), partialBlockRead_(0), type_(0) {} + uint32_t cardSize(void); + uint8_t erase(uint32_t firstBlock, uint32_t lastBlock); + uint8_t eraseSingleBlockEnable(void); + /** + * \return error code for last error. See Sd2Card.h for a list of error codes. + */ + uint8_t errorCode(void) const {return errorCode_;} + /** \return error data for last error. */ + uint8_t errorData(void) const {return status_;} + /** + * Initialize an SD flash memory card with default clock rate and chip + * select pin. See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin). + */ + uint8_t init(void) { + return init(SPI_FULL_SPEED, SD_CHIP_SELECT_PIN); + } + /** + * Initialize an SD flash memory card with the selected SPI clock rate + * and the default SD chip select pin. + * See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin). + */ + uint8_t init(uint8_t sckRateID) { + return init(sckRateID, SD_CHIP_SELECT_PIN); + } + uint8_t init(uint8_t sckRateID, uint8_t chipSelectPin); + void partialBlockRead(uint8_t value); + /** Returns the current value, true or false, for partial block read. */ + uint8_t partialBlockRead(void) const {return partialBlockRead_;} + uint8_t readBlock(uint32_t block, uint8_t* dst); + uint8_t readData(uint32_t block, + uint16_t offset, uint16_t count, uint8_t* dst); + /** + * Read a cards CID register. The CID contains card identification + * information such as Manufacturer ID, Product name, Product serial + * number and Manufacturing date. */ + uint8_t readCID(cid_t* cid) { + return readRegister(CMD10, cid); + } + /** + * Read a cards CSD register. The CSD contains Card-Specific Data that + * provides information regarding access to the card's contents. */ + uint8_t readCSD(csd_t* csd) { + return readRegister(CMD9, csd); + } + void readEnd(void); + uint8_t setSckRate(uint8_t sckRateID); + /** Return the card type: SD V1, SD V2 or SDHC */ + uint8_t type(void) const {return type_;} + uint8_t writeBlock(uint32_t blockNumber, const uint8_t* src); + uint8_t writeData(const uint8_t* src); + uint8_t writeStart(uint32_t blockNumber, uint32_t eraseCount); + uint8_t writeStop(void); + private: + uint32_t block_; + uint8_t chipSelectPin_; + uint8_t errorCode_; + uint8_t inBlock_; + uint16_t offset_; + uint8_t partialBlockRead_; + uint8_t status_; + uint8_t type_; + // private functions + uint8_t cardAcmd(uint8_t cmd, uint32_t arg) { + cardCommand(CMD55, 0); + return cardCommand(cmd, arg); + } + uint8_t cardCommand(uint8_t cmd, uint32_t arg); + void error(uint8_t code) {errorCode_ = code;} + uint8_t readRegister(uint8_t cmd, void* buf); + uint8_t sendWriteCommand(uint32_t blockNumber, uint32_t eraseCount); + void chipSelectHigh(void); + void chipSelectLow(void); + void type(uint8_t value) {type_ = value;} + uint8_t waitNotBusy(uint16_t timeoutMillis); + uint8_t writeData(uint8_t token, const uint8_t* src); + uint8_t waitStartBlock(void); +}; +#endif // Sd2Card_h diff --git a/libs/arduino-1.0/libraries/SD/utility/Sd2PinMap.h b/libs/arduino-1.0/libraries/SD/utility/Sd2PinMap.h new file mode 100644 index 0000000..c5c29c6 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/Sd2PinMap.h @@ -0,0 +1,352 @@ +/* Arduino SdFat Library + * Copyright (C) 2010 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ +// Warning this file was generated by a program. +#ifndef Sd2PinMap_h +#define Sd2PinMap_h +#include + +//------------------------------------------------------------------------------ +/** struct for mapping digital pins */ +struct pin_map_t { + volatile uint8_t* ddr; + volatile uint8_t* pin; + volatile uint8_t* port; + uint8_t bit; +}; +//------------------------------------------------------------------------------ +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +// Mega + +// Two Wire (aka I2C) ports +uint8_t const SDA_PIN = 20; +uint8_t const SCL_PIN = 21; + +// SPI port +uint8_t const SS_PIN = 53; +uint8_t const MOSI_PIN = 51; +uint8_t const MISO_PIN = 50; +uint8_t const SCK_PIN = 52; + +static const pin_map_t digitalPinMap[] = { + {&DDRE, &PINE, &PORTE, 0}, // E0 0 + {&DDRE, &PINE, &PORTE, 1}, // E1 1 + {&DDRE, &PINE, &PORTE, 4}, // E4 2 + {&DDRE, &PINE, &PORTE, 5}, // E5 3 + {&DDRG, &PING, &PORTG, 5}, // G5 4 + {&DDRE, &PINE, &PORTE, 3}, // E3 5 + {&DDRH, &PINH, &PORTH, 3}, // H3 6 + {&DDRH, &PINH, &PORTH, 4}, // H4 7 + {&DDRH, &PINH, &PORTH, 5}, // H5 8 + {&DDRH, &PINH, &PORTH, 6}, // H6 9 + {&DDRB, &PINB, &PORTB, 4}, // B4 10 + {&DDRB, &PINB, &PORTB, 5}, // B5 11 + {&DDRB, &PINB, &PORTB, 6}, // B6 12 + {&DDRB, &PINB, &PORTB, 7}, // B7 13 + {&DDRJ, &PINJ, &PORTJ, 1}, // J1 14 + {&DDRJ, &PINJ, &PORTJ, 0}, // J0 15 + {&DDRH, &PINH, &PORTH, 1}, // H1 16 + {&DDRH, &PINH, &PORTH, 0}, // H0 17 + {&DDRD, &PIND, &PORTD, 3}, // D3 18 + {&DDRD, &PIND, &PORTD, 2}, // D2 19 + {&DDRD, &PIND, &PORTD, 1}, // D1 20 + {&DDRD, &PIND, &PORTD, 0}, // D0 21 + {&DDRA, &PINA, &PORTA, 0}, // A0 22 + {&DDRA, &PINA, &PORTA, 1}, // A1 23 + {&DDRA, &PINA, &PORTA, 2}, // A2 24 + {&DDRA, &PINA, &PORTA, 3}, // A3 25 + {&DDRA, &PINA, &PORTA, 4}, // A4 26 + {&DDRA, &PINA, &PORTA, 5}, // A5 27 + {&DDRA, &PINA, &PORTA, 6}, // A6 28 + {&DDRA, &PINA, &PORTA, 7}, // A7 29 + {&DDRC, &PINC, &PORTC, 7}, // C7 30 + {&DDRC, &PINC, &PORTC, 6}, // C6 31 + {&DDRC, &PINC, &PORTC, 5}, // C5 32 + {&DDRC, &PINC, &PORTC, 4}, // C4 33 + {&DDRC, &PINC, &PORTC, 3}, // C3 34 + {&DDRC, &PINC, &PORTC, 2}, // C2 35 + {&DDRC, &PINC, &PORTC, 1}, // C1 36 + {&DDRC, &PINC, &PORTC, 0}, // C0 37 + {&DDRD, &PIND, &PORTD, 7}, // D7 38 + {&DDRG, &PING, &PORTG, 2}, // G2 39 + {&DDRG, &PING, &PORTG, 1}, // G1 40 + {&DDRG, &PING, &PORTG, 0}, // G0 41 + {&DDRL, &PINL, &PORTL, 7}, // L7 42 + {&DDRL, &PINL, &PORTL, 6}, // L6 43 + {&DDRL, &PINL, &PORTL, 5}, // L5 44 + {&DDRL, &PINL, &PORTL, 4}, // L4 45 + {&DDRL, &PINL, &PORTL, 3}, // L3 46 + {&DDRL, &PINL, &PORTL, 2}, // L2 47 + {&DDRL, &PINL, &PORTL, 1}, // L1 48 + {&DDRL, &PINL, &PORTL, 0}, // L0 49 + {&DDRB, &PINB, &PORTB, 3}, // B3 50 + {&DDRB, &PINB, &PORTB, 2}, // B2 51 + {&DDRB, &PINB, &PORTB, 1}, // B1 52 + {&DDRB, &PINB, &PORTB, 0}, // B0 53 + {&DDRF, &PINF, &PORTF, 0}, // F0 54 + {&DDRF, &PINF, &PORTF, 1}, // F1 55 + {&DDRF, &PINF, &PORTF, 2}, // F2 56 + {&DDRF, &PINF, &PORTF, 3}, // F3 57 + {&DDRF, &PINF, &PORTF, 4}, // F4 58 + {&DDRF, &PINF, &PORTF, 5}, // F5 59 + {&DDRF, &PINF, &PORTF, 6}, // F6 60 + {&DDRF, &PINF, &PORTF, 7}, // F7 61 + {&DDRK, &PINK, &PORTK, 0}, // K0 62 + {&DDRK, &PINK, &PORTK, 1}, // K1 63 + {&DDRK, &PINK, &PORTK, 2}, // K2 64 + {&DDRK, &PINK, &PORTK, 3}, // K3 65 + {&DDRK, &PINK, &PORTK, 4}, // K4 66 + {&DDRK, &PINK, &PORTK, 5}, // K5 67 + {&DDRK, &PINK, &PORTK, 6}, // K6 68 + {&DDRK, &PINK, &PORTK, 7} // K7 69 +}; +//------------------------------------------------------------------------------ +#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) +// Sanguino + +// Two Wire (aka I2C) ports +uint8_t const SDA_PIN = 17; +uint8_t const SCL_PIN = 18; + +// SPI port +uint8_t const SS_PIN = 4; +uint8_t const MOSI_PIN = 5; +uint8_t const MISO_PIN = 6; +uint8_t const SCK_PIN = 7; + +static const pin_map_t digitalPinMap[] = { + {&DDRB, &PINB, &PORTB, 0}, // B0 0 + {&DDRB, &PINB, &PORTB, 1}, // B1 1 + {&DDRB, &PINB, &PORTB, 2}, // B2 2 + {&DDRB, &PINB, &PORTB, 3}, // B3 3 + {&DDRB, &PINB, &PORTB, 4}, // B4 4 + {&DDRB, &PINB, &PORTB, 5}, // B5 5 + {&DDRB, &PINB, &PORTB, 6}, // B6 6 + {&DDRB, &PINB, &PORTB, 7}, // B7 7 + {&DDRD, &PIND, &PORTD, 0}, // D0 8 + {&DDRD, &PIND, &PORTD, 1}, // D1 9 + {&DDRD, &PIND, &PORTD, 2}, // D2 10 + {&DDRD, &PIND, &PORTD, 3}, // D3 11 + {&DDRD, &PIND, &PORTD, 4}, // D4 12 + {&DDRD, &PIND, &PORTD, 5}, // D5 13 + {&DDRD, &PIND, &PORTD, 6}, // D6 14 + {&DDRD, &PIND, &PORTD, 7}, // D7 15 + {&DDRC, &PINC, &PORTC, 0}, // C0 16 + {&DDRC, &PINC, &PORTC, 1}, // C1 17 + {&DDRC, &PINC, &PORTC, 2}, // C2 18 + {&DDRC, &PINC, &PORTC, 3}, // C3 19 + {&DDRC, &PINC, &PORTC, 4}, // C4 20 + {&DDRC, &PINC, &PORTC, 5}, // C5 21 + {&DDRC, &PINC, &PORTC, 6}, // C6 22 + {&DDRC, &PINC, &PORTC, 7}, // C7 23 + {&DDRA, &PINA, &PORTA, 7}, // A7 24 + {&DDRA, &PINA, &PORTA, 6}, // A6 25 + {&DDRA, &PINA, &PORTA, 5}, // A5 26 + {&DDRA, &PINA, &PORTA, 4}, // A4 27 + {&DDRA, &PINA, &PORTA, 3}, // A3 28 + {&DDRA, &PINA, &PORTA, 2}, // A2 29 + {&DDRA, &PINA, &PORTA, 1}, // A1 30 + {&DDRA, &PINA, &PORTA, 0} // A0 31 +}; +//------------------------------------------------------------------------------ +#elif defined(__AVR_ATmega32U4__) +// Leonardo + +// Two Wire (aka I2C) ports +uint8_t const SDA_PIN = 2; +uint8_t const SCL_PIN = 3; + +// SPI port +uint8_t const SS_PIN = 17; +uint8_t const MOSI_PIN = 16; +uint8_t const MISO_PIN = 14; +uint8_t const SCK_PIN = 15; + +static const pin_map_t digitalPinMap[] = { + {&DDRD, &PIND, &PORTD, 2}, // D2 0 + {&DDRD, &PIND, &PORTD, 3}, // D3 1 + {&DDRD, &PIND, &PORTD, 1}, // D1 2 + {&DDRD, &PIND, &PORTD, 0}, // D0 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRC, &PINC, &PORTC, 6}, // C6 5 + {&DDRD, &PIND, &PORTD, 7}, // D7 6 + {&DDRE, &PINE, &PORTE, 6}, // E6 7 + {&DDRB, &PINB, &PORTB, 4}, // B4 8 + {&DDRB, &PINB, &PORTB, 5}, // B5 9 + {&DDRB, &PINB, &PORTB, 6}, // B6 10 + {&DDRB, &PINB, &PORTB, 7}, // B7 11 + {&DDRD, &PIND, &PORTD, 6}, // D6 12 + {&DDRC, &PINC, &PORTC, 7}, // C7 13 + {&DDRB, &PINB, &PORTB, 3}, // B3 14 + {&DDRB, &PINB, &PORTB, 1}, // B1 15 + {&DDRB, &PINB, &PORTB, 2}, // B2 16 + {&DDRB, &PINB, &PORTB, 0}, // B0 17 + {&DDRF, &PINF, &PORTF, 7}, // F7 18 + {&DDRF, &PINF, &PORTF, 6}, // F6 19 + {&DDRF, &PINF, &PORTF, 5}, // F5 20 + {&DDRF, &PINF, &PORTF, 4}, // F4 21 + {&DDRF, &PINF, &PORTF, 1}, // F1 22 + {&DDRF, &PINF, &PORTF, 0}, // F0 23 +}; +//------------------------------------------------------------------------------ +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) +// Teensy++ 1.0 & 2.0 + +// Two Wire (aka I2C) ports +uint8_t const SDA_PIN = 1; +uint8_t const SCL_PIN = 0; + +// SPI port +uint8_t const SS_PIN = 20; +uint8_t const MOSI_PIN = 22; +uint8_t const MISO_PIN = 23; +uint8_t const SCK_PIN = 21; + +static const pin_map_t digitalPinMap[] = { + {&DDRD, &PIND, &PORTD, 0}, // D0 0 + {&DDRD, &PIND, &PORTD, 1}, // D1 1 + {&DDRD, &PIND, &PORTD, 2}, // D2 2 + {&DDRD, &PIND, &PORTD, 3}, // D3 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRD, &PIND, &PORTD, 5}, // D5 5 + {&DDRD, &PIND, &PORTD, 6}, // D6 6 + {&DDRD, &PIND, &PORTD, 7}, // D7 7 + {&DDRE, &PINE, &PORTE, 0}, // E0 8 + {&DDRE, &PINE, &PORTE, 1}, // E1 9 + {&DDRC, &PINC, &PORTC, 0}, // C0 10 + {&DDRC, &PINC, &PORTC, 1}, // C1 11 + {&DDRC, &PINC, &PORTC, 2}, // C2 12 + {&DDRC, &PINC, &PORTC, 3}, // C3 13 + {&DDRC, &PINC, &PORTC, 4}, // C4 14 + {&DDRC, &PINC, &PORTC, 5}, // C5 15 + {&DDRC, &PINC, &PORTC, 6}, // C6 16 + {&DDRC, &PINC, &PORTC, 7}, // C7 17 + {&DDRE, &PINE, &PORTE, 6}, // E6 18 + {&DDRE, &PINE, &PORTE, 7}, // E7 19 + {&DDRB, &PINB, &PORTB, 0}, // B0 20 + {&DDRB, &PINB, &PORTB, 1}, // B1 21 + {&DDRB, &PINB, &PORTB, 2}, // B2 22 + {&DDRB, &PINB, &PORTB, 3}, // B3 23 + {&DDRB, &PINB, &PORTB, 4}, // B4 24 + {&DDRB, &PINB, &PORTB, 5}, // B5 25 + {&DDRB, &PINB, &PORTB, 6}, // B6 26 + {&DDRB, &PINB, &PORTB, 7}, // B7 27 + {&DDRA, &PINA, &PORTA, 0}, // A0 28 + {&DDRA, &PINA, &PORTA, 1}, // A1 29 + {&DDRA, &PINA, &PORTA, 2}, // A2 30 + {&DDRA, &PINA, &PORTA, 3}, // A3 31 + {&DDRA, &PINA, &PORTA, 4}, // A4 32 + {&DDRA, &PINA, &PORTA, 5}, // A5 33 + {&DDRA, &PINA, &PORTA, 6}, // A6 34 + {&DDRA, &PINA, &PORTA, 7}, // A7 35 + {&DDRE, &PINE, &PORTE, 4}, // E4 36 + {&DDRE, &PINE, &PORTE, 5}, // E5 37 + {&DDRF, &PINF, &PORTF, 0}, // F0 38 + {&DDRF, &PINF, &PORTF, 1}, // F1 39 + {&DDRF, &PINF, &PORTF, 2}, // F2 40 + {&DDRF, &PINF, &PORTF, 3}, // F3 41 + {&DDRF, &PINF, &PORTF, 4}, // F4 42 + {&DDRF, &PINF, &PORTF, 5}, // F5 43 + {&DDRF, &PINF, &PORTF, 6}, // F6 44 + {&DDRF, &PINF, &PORTF, 7} // F7 45 +}; +//------------------------------------------------------------------------------ +#else // defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +// 168 and 328 Arduinos + +// Two Wire (aka I2C) ports +uint8_t const SDA_PIN = 18; +uint8_t const SCL_PIN = 19; + +// SPI port +uint8_t const SS_PIN = 10; +uint8_t const MOSI_PIN = 11; +uint8_t const MISO_PIN = 12; +uint8_t const SCK_PIN = 13; + +static const pin_map_t digitalPinMap[] = { + {&DDRD, &PIND, &PORTD, 0}, // D0 0 + {&DDRD, &PIND, &PORTD, 1}, // D1 1 + {&DDRD, &PIND, &PORTD, 2}, // D2 2 + {&DDRD, &PIND, &PORTD, 3}, // D3 3 + {&DDRD, &PIND, &PORTD, 4}, // D4 4 + {&DDRD, &PIND, &PORTD, 5}, // D5 5 + {&DDRD, &PIND, &PORTD, 6}, // D6 6 + {&DDRD, &PIND, &PORTD, 7}, // D7 7 + {&DDRB, &PINB, &PORTB, 0}, // B0 8 + {&DDRB, &PINB, &PORTB, 1}, // B1 9 + {&DDRB, &PINB, &PORTB, 2}, // B2 10 + {&DDRB, &PINB, &PORTB, 3}, // B3 11 + {&DDRB, &PINB, &PORTB, 4}, // B4 12 + {&DDRB, &PINB, &PORTB, 5}, // B5 13 + {&DDRC, &PINC, &PORTC, 0}, // C0 14 + {&DDRC, &PINC, &PORTC, 1}, // C1 15 + {&DDRC, &PINC, &PORTC, 2}, // C2 16 + {&DDRC, &PINC, &PORTC, 3}, // C3 17 + {&DDRC, &PINC, &PORTC, 4}, // C4 18 + {&DDRC, &PINC, &PORTC, 5} // C5 19 +}; +#endif // defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +//------------------------------------------------------------------------------ +static const uint8_t digitalPinCount = sizeof(digitalPinMap)/sizeof(pin_map_t); + +uint8_t badPinNumber(void) + __attribute__((error("Pin number is too large or not a constant"))); + +static inline __attribute__((always_inline)) + uint8_t getPinMode(uint8_t pin) { + if (__builtin_constant_p(pin) && pin < digitalPinCount) { + return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1; + } else { + return badPinNumber(); + } +} +static inline __attribute__((always_inline)) + void setPinMode(uint8_t pin, uint8_t mode) { + if (__builtin_constant_p(pin) && pin < digitalPinCount) { + if (mode) { + *digitalPinMap[pin].ddr |= 1 << digitalPinMap[pin].bit; + } else { + *digitalPinMap[pin].ddr &= ~(1 << digitalPinMap[pin].bit); + } + } else { + badPinNumber(); + } +} +static inline __attribute__((always_inline)) + uint8_t fastDigitalRead(uint8_t pin) { + if (__builtin_constant_p(pin) && pin < digitalPinCount) { + return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1; + } else { + return badPinNumber(); + } +} +static inline __attribute__((always_inline)) + void fastDigitalWrite(uint8_t pin, uint8_t value) { + if (__builtin_constant_p(pin) && pin < digitalPinCount) { + if (value) { + *digitalPinMap[pin].port |= 1 << digitalPinMap[pin].bit; + } else { + *digitalPinMap[pin].port &= ~(1 << digitalPinMap[pin].bit); + } + } else { + badPinNumber(); + } +} +#endif // Sd2PinMap_h diff --git a/libs/arduino-1.0/libraries/SD/utility/SdFat.h b/libs/arduino-1.0/libraries/SD/utility/SdFat.h new file mode 100644 index 0000000..2c7149c --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdFat.h @@ -0,0 +1,547 @@ +/* Arduino SdFat Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ +#ifndef SdFat_h +#define SdFat_h +/** + * \file + * SdFile and SdVolume classes + */ +#include +#include "Sd2Card.h" +#include "FatStructs.h" +#include "Print.h" +//------------------------------------------------------------------------------ +/** + * Allow use of deprecated functions if non-zero + */ +#define ALLOW_DEPRECATED_FUNCTIONS 1 +//------------------------------------------------------------------------------ +// forward declaration since SdVolume is used in SdFile +class SdVolume; +//============================================================================== +// SdFile class + +// flags for ls() +/** ls() flag to print modify date */ +uint8_t const LS_DATE = 1; +/** ls() flag to print file size */ +uint8_t const LS_SIZE = 2; +/** ls() flag for recursive list of subdirectories */ +uint8_t const LS_R = 4; + +// use the gnu style oflag in open() +/** open() oflag for reading */ +uint8_t const O_READ = 0X01; +/** open() oflag - same as O_READ */ +uint8_t const O_RDONLY = O_READ; +/** open() oflag for write */ +uint8_t const O_WRITE = 0X02; +/** open() oflag - same as O_WRITE */ +uint8_t const O_WRONLY = O_WRITE; +/** open() oflag for reading and writing */ +uint8_t const O_RDWR = (O_READ | O_WRITE); +/** open() oflag mask for access modes */ +uint8_t const O_ACCMODE = (O_READ | O_WRITE); +/** The file offset shall be set to the end of the file prior to each write. */ +uint8_t const O_APPEND = 0X04; +/** synchronous writes - call sync() after each write */ +uint8_t const O_SYNC = 0X08; +/** create the file if nonexistent */ +uint8_t const O_CREAT = 0X10; +/** If O_CREAT and O_EXCL are set, open() shall fail if the file exists */ +uint8_t const O_EXCL = 0X20; +/** truncate the file to zero length */ +uint8_t const O_TRUNC = 0X40; + +// flags for timestamp +/** set the file's last access date */ +uint8_t const T_ACCESS = 1; +/** set the file's creation date and time */ +uint8_t const T_CREATE = 2; +/** Set the file's write date and time */ +uint8_t const T_WRITE = 4; +// values for type_ +/** This SdFile has not been opened. */ +uint8_t const FAT_FILE_TYPE_CLOSED = 0; +/** SdFile for a file */ +uint8_t const FAT_FILE_TYPE_NORMAL = 1; +/** SdFile for a FAT16 root directory */ +uint8_t const FAT_FILE_TYPE_ROOT16 = 2; +/** SdFile for a FAT32 root directory */ +uint8_t const FAT_FILE_TYPE_ROOT32 = 3; +/** SdFile for a subdirectory */ +uint8_t const FAT_FILE_TYPE_SUBDIR = 4; +/** Test value for directory type */ +uint8_t const FAT_FILE_TYPE_MIN_DIR = FAT_FILE_TYPE_ROOT16; + +/** date field for FAT directory entry */ +static inline uint16_t FAT_DATE(uint16_t year, uint8_t month, uint8_t day) { + return (year - 1980) << 9 | month << 5 | day; +} +/** year part of FAT directory date field */ +static inline uint16_t FAT_YEAR(uint16_t fatDate) { + return 1980 + (fatDate >> 9); +} +/** month part of FAT directory date field */ +static inline uint8_t FAT_MONTH(uint16_t fatDate) { + return (fatDate >> 5) & 0XF; +} +/** day part of FAT directory date field */ +static inline uint8_t FAT_DAY(uint16_t fatDate) { + return fatDate & 0X1F; +} +/** time field for FAT directory entry */ +static inline uint16_t FAT_TIME(uint8_t hour, uint8_t minute, uint8_t second) { + return hour << 11 | minute << 5 | second >> 1; +} +/** hour part of FAT directory time field */ +static inline uint8_t FAT_HOUR(uint16_t fatTime) { + return fatTime >> 11; +} +/** minute part of FAT directory time field */ +static inline uint8_t FAT_MINUTE(uint16_t fatTime) { + return(fatTime >> 5) & 0X3F; +} +/** second part of FAT directory time field */ +static inline uint8_t FAT_SECOND(uint16_t fatTime) { + return 2*(fatTime & 0X1F); +} +/** Default date for file timestamps is 1 Jan 2000 */ +uint16_t const FAT_DEFAULT_DATE = ((2000 - 1980) << 9) | (1 << 5) | 1; +/** Default time for file timestamp is 1 am */ +uint16_t const FAT_DEFAULT_TIME = (1 << 11); +//------------------------------------------------------------------------------ +/** + * \class SdFile + * \brief Access FAT16 and FAT32 files on SD and SDHC cards. + */ +class SdFile : public Print { + public: + /** Create an instance of SdFile. */ + SdFile(void) : type_(FAT_FILE_TYPE_CLOSED) {} + /** + * writeError is set to true if an error occurs during a write(). + * Set writeError to false before calling print() and/or write() and check + * for true after calls to print() and/or write(). + */ + //bool writeError; + /** + * Cancel unbuffered reads for this file. + * See setUnbufferedRead() + */ + void clearUnbufferedRead(void) { + flags_ &= ~F_FILE_UNBUFFERED_READ; + } + uint8_t close(void); + uint8_t contiguousRange(uint32_t* bgnBlock, uint32_t* endBlock); + uint8_t createContiguous(SdFile* dirFile, + const char* fileName, uint32_t size); + /** \return The current cluster number for a file or directory. */ + uint32_t curCluster(void) const {return curCluster_;} + /** \return The current position for a file or directory. */ + uint32_t curPosition(void) const {return curPosition_;} + /** + * Set the date/time callback function + * + * \param[in] dateTime The user's call back function. The callback + * function is of the form: + * + * \code + * void dateTime(uint16_t* date, uint16_t* time) { + * uint16_t year; + * uint8_t month, day, hour, minute, second; + * + * // User gets date and time from GPS or real-time clock here + * + * // return date using FAT_DATE macro to format fields + * *date = FAT_DATE(year, month, day); + * + * // return time using FAT_TIME macro to format fields + * *time = FAT_TIME(hour, minute, second); + * } + * \endcode + * + * Sets the function that is called when a file is created or when + * a file's directory entry is modified by sync(). All timestamps, + * access, creation, and modify, are set when a file is created. + * sync() maintains the last access date and last modify date/time. + * + * See the timestamp() function. + */ + static void dateTimeCallback( + void (*dateTime)(uint16_t* date, uint16_t* time)) { + dateTime_ = dateTime; + } + /** + * Cancel the date/time callback function. + */ + static void dateTimeCallbackCancel(void) { + // use explicit zero since NULL is not defined for Sanguino + dateTime_ = 0; + } + /** \return Address of the block that contains this file's directory. */ + uint32_t dirBlock(void) const {return dirBlock_;} + uint8_t dirEntry(dir_t* dir); + /** \return Index of this file's directory in the block dirBlock. */ + uint8_t dirIndex(void) const {return dirIndex_;} + static void dirName(const dir_t& dir, char* name); + /** \return The total number of bytes in a file or directory. */ + uint32_t fileSize(void) const {return fileSize_;} + /** \return The first cluster number for a file or directory. */ + uint32_t firstCluster(void) const {return firstCluster_;} + /** \return True if this is a SdFile for a directory else false. */ + uint8_t isDir(void) const {return type_ >= FAT_FILE_TYPE_MIN_DIR;} + /** \return True if this is a SdFile for a file else false. */ + uint8_t isFile(void) const {return type_ == FAT_FILE_TYPE_NORMAL;} + /** \return True if this is a SdFile for an open file/directory else false. */ + uint8_t isOpen(void) const {return type_ != FAT_FILE_TYPE_CLOSED;} + /** \return True if this is a SdFile for a subdirectory else false. */ + uint8_t isSubDir(void) const {return type_ == FAT_FILE_TYPE_SUBDIR;} + /** \return True if this is a SdFile for the root directory. */ + uint8_t isRoot(void) const { + return type_ == FAT_FILE_TYPE_ROOT16 || type_ == FAT_FILE_TYPE_ROOT32; + } + void ls(uint8_t flags = 0, uint8_t indent = 0); + uint8_t makeDir(SdFile* dir, const char* dirName); + uint8_t open(SdFile* dirFile, uint16_t index, uint8_t oflag); + uint8_t open(SdFile* dirFile, const char* fileName, uint8_t oflag); + + uint8_t openRoot(SdVolume* vol); + static void printDirName(const dir_t& dir, uint8_t width); + static void printFatDate(uint16_t fatDate); + static void printFatTime(uint16_t fatTime); + static void printTwoDigits(uint8_t v); + /** + * Read the next byte from a file. + * + * \return For success read returns the next byte in the file as an int. + * If an error occurs or end of file is reached -1 is returned. + */ + int16_t read(void) { + uint8_t b; + return read(&b, 1) == 1 ? b : -1; + } + int16_t read(void* buf, uint16_t nbyte); + int8_t readDir(dir_t* dir); + static uint8_t remove(SdFile* dirFile, const char* fileName); + uint8_t remove(void); + /** Set the file's current position to zero. */ + void rewind(void) { + curPosition_ = curCluster_ = 0; + } + uint8_t rmDir(void); + uint8_t rmRfStar(void); + /** Set the files position to current position + \a pos. See seekSet(). */ + uint8_t seekCur(uint32_t pos) { + return seekSet(curPosition_ + pos); + } + /** + * Set the files current position to end of file. Useful to position + * a file for append. See seekSet(). + */ + uint8_t seekEnd(void) {return seekSet(fileSize_);} + uint8_t seekSet(uint32_t pos); + /** + * Use unbuffered reads to access this file. Used with Wave + * Shield ISR. Used with Sd2Card::partialBlockRead() in WaveRP. + * + * Not recommended for normal applications. + */ + void setUnbufferedRead(void) { + if (isFile()) flags_ |= F_FILE_UNBUFFERED_READ; + } + uint8_t timestamp(uint8_t flag, uint16_t year, uint8_t month, uint8_t day, + uint8_t hour, uint8_t minute, uint8_t second); + uint8_t sync(void); + /** Type of this SdFile. You should use isFile() or isDir() instead of type() + * if possible. + * + * \return The file or directory type. + */ + uint8_t type(void) const {return type_;} + uint8_t truncate(uint32_t size); + /** \return Unbuffered read flag. */ + uint8_t unbufferedRead(void) const { + return flags_ & F_FILE_UNBUFFERED_READ; + } + /** \return SdVolume that contains this file. */ + SdVolume* volume(void) const {return vol_;} + size_t write(uint8_t b); + size_t write(const void* buf, uint16_t nbyte); + size_t write(const char* str); + void write_P(PGM_P str); + void writeln_P(PGM_P str); +//------------------------------------------------------------------------------ +#if ALLOW_DEPRECATED_FUNCTIONS +// Deprecated functions - suppress cpplint warnings with NOLINT comment + /** \deprecated Use: + * uint8_t SdFile::contiguousRange(uint32_t* bgnBlock, uint32_t* endBlock); + */ + uint8_t contiguousRange(uint32_t& bgnBlock, uint32_t& endBlock) { // NOLINT + return contiguousRange(&bgnBlock, &endBlock); + } + /** \deprecated Use: + * uint8_t SdFile::createContiguous(SdFile* dirFile, + * const char* fileName, uint32_t size) + */ + uint8_t createContiguous(SdFile& dirFile, // NOLINT + const char* fileName, uint32_t size) { + return createContiguous(&dirFile, fileName, size); + } + + /** + * \deprecated Use: + * static void SdFile::dateTimeCallback( + * void (*dateTime)(uint16_t* date, uint16_t* time)); + */ + static void dateTimeCallback( + void (*dateTime)(uint16_t& date, uint16_t& time)) { // NOLINT + oldDateTime_ = dateTime; + dateTime_ = dateTime ? oldToNew : 0; + } + /** \deprecated Use: uint8_t SdFile::dirEntry(dir_t* dir); */ + uint8_t dirEntry(dir_t& dir) {return dirEntry(&dir);} // NOLINT + /** \deprecated Use: + * uint8_t SdFile::makeDir(SdFile* dir, const char* dirName); + */ + uint8_t makeDir(SdFile& dir, const char* dirName) { // NOLINT + return makeDir(&dir, dirName); + } + /** \deprecated Use: + * uint8_t SdFile::open(SdFile* dirFile, const char* fileName, uint8_t oflag); + */ + uint8_t open(SdFile& dirFile, // NOLINT + const char* fileName, uint8_t oflag) { + return open(&dirFile, fileName, oflag); + } + /** \deprecated Do not use in new apps */ + uint8_t open(SdFile& dirFile, const char* fileName) { // NOLINT + return open(dirFile, fileName, O_RDWR); + } + /** \deprecated Use: + * uint8_t SdFile::open(SdFile* dirFile, uint16_t index, uint8_t oflag); + */ + uint8_t open(SdFile& dirFile, uint16_t index, uint8_t oflag) { // NOLINT + return open(&dirFile, index, oflag); + } + /** \deprecated Use: uint8_t SdFile::openRoot(SdVolume* vol); */ + uint8_t openRoot(SdVolume& vol) {return openRoot(&vol);} // NOLINT + + /** \deprecated Use: int8_t SdFile::readDir(dir_t* dir); */ + int8_t readDir(dir_t& dir) {return readDir(&dir);} // NOLINT + /** \deprecated Use: + * static uint8_t SdFile::remove(SdFile* dirFile, const char* fileName); + */ + static uint8_t remove(SdFile& dirFile, const char* fileName) { // NOLINT + return remove(&dirFile, fileName); + } +//------------------------------------------------------------------------------ +// rest are private + private: + static void (*oldDateTime_)(uint16_t& date, uint16_t& time); // NOLINT + static void oldToNew(uint16_t* date, uint16_t* time) { + uint16_t d; + uint16_t t; + oldDateTime_(d, t); + *date = d; + *time = t; + } +#endif // ALLOW_DEPRECATED_FUNCTIONS + private: + // bits defined in flags_ + // should be 0XF + static uint8_t const F_OFLAG = (O_ACCMODE | O_APPEND | O_SYNC); + // available bits + static uint8_t const F_UNUSED = 0X30; + // use unbuffered SD read + static uint8_t const F_FILE_UNBUFFERED_READ = 0X40; + // sync of directory entry required + static uint8_t const F_FILE_DIR_DIRTY = 0X80; + +// make sure F_OFLAG is ok +#if ((F_UNUSED | F_FILE_UNBUFFERED_READ | F_FILE_DIR_DIRTY) & F_OFLAG) +#error flags_ bits conflict +#endif // flags_ bits + + // private data + uint8_t flags_; // See above for definition of flags_ bits + uint8_t type_; // type of file see above for values + uint32_t curCluster_; // cluster for current file position + uint32_t curPosition_; // current file position in bytes from beginning + uint32_t dirBlock_; // SD block that contains directory entry for file + uint8_t dirIndex_; // index of entry in dirBlock 0 <= dirIndex_ <= 0XF + uint32_t fileSize_; // file size in bytes + uint32_t firstCluster_; // first cluster of file + SdVolume* vol_; // volume where file is located + + // private functions + uint8_t addCluster(void); + uint8_t addDirCluster(void); + dir_t* cacheDirEntry(uint8_t action); + static void (*dateTime_)(uint16_t* date, uint16_t* time); + static uint8_t make83Name(const char* str, uint8_t* name); + uint8_t openCachedEntry(uint8_t cacheIndex, uint8_t oflags); + dir_t* readDirCache(void); +}; +//============================================================================== +// SdVolume class +/** + * \brief Cache for an SD data block + */ +union cache_t { + /** Used to access cached file data blocks. */ + uint8_t data[512]; + /** Used to access cached FAT16 entries. */ + uint16_t fat16[256]; + /** Used to access cached FAT32 entries. */ + uint32_t fat32[128]; + /** Used to access cached directory entries. */ + dir_t dir[16]; + /** Used to access a cached MasterBoot Record. */ + mbr_t mbr; + /** Used to access to a cached FAT boot sector. */ + fbs_t fbs; +}; +//------------------------------------------------------------------------------ +/** + * \class SdVolume + * \brief Access FAT16 and FAT32 volumes on SD and SDHC cards. + */ +class SdVolume { + public: + /** Create an instance of SdVolume */ + SdVolume(void) :allocSearchStart_(2), fatType_(0) {} + /** Clear the cache and returns a pointer to the cache. Used by the WaveRP + * recorder to do raw write to the SD card. Not for normal apps. + */ + static uint8_t* cacheClear(void) { + cacheFlush(); + cacheBlockNumber_ = 0XFFFFFFFF; + return cacheBuffer_.data; + } + /** + * Initialize a FAT volume. Try partition one first then try super + * floppy format. + * + * \param[in] dev The Sd2Card where the volume is located. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. Reasons for + * failure include not finding a valid partition, not finding a valid + * FAT file system or an I/O error. + */ + uint8_t init(Sd2Card* dev) { return init(dev, 1) ? true : init(dev, 0);} + uint8_t init(Sd2Card* dev, uint8_t part); + + // inline functions that return volume info + /** \return The volume's cluster size in blocks. */ + uint8_t blocksPerCluster(void) const {return blocksPerCluster_;} + /** \return The number of blocks in one FAT. */ + uint32_t blocksPerFat(void) const {return blocksPerFat_;} + /** \return The total number of clusters in the volume. */ + uint32_t clusterCount(void) const {return clusterCount_;} + /** \return The shift count required to multiply by blocksPerCluster. */ + uint8_t clusterSizeShift(void) const {return clusterSizeShift_;} + /** \return The logical block number for the start of file data. */ + uint32_t dataStartBlock(void) const {return dataStartBlock_;} + /** \return The number of FAT structures on the volume. */ + uint8_t fatCount(void) const {return fatCount_;} + /** \return The logical block number for the start of the first FAT. */ + uint32_t fatStartBlock(void) const {return fatStartBlock_;} + /** \return The FAT type of the volume. Values are 12, 16 or 32. */ + uint8_t fatType(void) const {return fatType_;} + /** \return The number of entries in the root directory for FAT16 volumes. */ + uint32_t rootDirEntryCount(void) const {return rootDirEntryCount_;} + /** \return The logical block number for the start of the root directory + on FAT16 volumes or the first cluster number on FAT32 volumes. */ + uint32_t rootDirStart(void) const {return rootDirStart_;} + /** return a pointer to the Sd2Card object for this volume */ + static Sd2Card* sdCard(void) {return sdCard_;} +//------------------------------------------------------------------------------ +#if ALLOW_DEPRECATED_FUNCTIONS + // Deprecated functions - suppress cpplint warnings with NOLINT comment + /** \deprecated Use: uint8_t SdVolume::init(Sd2Card* dev); */ + uint8_t init(Sd2Card& dev) {return init(&dev);} // NOLINT + + /** \deprecated Use: uint8_t SdVolume::init(Sd2Card* dev, uint8_t vol); */ + uint8_t init(Sd2Card& dev, uint8_t part) { // NOLINT + return init(&dev, part); + } +#endif // ALLOW_DEPRECATED_FUNCTIONS +//------------------------------------------------------------------------------ + private: + // Allow SdFile access to SdVolume private data. + friend class SdFile; + + // value for action argument in cacheRawBlock to indicate read from cache + static uint8_t const CACHE_FOR_READ = 0; + // value for action argument in cacheRawBlock to indicate cache dirty + static uint8_t const CACHE_FOR_WRITE = 1; + + static cache_t cacheBuffer_; // 512 byte cache for device blocks + static uint32_t cacheBlockNumber_; // Logical number of block in the cache + static Sd2Card* sdCard_; // Sd2Card object for cache + static uint8_t cacheDirty_; // cacheFlush() will write block if true + static uint32_t cacheMirrorBlock_; // block number for mirror FAT +// + uint32_t allocSearchStart_; // start cluster for alloc search + uint8_t blocksPerCluster_; // cluster size in blocks + uint32_t blocksPerFat_; // FAT size in blocks + uint32_t clusterCount_; // clusters in one FAT + uint8_t clusterSizeShift_; // shift to convert cluster count to block count + uint32_t dataStartBlock_; // first data block number + uint8_t fatCount_; // number of FATs on volume + uint32_t fatStartBlock_; // start block for first FAT + uint8_t fatType_; // volume type (12, 16, OR 32) + uint16_t rootDirEntryCount_; // number of entries in FAT16 root dir + uint32_t rootDirStart_; // root start block for FAT16, cluster for FAT32 + //---------------------------------------------------------------------------- + uint8_t allocContiguous(uint32_t count, uint32_t* curCluster); + uint8_t blockOfCluster(uint32_t position) const { + return (position >> 9) & (blocksPerCluster_ - 1);} + uint32_t clusterStartBlock(uint32_t cluster) const { + return dataStartBlock_ + ((cluster - 2) << clusterSizeShift_);} + uint32_t blockNumber(uint32_t cluster, uint32_t position) const { + return clusterStartBlock(cluster) + blockOfCluster(position);} + static uint8_t cacheFlush(void); + static uint8_t cacheRawBlock(uint32_t blockNumber, uint8_t action); + static void cacheSetDirty(void) {cacheDirty_ |= CACHE_FOR_WRITE;} + static uint8_t cacheZeroBlock(uint32_t blockNumber); + uint8_t chainSize(uint32_t beginCluster, uint32_t* size) const; + uint8_t fatGet(uint32_t cluster, uint32_t* value) const; + uint8_t fatPut(uint32_t cluster, uint32_t value); + uint8_t fatPutEOC(uint32_t cluster) { + return fatPut(cluster, 0x0FFFFFFF); + } + uint8_t freeChain(uint32_t cluster); + uint8_t isEOC(uint32_t cluster) const { + return cluster >= (fatType_ == 16 ? FAT16EOC_MIN : FAT32EOC_MIN); + } + uint8_t readBlock(uint32_t block, uint8_t* dst) { + return sdCard_->readBlock(block, dst);} + uint8_t readData(uint32_t block, uint16_t offset, + uint16_t count, uint8_t* dst) { + return sdCard_->readData(block, offset, count, dst); + } + uint8_t writeBlock(uint32_t block, const uint8_t* dst) { + return sdCard_->writeBlock(block, dst); + } +}; +#endif // SdFat_h diff --git a/libs/arduino-1.0/libraries/SD/utility/SdFatUtil.h b/libs/arduino-1.0/libraries/SD/utility/SdFatUtil.h new file mode 100644 index 0000000..b5e50db --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdFatUtil.h @@ -0,0 +1,71 @@ +/* Arduino SdFat Library + * Copyright (C) 2008 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ +#ifndef SdFatUtil_h +#define SdFatUtil_h +/** + * \file + * Useful utility functions. + */ +#include +#include +/** Store and print a string in flash memory.*/ +#define PgmPrint(x) SerialPrint_P(PSTR(x)) +/** Store and print a string in flash memory followed by a CR/LF.*/ +#define PgmPrintln(x) SerialPrintln_P(PSTR(x)) +/** Defined so doxygen works for function definitions. */ +#define NOINLINE __attribute__((noinline,unused)) +#define UNUSEDOK __attribute__((unused)) +//------------------------------------------------------------------------------ +/** Return the number of bytes currently free in RAM. */ +static UNUSEDOK int FreeRam(void) { + extern int __bss_end; + extern int* __brkval; + int free_memory; + if (reinterpret_cast(__brkval) == 0) { + // if no heap use from end of bss section + free_memory = reinterpret_cast(&free_memory) + - reinterpret_cast(&__bss_end); + } else { + // use from top of stack to heap + free_memory = reinterpret_cast(&free_memory) + - reinterpret_cast(__brkval); + } + return free_memory; +} +//------------------------------------------------------------------------------ +/** + * %Print a string in flash memory to the serial port. + * + * \param[in] str Pointer to string stored in flash memory. + */ +static NOINLINE void SerialPrint_P(PGM_P str) { + for (uint8_t c; (c = pgm_read_byte(str)); str++) Serial.write(c); +} +//------------------------------------------------------------------------------ +/** + * %Print a string in flash memory followed by a CR/LF. + * + * \param[in] str Pointer to string stored in flash memory. + */ +static NOINLINE void SerialPrintln_P(PGM_P str) { + SerialPrint_P(str); + Serial.println(); +} +#endif // #define SdFatUtil_h diff --git a/libs/arduino-1.0/libraries/SD/utility/SdFatmainpage.h b/libs/arduino-1.0/libraries/SD/utility/SdFatmainpage.h new file mode 100644 index 0000000..d26cb85 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdFatmainpage.h @@ -0,0 +1,202 @@ +/* Arduino SdFat Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ + +/** +\mainpage Arduino SdFat Library +
Copyright © 2009 by William Greiman +
+ +\section Intro Introduction +The Arduino SdFat Library is a minimal implementation of FAT16 and FAT32 +file systems on SD flash memory cards. Standard SD and high capacity +SDHC cards are supported. + +The SdFat only supports short 8.3 names. + +The main classes in SdFat are Sd2Card, SdVolume, and SdFile. + +The Sd2Card class supports access to standard SD cards and SDHC cards. Most +applications will only need to call the Sd2Card::init() member function. + +The SdVolume class supports FAT16 and FAT32 partitions. Most applications +will only need to call the SdVolume::init() member function. + +The SdFile class provides file access functions such as open(), read(), +remove(), write(), close() and sync(). This class supports access to the root +directory and subdirectories. + +A number of example are provided in the SdFat/examples folder. These were +developed to test SdFat and illustrate its use. + +SdFat was developed for high speed data recording. SdFat was used to implement +an audio record/play class, WaveRP, for the Adafruit Wave Shield. This +application uses special Sd2Card calls to write to contiguous files in raw mode. +These functions reduce write latency so that audio can be recorded with the +small amount of RAM in the Arduino. + +\section SDcard SD\SDHC Cards + +Arduinos access SD cards using the cards SPI protocol. PCs, Macs, and +most consumer devices use the 4-bit parallel SD protocol. A card that +functions well on A PC or Mac may not work well on the Arduino. + +Most cards have good SPI read performance but cards vary widely in SPI +write performance. Write performance is limited by how efficiently the +card manages internal erase/remapping operations. The Arduino cannot +optimize writes to reduce erase operations because of its limit RAM. + +SanDisk cards generally have good write performance. They seem to have +more internal RAM buffering than other cards and therefore can limit +the number of flash erase operations that the Arduino forces due to its +limited RAM. + +\section Hardware Hardware Configuration + +SdFat was developed using an + Adafruit Industries + Wave Shield. + +The hardware interface to the SD card should not use a resistor based level +shifter. SdFat sets the SPI bus frequency to 8 MHz which results in signal +rise times that are too slow for the edge detectors in many newer SD card +controllers when resistor voltage dividers are used. + +The 5 to 3.3 V level shifter for 5 V Arduinos should be IC based like the +74HC4050N based circuit shown in the file SdLevel.png. The Adafruit Wave Shield +uses a 74AHC125N. Gravitech sells SD and MicroSD Card Adapters based on the +74LCX245. + +If you are using a resistor based level shifter and are having problems try +setting the SPI bus frequency to 4 MHz. This can be done by using +card.init(SPI_HALF_SPEED) to initialize the SD card. + +\section comment Bugs and Comments + +If you wish to report bugs or have comments, send email to fat16lib@sbcglobal.net. + +\section SdFatClass SdFat Usage + +SdFat uses a slightly restricted form of short names. +Only printable ASCII characters are supported. No characters with code point +values greater than 127 are allowed. Space is not allowed even though space +was allowed in the API of early versions of DOS. + +Short names are limited to 8 characters followed by an optional period (.) +and extension of up to 3 characters. The characters may be any combination +of letters and digits. The following special characters are also allowed: + +$ % ' - _ @ ~ ` ! ( ) { } ^ # & + +Short names are always converted to upper case and their original case +value is lost. + +\note + The Arduino Print class uses character +at a time writes so it was necessary to use a \link SdFile::sync() sync() \endlink +function to control when data is written to the SD card. + +\par +An application which writes to a file using \link Print::print() print()\endlink, +\link Print::println() println() \endlink +or \link SdFile::write write() \endlink must call \link SdFile::sync() sync() \endlink +at the appropriate time to force data and directory information to be written +to the SD Card. Data and directory information are also written to the SD card +when \link SdFile::close() close() \endlink is called. + +\par +Applications must use care calling \link SdFile::sync() sync() \endlink +since 2048 bytes of I/O is required to update file and +directory information. This includes writing the current data block, reading +the block that contains the directory entry for update, writing the directory +block back and reading back the current data block. + +It is possible to open a file with two or more instances of SdFile. A file may +be corrupted if data is written to the file by more than one instance of SdFile. + +\section HowTo How to format SD Cards as FAT Volumes + +You should use a freshly formatted SD card for best performance. FAT +file systems become slower if many files have been created and deleted. +This is because the directory entry for a deleted file is marked as deleted, +but is not deleted. When a new file is created, these entries must be scanned +before creating the file, a flaw in the FAT design. Also files can become +fragmented which causes reads and writes to be slower. + +Microsoft operating systems support removable media formatted with a +Master Boot Record, MBR, or formatted as a super floppy with a FAT Boot Sector +in block zero. + +Microsoft operating systems expect MBR formatted removable media +to have only one partition. The first partition should be used. + +Microsoft operating systems do not support partitioning SD flash cards. +If you erase an SD card with a program like KillDisk, Most versions of +Windows will format the card as a super floppy. + +The best way to restore an SD card's format is to use SDFormatter +which can be downloaded from: + +http://www.sdcard.org/consumers/formatter/ + +SDFormatter aligns flash erase boundaries with file +system structures which reduces write latency and file system overhead. + +SDFormatter does not have an option for FAT type so it may format +small cards as FAT12. + +After the MBR is restored by SDFormatter you may need to reformat small +cards that have been formatted FAT12 to force the volume type to be FAT16. + +If you reformat the SD card with an OS utility, choose a cluster size that +will result in: + +4084 < CountOfClusters && CountOfClusters < 65525 + +The volume will then be FAT16. + +If you are formatting an SD card on OS X or Linux, be sure to use the first +partition. Format this partition with a cluster count in above range. + +\section References References + +Adafruit Industries: + +http://www.adafruit.com/ + +http://www.ladyada.net/make/waveshield/ + +The Arduino site: + +http://www.arduino.cc/ + +For more information about FAT file systems see: + +http://www.microsoft.com/whdc/system/platform/firmware/fatgen.mspx + +For information about using SD cards as SPI devices see: + +http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf + +The ATmega328 datasheet: + +http://www.atmel.com/dyn/resources/prod_documents/doc8161.pdf + + + */ diff --git a/libs/arduino-1.0/libraries/SD/utility/SdFile.cpp b/libs/arduino-1.0/libraries/SD/utility/SdFile.cpp new file mode 100644 index 0000000..1d36271 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdFile.cpp @@ -0,0 +1,1253 @@ +/* Arduino SdFat Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ +#include +#include +#include +//------------------------------------------------------------------------------ +// callback function for date/time +void (*SdFile::dateTime_)(uint16_t* date, uint16_t* time) = NULL; + +#if ALLOW_DEPRECATED_FUNCTIONS +// suppress cpplint warnings with NOLINT comment +void (*SdFile::oldDateTime_)(uint16_t& date, uint16_t& time) = NULL; // NOLINT +#endif // ALLOW_DEPRECATED_FUNCTIONS +//------------------------------------------------------------------------------ +// add a cluster to a file +uint8_t SdFile::addCluster() { + if (!vol_->allocContiguous(1, &curCluster_)) return false; + + // if first cluster of file link to directory entry + if (firstCluster_ == 0) { + firstCluster_ = curCluster_; + flags_ |= F_FILE_DIR_DIRTY; + } + return true; +} +//------------------------------------------------------------------------------ +// Add a cluster to a directory file and zero the cluster. +// return with first block of cluster in the cache +uint8_t SdFile::addDirCluster(void) { + if (!addCluster()) return false; + + // zero data in cluster insure first cluster is in cache + uint32_t block = vol_->clusterStartBlock(curCluster_); + for (uint8_t i = vol_->blocksPerCluster_; i != 0; i--) { + if (!SdVolume::cacheZeroBlock(block + i - 1)) return false; + } + // Increase directory file size by cluster size + fileSize_ += 512UL << vol_->clusterSizeShift_; + return true; +} +//------------------------------------------------------------------------------ +// cache a file's directory entry +// return pointer to cached entry or null for failure +dir_t* SdFile::cacheDirEntry(uint8_t action) { + if (!SdVolume::cacheRawBlock(dirBlock_, action)) return NULL; + return SdVolume::cacheBuffer_.dir + dirIndex_; +} +//------------------------------------------------------------------------------ +/** + * Close a file and force cached data and directory information + * to be written to the storage device. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include no file is open or an I/O error. + */ +uint8_t SdFile::close(void) { + if (!sync())return false; + type_ = FAT_FILE_TYPE_CLOSED; + return true; +} +//------------------------------------------------------------------------------ +/** + * Check for contiguous file and return its raw block range. + * + * \param[out] bgnBlock the first block address for the file. + * \param[out] endBlock the last block address for the file. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include file is not contiguous, file has zero length + * or an I/O error occurred. + */ +uint8_t SdFile::contiguousRange(uint32_t* bgnBlock, uint32_t* endBlock) { + // error if no blocks + if (firstCluster_ == 0) return false; + + for (uint32_t c = firstCluster_; ; c++) { + uint32_t next; + if (!vol_->fatGet(c, &next)) return false; + + // check for contiguous + if (next != (c + 1)) { + // error if not end of chain + if (!vol_->isEOC(next)) return false; + *bgnBlock = vol_->clusterStartBlock(firstCluster_); + *endBlock = vol_->clusterStartBlock(c) + + vol_->blocksPerCluster_ - 1; + return true; + } + } +} +//------------------------------------------------------------------------------ +/** + * Create and open a new contiguous file of a specified size. + * + * \note This function only supports short DOS 8.3 names. + * See open() for more information. + * + * \param[in] dirFile The directory where the file will be created. + * \param[in] fileName A valid DOS 8.3 file name. + * \param[in] size The desired file size. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include \a fileName contains + * an invalid DOS 8.3 file name, the FAT volume has not been initialized, + * a file is already open, the file already exists, the root + * directory is full or an I/O error. + * + */ +uint8_t SdFile::createContiguous(SdFile* dirFile, + const char* fileName, uint32_t size) { + // don't allow zero length file + if (size == 0) return false; + if (!open(dirFile, fileName, O_CREAT | O_EXCL | O_RDWR)) return false; + + // calculate number of clusters needed + uint32_t count = ((size - 1) >> (vol_->clusterSizeShift_ + 9)) + 1; + + // allocate clusters + if (!vol_->allocContiguous(count, &firstCluster_)) { + remove(); + return false; + } + fileSize_ = size; + + // insure sync() will update dir entry + flags_ |= F_FILE_DIR_DIRTY; + return sync(); +} +//------------------------------------------------------------------------------ +/** + * Return a files directory entry + * + * \param[out] dir Location for return of the files directory entry. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t SdFile::dirEntry(dir_t* dir) { + // make sure fields on SD are correct + if (!sync()) return false; + + // read entry + dir_t* p = cacheDirEntry(SdVolume::CACHE_FOR_READ); + if (!p) return false; + + // copy to caller's struct + memcpy(dir, p, sizeof(dir_t)); + return true; +} +//------------------------------------------------------------------------------ +/** + * Format the name field of \a dir into the 13 byte array + * \a name in standard 8.3 short name format. + * + * \param[in] dir The directory structure containing the name. + * \param[out] name A 13 byte char array for the formatted name. + */ +void SdFile::dirName(const dir_t& dir, char* name) { + uint8_t j = 0; + for (uint8_t i = 0; i < 11; i++) { + if (dir.name[i] == ' ')continue; + if (i == 8) name[j++] = '.'; + name[j++] = dir.name[i]; + } + name[j] = 0; +} +//------------------------------------------------------------------------------ +/** List directory contents to Serial. + * + * \param[in] flags The inclusive OR of + * + * LS_DATE - %Print file modification date + * + * LS_SIZE - %Print file size. + * + * LS_R - Recursive list of subdirectories. + * + * \param[in] indent Amount of space before file name. Used for recursive + * list to indicate subdirectory level. + */ +void SdFile::ls(uint8_t flags, uint8_t indent) { + dir_t* p; + + rewind(); + while ((p = readDirCache())) { + // done if past last used entry + if (p->name[0] == DIR_NAME_FREE) break; + + // skip deleted entry and entries for . and .. + if (p->name[0] == DIR_NAME_DELETED || p->name[0] == '.') continue; + + // only list subdirectories and files + if (!DIR_IS_FILE_OR_SUBDIR(p)) continue; + + // print any indent spaces + for (int8_t i = 0; i < indent; i++) Serial.print(' '); + + // print file name with possible blank fill + printDirName(*p, flags & (LS_DATE | LS_SIZE) ? 14 : 0); + + // print modify date/time if requested + if (flags & LS_DATE) { + printFatDate(p->lastWriteDate); + Serial.print(' '); + printFatTime(p->lastWriteTime); + } + // print size if requested + if (!DIR_IS_SUBDIR(p) && (flags & LS_SIZE)) { + Serial.print(' '); + Serial.print(p->fileSize); + } + Serial.println(); + + // list subdirectory content if requested + if ((flags & LS_R) && DIR_IS_SUBDIR(p)) { + uint16_t index = curPosition()/32 - 1; + SdFile s; + if (s.open(this, index, O_READ)) s.ls(flags, indent + 2); + seekSet(32 * (index + 1)); + } + } +} +//------------------------------------------------------------------------------ +// format directory name field from a 8.3 name string +uint8_t SdFile::make83Name(const char* str, uint8_t* name) { + uint8_t c; + uint8_t n = 7; // max index for part before dot + uint8_t i = 0; + // blank fill name and extension + while (i < 11) name[i++] = ' '; + i = 0; + while ((c = *str++) != '\0') { + if (c == '.') { + if (n == 10) return false; // only one dot allowed + n = 10; // max index for full 8.3 name + i = 8; // place for extension + } else { + // illegal FAT characters + PGM_P p = PSTR("|<>^+=?/[];,*\"\\"); + uint8_t b; + while ((b = pgm_read_byte(p++))) if (b == c) return false; + // check size and only allow ASCII printable characters + if (i > n || c < 0X21 || c > 0X7E)return false; + // only upper case allowed in 8.3 names - convert lower to upper + name[i++] = c < 'a' || c > 'z' ? c : c + ('A' - 'a'); + } + } + // must have a file name, extension is optional + return name[0] != ' '; +} +//------------------------------------------------------------------------------ +/** Make a new directory. + * + * \param[in] dir An open SdFat instance for the directory that will containing + * the new directory. + * + * \param[in] dirName A valid 8.3 DOS name for the new directory. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include this SdFile is already open, \a dir is not a + * directory, \a dirName is invalid or already exists in \a dir. + */ +uint8_t SdFile::makeDir(SdFile* dir, const char* dirName) { + dir_t d; + + // create a normal file + if (!open(dir, dirName, O_CREAT | O_EXCL | O_RDWR)) return false; + + // convert SdFile to directory + flags_ = O_READ; + type_ = FAT_FILE_TYPE_SUBDIR; + + // allocate and zero first cluster + if (!addDirCluster())return false; + + // force entry to SD + if (!sync()) return false; + + // cache entry - should already be in cache due to sync() call + dir_t* p = cacheDirEntry(SdVolume::CACHE_FOR_WRITE); + if (!p) return false; + + // change directory entry attribute + p->attributes = DIR_ATT_DIRECTORY; + + // make entry for '.' + memcpy(&d, p, sizeof(d)); + for (uint8_t i = 1; i < 11; i++) d.name[i] = ' '; + d.name[0] = '.'; + + // cache block for '.' and '..' + uint32_t block = vol_->clusterStartBlock(firstCluster_); + if (!SdVolume::cacheRawBlock(block, SdVolume::CACHE_FOR_WRITE)) return false; + + // copy '.' to block + memcpy(&SdVolume::cacheBuffer_.dir[0], &d, sizeof(d)); + + // make entry for '..' + d.name[1] = '.'; + if (dir->isRoot()) { + d.firstClusterLow = 0; + d.firstClusterHigh = 0; + } else { + d.firstClusterLow = dir->firstCluster_ & 0XFFFF; + d.firstClusterHigh = dir->firstCluster_ >> 16; + } + // copy '..' to block + memcpy(&SdVolume::cacheBuffer_.dir[1], &d, sizeof(d)); + + // set position after '..' + curPosition_ = 2 * sizeof(d); + + // write first block + return SdVolume::cacheFlush(); +} +//------------------------------------------------------------------------------ +/** + * Open a file or directory by name. + * + * \param[in] dirFile An open SdFat instance for the directory containing the + * file to be opened. + * + * \param[in] fileName A valid 8.3 DOS name for a file to be opened. + * + * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive + * OR of flags from the following list + * + * O_READ - Open for reading. + * + * O_RDONLY - Same as O_READ. + * + * O_WRITE - Open for writing. + * + * O_WRONLY - Same as O_WRITE. + * + * O_RDWR - Open for reading and writing. + * + * O_APPEND - If set, the file offset shall be set to the end of the + * file prior to each write. + * + * O_CREAT - If the file exists, this flag has no effect except as noted + * under O_EXCL below. Otherwise, the file shall be created + * + * O_EXCL - If O_CREAT and O_EXCL are set, open() shall fail if the file exists. + * + * O_SYNC - Call sync() after each write. This flag should not be used with + * write(uint8_t), write_P(PGM_P), writeln_P(PGM_P), or the Arduino Print class. + * These functions do character at a time writes so sync() will be called + * after each byte. + * + * O_TRUNC - If the file exists and is a regular file, and the file is + * successfully opened and is not read only, its length shall be truncated to 0. + * + * \note Directory files must be opened read only. Write and truncation is + * not allowed for directory files. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include this SdFile is already open, \a difFile is not + * a directory, \a fileName is invalid, the file does not exist + * or can't be opened in the access mode specified by oflag. + */ +uint8_t SdFile::open(SdFile* dirFile, const char* fileName, uint8_t oflag) { + uint8_t dname[11]; + dir_t* p; + + // error if already open + if (isOpen())return false; + + if (!make83Name(fileName, dname)) return false; + vol_ = dirFile->vol_; + dirFile->rewind(); + + // bool for empty entry found + uint8_t emptyFound = false; + + // search for file + while (dirFile->curPosition_ < dirFile->fileSize_) { + uint8_t index = 0XF & (dirFile->curPosition_ >> 5); + p = dirFile->readDirCache(); + if (p == NULL) return false; + + if (p->name[0] == DIR_NAME_FREE || p->name[0] == DIR_NAME_DELETED) { + // remember first empty slot + if (!emptyFound) { + emptyFound = true; + dirIndex_ = index; + dirBlock_ = SdVolume::cacheBlockNumber_; + } + // done if no entries follow + if (p->name[0] == DIR_NAME_FREE) break; + } else if (!memcmp(dname, p->name, 11)) { + // don't open existing file if O_CREAT and O_EXCL + if ((oflag & (O_CREAT | O_EXCL)) == (O_CREAT | O_EXCL)) return false; + + // open found file + return openCachedEntry(0XF & index, oflag); + } + } + // only create file if O_CREAT and O_WRITE + if ((oflag & (O_CREAT | O_WRITE)) != (O_CREAT | O_WRITE)) return false; + + // cache found slot or add cluster if end of file + if (emptyFound) { + p = cacheDirEntry(SdVolume::CACHE_FOR_WRITE); + if (!p) return false; + } else { + if (dirFile->type_ == FAT_FILE_TYPE_ROOT16) return false; + + // add and zero cluster for dirFile - first cluster is in cache for write + if (!dirFile->addDirCluster()) return false; + + // use first entry in cluster + dirIndex_ = 0; + p = SdVolume::cacheBuffer_.dir; + } + // initialize as empty file + memset(p, 0, sizeof(dir_t)); + memcpy(p->name, dname, 11); + + // set timestamps + if (dateTime_) { + // call user function + dateTime_(&p->creationDate, &p->creationTime); + } else { + // use default date/time + p->creationDate = FAT_DEFAULT_DATE; + p->creationTime = FAT_DEFAULT_TIME; + } + p->lastAccessDate = p->creationDate; + p->lastWriteDate = p->creationDate; + p->lastWriteTime = p->creationTime; + + // force write of entry to SD + if (!SdVolume::cacheFlush()) return false; + + // open entry in cache + return openCachedEntry(dirIndex_, oflag); +} +//------------------------------------------------------------------------------ +/** + * Open a file by index. + * + * \param[in] dirFile An open SdFat instance for the directory. + * + * \param[in] index The \a index of the directory entry for the file to be + * opened. The value for \a index is (directory file position)/32. + * + * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive + * OR of flags O_READ, O_WRITE, O_TRUNC, and O_SYNC. + * + * See open() by fileName for definition of flags and return values. + * + */ +uint8_t SdFile::open(SdFile* dirFile, uint16_t index, uint8_t oflag) { + // error if already open + if (isOpen())return false; + + // don't open existing file if O_CREAT and O_EXCL - user call error + if ((oflag & (O_CREAT | O_EXCL)) == (O_CREAT | O_EXCL)) return false; + + vol_ = dirFile->vol_; + + // seek to location of entry + if (!dirFile->seekSet(32 * index)) return false; + + // read entry into cache + dir_t* p = dirFile->readDirCache(); + if (p == NULL) return false; + + // error if empty slot or '.' or '..' + if (p->name[0] == DIR_NAME_FREE || + p->name[0] == DIR_NAME_DELETED || p->name[0] == '.') { + return false; + } + // open cached entry + return openCachedEntry(index & 0XF, oflag); +} +//------------------------------------------------------------------------------ +// open a cached directory entry. Assumes vol_ is initializes +uint8_t SdFile::openCachedEntry(uint8_t dirIndex, uint8_t oflag) { + // location of entry in cache + dir_t* p = SdVolume::cacheBuffer_.dir + dirIndex; + + // write or truncate is an error for a directory or read-only file + if (p->attributes & (DIR_ATT_READ_ONLY | DIR_ATT_DIRECTORY)) { + if (oflag & (O_WRITE | O_TRUNC)) return false; + } + // remember location of directory entry on SD + dirIndex_ = dirIndex; + dirBlock_ = SdVolume::cacheBlockNumber_; + + // copy first cluster number for directory fields + firstCluster_ = (uint32_t)p->firstClusterHigh << 16; + firstCluster_ |= p->firstClusterLow; + + // make sure it is a normal file or subdirectory + if (DIR_IS_FILE(p)) { + fileSize_ = p->fileSize; + type_ = FAT_FILE_TYPE_NORMAL; + } else if (DIR_IS_SUBDIR(p)) { + if (!vol_->chainSize(firstCluster_, &fileSize_)) return false; + type_ = FAT_FILE_TYPE_SUBDIR; + } else { + return false; + } + // save open flags for read/write + flags_ = oflag & (O_ACCMODE | O_SYNC | O_APPEND); + + // set to start of file + curCluster_ = 0; + curPosition_ = 0; + + // truncate file to zero length if requested + if (oflag & O_TRUNC) return truncate(0); + return true; +} +//------------------------------------------------------------------------------ +/** + * Open a volume's root directory. + * + * \param[in] vol The FAT volume containing the root directory to be opened. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include the FAT volume has not been initialized + * or it a FAT12 volume. + */ +uint8_t SdFile::openRoot(SdVolume* vol) { + // error if file is already open + if (isOpen()) return false; + + if (vol->fatType() == 16) { + type_ = FAT_FILE_TYPE_ROOT16; + firstCluster_ = 0; + fileSize_ = 32 * vol->rootDirEntryCount(); + } else if (vol->fatType() == 32) { + type_ = FAT_FILE_TYPE_ROOT32; + firstCluster_ = vol->rootDirStart(); + if (!vol->chainSize(firstCluster_, &fileSize_)) return false; + } else { + // volume is not initialized or FAT12 + return false; + } + vol_ = vol; + // read only + flags_ = O_READ; + + // set to start of file + curCluster_ = 0; + curPosition_ = 0; + + // root has no directory entry + dirBlock_ = 0; + dirIndex_ = 0; + return true; +} +//------------------------------------------------------------------------------ +/** %Print the name field of a directory entry in 8.3 format to Serial. + * + * \param[in] dir The directory structure containing the name. + * \param[in] width Blank fill name if length is less than \a width. + */ +void SdFile::printDirName(const dir_t& dir, uint8_t width) { + uint8_t w = 0; + for (uint8_t i = 0; i < 11; i++) { + if (dir.name[i] == ' ')continue; + if (i == 8) { + Serial.print('.'); + w++; + } + Serial.write(dir.name[i]); + w++; + } + if (DIR_IS_SUBDIR(&dir)) { + Serial.print('/'); + w++; + } + while (w < width) { + Serial.print(' '); + w++; + } +} +//------------------------------------------------------------------------------ +/** %Print a directory date field to Serial. + * + * Format is yyyy-mm-dd. + * + * \param[in] fatDate The date field from a directory entry. + */ +void SdFile::printFatDate(uint16_t fatDate) { + Serial.print(FAT_YEAR(fatDate)); + Serial.print('-'); + printTwoDigits(FAT_MONTH(fatDate)); + Serial.print('-'); + printTwoDigits(FAT_DAY(fatDate)); +} +//------------------------------------------------------------------------------ +/** %Print a directory time field to Serial. + * + * Format is hh:mm:ss. + * + * \param[in] fatTime The time field from a directory entry. + */ +void SdFile::printFatTime(uint16_t fatTime) { + printTwoDigits(FAT_HOUR(fatTime)); + Serial.print(':'); + printTwoDigits(FAT_MINUTE(fatTime)); + Serial.print(':'); + printTwoDigits(FAT_SECOND(fatTime)); +} +//------------------------------------------------------------------------------ +/** %Print a value as two digits to Serial. + * + * \param[in] v Value to be printed, 0 <= \a v <= 99 + */ +void SdFile::printTwoDigits(uint8_t v) { + char str[3]; + str[0] = '0' + v/10; + str[1] = '0' + v % 10; + str[2] = 0; + Serial.print(str); +} +//------------------------------------------------------------------------------ +/** + * Read data from a file starting at the current position. + * + * \param[out] buf Pointer to the location that will receive the data. + * + * \param[in] nbyte Maximum number of bytes to read. + * + * \return For success read() returns the number of bytes read. + * A value less than \a nbyte, including zero, will be returned + * if end of file is reached. + * If an error occurs, read() returns -1. Possible errors include + * read() called before a file has been opened, corrupt file system + * or an I/O error occurred. + */ +int16_t SdFile::read(void* buf, uint16_t nbyte) { + uint8_t* dst = reinterpret_cast(buf); + + // error if not open or write only + if (!isOpen() || !(flags_ & O_READ)) return -1; + + // max bytes left in file + if (nbyte > (fileSize_ - curPosition_)) nbyte = fileSize_ - curPosition_; + + // amount left to read + uint16_t toRead = nbyte; + while (toRead > 0) { + uint32_t block; // raw device block number + uint16_t offset = curPosition_ & 0X1FF; // offset in block + if (type_ == FAT_FILE_TYPE_ROOT16) { + block = vol_->rootDirStart() + (curPosition_ >> 9); + } else { + uint8_t blockOfCluster = vol_->blockOfCluster(curPosition_); + if (offset == 0 && blockOfCluster == 0) { + // start of new cluster + if (curPosition_ == 0) { + // use first cluster in file + curCluster_ = firstCluster_; + } else { + // get next cluster from FAT + if (!vol_->fatGet(curCluster_, &curCluster_)) return -1; + } + } + block = vol_->clusterStartBlock(curCluster_) + blockOfCluster; + } + uint16_t n = toRead; + + // amount to be read from current block + if (n > (512 - offset)) n = 512 - offset; + + // no buffering needed if n == 512 or user requests no buffering + if ((unbufferedRead() || n == 512) && + block != SdVolume::cacheBlockNumber_) { + if (!vol_->readData(block, offset, n, dst)) return -1; + dst += n; + } else { + // read block to cache and copy data to caller + if (!SdVolume::cacheRawBlock(block, SdVolume::CACHE_FOR_READ)) return -1; + uint8_t* src = SdVolume::cacheBuffer_.data + offset; + uint8_t* end = src + n; + while (src != end) *dst++ = *src++; + } + curPosition_ += n; + toRead -= n; + } + return nbyte; +} +//------------------------------------------------------------------------------ +/** + * Read the next directory entry from a directory file. + * + * \param[out] dir The dir_t struct that will receive the data. + * + * \return For success readDir() returns the number of bytes read. + * A value of zero will be returned if end of file is reached. + * If an error occurs, readDir() returns -1. Possible errors include + * readDir() called before a directory has been opened, this is not + * a directory file or an I/O error occurred. + */ +int8_t SdFile::readDir(dir_t* dir) { + int8_t n; + // if not a directory file or miss-positioned return an error + if (!isDir() || (0X1F & curPosition_)) return -1; + + while ((n = read(dir, sizeof(dir_t))) == sizeof(dir_t)) { + // last entry if DIR_NAME_FREE + if (dir->name[0] == DIR_NAME_FREE) break; + // skip empty entries and entry for . and .. + if (dir->name[0] == DIR_NAME_DELETED || dir->name[0] == '.') continue; + // return if normal file or subdirectory + if (DIR_IS_FILE_OR_SUBDIR(dir)) return n; + } + // error, end of file, or past last entry + return n < 0 ? -1 : 0; +} +//------------------------------------------------------------------------------ +// Read next directory entry into the cache +// Assumes file is correctly positioned +dir_t* SdFile::readDirCache(void) { + // error if not directory + if (!isDir()) return NULL; + + // index of entry in cache + uint8_t i = (curPosition_ >> 5) & 0XF; + + // use read to locate and cache block + if (read() < 0) return NULL; + + // advance to next entry + curPosition_ += 31; + + // return pointer to entry + return (SdVolume::cacheBuffer_.dir + i); +} +//------------------------------------------------------------------------------ +/** + * Remove a file. + * + * The directory entry and all data for the file are deleted. + * + * \note This function should not be used to delete the 8.3 version of a + * file that has a long name. For example if a file has the long name + * "New Text Document.txt" you should not delete the 8.3 name "NEWTEX~1.TXT". + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include the file read-only, is a directory, + * or an I/O error occurred. + */ +uint8_t SdFile::remove(void) { + // free any clusters - will fail if read-only or directory + if (!truncate(0)) return false; + + // cache directory entry + dir_t* d = cacheDirEntry(SdVolume::CACHE_FOR_WRITE); + if (!d) return false; + + // mark entry deleted + d->name[0] = DIR_NAME_DELETED; + + // set this SdFile closed + type_ = FAT_FILE_TYPE_CLOSED; + + // write entry to SD + return SdVolume::cacheFlush(); +} +//------------------------------------------------------------------------------ +/** + * Remove a file. + * + * The directory entry and all data for the file are deleted. + * + * \param[in] dirFile The directory that contains the file. + * \param[in] fileName The name of the file to be removed. + * + * \note This function should not be used to delete the 8.3 version of a + * file that has a long name. For example if a file has the long name + * "New Text Document.txt" you should not delete the 8.3 name "NEWTEX~1.TXT". + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include the file is a directory, is read only, + * \a dirFile is not a directory, \a fileName is not found + * or an I/O error occurred. + */ +uint8_t SdFile::remove(SdFile* dirFile, const char* fileName) { + SdFile file; + if (!file.open(dirFile, fileName, O_WRITE)) return false; + return file.remove(); +} +//------------------------------------------------------------------------------ +/** Remove a directory file. + * + * The directory file will be removed only if it is empty and is not the + * root directory. rmDir() follows DOS and Windows and ignores the + * read-only attribute for the directory. + * + * \note This function should not be used to delete the 8.3 version of a + * directory that has a long name. For example if a directory has the + * long name "New folder" you should not delete the 8.3 name "NEWFOL~1". + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include the file is not a directory, is the root + * directory, is not empty, or an I/O error occurred. + */ +uint8_t SdFile::rmDir(void) { + // must be open subdirectory + if (!isSubDir()) return false; + + rewind(); + + // make sure directory is empty + while (curPosition_ < fileSize_) { + dir_t* p = readDirCache(); + if (p == NULL) return false; + // done if past last used entry + if (p->name[0] == DIR_NAME_FREE) break; + // skip empty slot or '.' or '..' + if (p->name[0] == DIR_NAME_DELETED || p->name[0] == '.') continue; + // error not empty + if (DIR_IS_FILE_OR_SUBDIR(p)) return false; + } + // convert empty directory to normal file for remove + type_ = FAT_FILE_TYPE_NORMAL; + flags_ |= O_WRITE; + return remove(); +} +//------------------------------------------------------------------------------ +/** Recursively delete a directory and all contained files. + * + * This is like the Unix/Linux 'rm -rf *' if called with the root directory + * hence the name. + * + * Warning - This will remove all contents of the directory including + * subdirectories. The directory will then be removed if it is not root. + * The read-only attribute for files will be ignored. + * + * \note This function should not be used to delete the 8.3 version of + * a directory that has a long name. See remove() and rmDir(). + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t SdFile::rmRfStar(void) { + rewind(); + while (curPosition_ < fileSize_) { + SdFile f; + + // remember position + uint16_t index = curPosition_/32; + + dir_t* p = readDirCache(); + if (!p) return false; + + // done if past last entry + if (p->name[0] == DIR_NAME_FREE) break; + + // skip empty slot or '.' or '..' + if (p->name[0] == DIR_NAME_DELETED || p->name[0] == '.') continue; + + // skip if part of long file name or volume label in root + if (!DIR_IS_FILE_OR_SUBDIR(p)) continue; + + if (!f.open(this, index, O_READ)) return false; + if (f.isSubDir()) { + // recursively delete + if (!f.rmRfStar()) return false; + } else { + // ignore read-only + f.flags_ |= O_WRITE; + if (!f.remove()) return false; + } + // position to next entry if required + if (curPosition_ != (32*(index + 1))) { + if (!seekSet(32*(index + 1))) return false; + } + } + // don't try to delete root + if (isRoot()) return true; + return rmDir(); +} +//------------------------------------------------------------------------------ +/** + * Sets a file's position. + * + * \param[in] pos The new position in bytes from the beginning of the file. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t SdFile::seekSet(uint32_t pos) { + // error if file not open or seek past end of file + if (!isOpen() || pos > fileSize_) return false; + + if (type_ == FAT_FILE_TYPE_ROOT16) { + curPosition_ = pos; + return true; + } + if (pos == 0) { + // set position to start of file + curCluster_ = 0; + curPosition_ = 0; + return true; + } + // calculate cluster index for cur and new position + uint32_t nCur = (curPosition_ - 1) >> (vol_->clusterSizeShift_ + 9); + uint32_t nNew = (pos - 1) >> (vol_->clusterSizeShift_ + 9); + + if (nNew < nCur || curPosition_ == 0) { + // must follow chain from first cluster + curCluster_ = firstCluster_; + } else { + // advance from curPosition + nNew -= nCur; + } + while (nNew--) { + if (!vol_->fatGet(curCluster_, &curCluster_)) return false; + } + curPosition_ = pos; + return true; +} +//------------------------------------------------------------------------------ +/** + * The sync() call causes all modified data and directory fields + * to be written to the storage device. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include a call to sync() before a file has been + * opened or an I/O error. + */ +uint8_t SdFile::sync(void) { + // only allow open files and directories + if (!isOpen()) return false; + + if (flags_ & F_FILE_DIR_DIRTY) { + dir_t* d = cacheDirEntry(SdVolume::CACHE_FOR_WRITE); + if (!d) return false; + + // do not set filesize for dir files + if (!isDir()) d->fileSize = fileSize_; + + // update first cluster fields + d->firstClusterLow = firstCluster_ & 0XFFFF; + d->firstClusterHigh = firstCluster_ >> 16; + + // set modify time if user supplied a callback date/time function + if (dateTime_) { + dateTime_(&d->lastWriteDate, &d->lastWriteTime); + d->lastAccessDate = d->lastWriteDate; + } + // clear directory dirty + flags_ &= ~F_FILE_DIR_DIRTY; + } + return SdVolume::cacheFlush(); +} +//------------------------------------------------------------------------------ +/** + * Set a file's timestamps in its directory entry. + * + * \param[in] flags Values for \a flags are constructed by a bitwise-inclusive + * OR of flags from the following list + * + * T_ACCESS - Set the file's last access date. + * + * T_CREATE - Set the file's creation date and time. + * + * T_WRITE - Set the file's last write/modification date and time. + * + * \param[in] year Valid range 1980 - 2107 inclusive. + * + * \param[in] month Valid range 1 - 12 inclusive. + * + * \param[in] day Valid range 1 - 31 inclusive. + * + * \param[in] hour Valid range 0 - 23 inclusive. + * + * \param[in] minute Valid range 0 - 59 inclusive. + * + * \param[in] second Valid range 0 - 59 inclusive + * + * \note It is possible to set an invalid date since there is no check for + * the number of days in a month. + * + * \note + * Modify and access timestamps may be overwritten if a date time callback + * function has been set by dateTimeCallback(). + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + */ +uint8_t SdFile::timestamp(uint8_t flags, uint16_t year, uint8_t month, + uint8_t day, uint8_t hour, uint8_t minute, uint8_t second) { + if (!isOpen() + || year < 1980 + || year > 2107 + || month < 1 + || month > 12 + || day < 1 + || day > 31 + || hour > 23 + || minute > 59 + || second > 59) { + return false; + } + dir_t* d = cacheDirEntry(SdVolume::CACHE_FOR_WRITE); + if (!d) return false; + + uint16_t dirDate = FAT_DATE(year, month, day); + uint16_t dirTime = FAT_TIME(hour, minute, second); + if (flags & T_ACCESS) { + d->lastAccessDate = dirDate; + } + if (flags & T_CREATE) { + d->creationDate = dirDate; + d->creationTime = dirTime; + // seems to be units of 1/100 second not 1/10 as Microsoft states + d->creationTimeTenths = second & 1 ? 100 : 0; + } + if (flags & T_WRITE) { + d->lastWriteDate = dirDate; + d->lastWriteTime = dirTime; + } + SdVolume::cacheSetDirty(); + return sync(); +} +//------------------------------------------------------------------------------ +/** + * Truncate a file to a specified length. The current file position + * will be maintained if it is less than or equal to \a length otherwise + * it will be set to end of file. + * + * \param[in] length The desired length for the file. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. + * Reasons for failure include file is read only, file is a directory, + * \a length is greater than the current file size or an I/O error occurs. + */ +uint8_t SdFile::truncate(uint32_t length) { +// error if not a normal file or read-only + if (!isFile() || !(flags_ & O_WRITE)) return false; + + // error if length is greater than current size + if (length > fileSize_) return false; + + // fileSize and length are zero - nothing to do + if (fileSize_ == 0) return true; + + // remember position for seek after truncation + uint32_t newPos = curPosition_ > length ? length : curPosition_; + + // position to last cluster in truncated file + if (!seekSet(length)) return false; + + if (length == 0) { + // free all clusters + if (!vol_->freeChain(firstCluster_)) return false; + firstCluster_ = 0; + } else { + uint32_t toFree; + if (!vol_->fatGet(curCluster_, &toFree)) return false; + + if (!vol_->isEOC(toFree)) { + // free extra clusters + if (!vol_->freeChain(toFree)) return false; + + // current cluster is end of chain + if (!vol_->fatPutEOC(curCluster_)) return false; + } + } + fileSize_ = length; + + // need to update directory entry + flags_ |= F_FILE_DIR_DIRTY; + + if (!sync()) return false; + + // set file to correct position + return seekSet(newPos); +} +//------------------------------------------------------------------------------ +/** + * Write data to an open file. + * + * \note Data is moved to the cache but may not be written to the + * storage device until sync() is called. + * + * \param[in] buf Pointer to the location of the data to be written. + * + * \param[in] nbyte Number of bytes to write. + * + * \return For success write() returns the number of bytes written, always + * \a nbyte. If an error occurs, write() returns -1. Possible errors + * include write() is called before a file has been opened, write is called + * for a read-only file, device is full, a corrupt file system or an I/O error. + * + */ +size_t SdFile::write(const void* buf, uint16_t nbyte) { + // convert void* to uint8_t* - must be before goto statements + const uint8_t* src = reinterpret_cast(buf); + + // number of bytes left to write - must be before goto statements + uint16_t nToWrite = nbyte; + + // error if not a normal file or is read-only + if (!isFile() || !(flags_ & O_WRITE)) goto writeErrorReturn; + + // seek to end of file if append flag + if ((flags_ & O_APPEND) && curPosition_ != fileSize_) { + if (!seekEnd()) goto writeErrorReturn; + } + + while (nToWrite > 0) { + uint8_t blockOfCluster = vol_->blockOfCluster(curPosition_); + uint16_t blockOffset = curPosition_ & 0X1FF; + if (blockOfCluster == 0 && blockOffset == 0) { + // start of new cluster + if (curCluster_ == 0) { + if (firstCluster_ == 0) { + // allocate first cluster of file + if (!addCluster()) goto writeErrorReturn; + } else { + curCluster_ = firstCluster_; + } + } else { + uint32_t next; + if (!vol_->fatGet(curCluster_, &next)) return false; + if (vol_->isEOC(next)) { + // add cluster if at end of chain + if (!addCluster()) goto writeErrorReturn; + } else { + curCluster_ = next; + } + } + } + // max space in block + uint16_t n = 512 - blockOffset; + + // lesser of space and amount to write + if (n > nToWrite) n = nToWrite; + + // block for data write + uint32_t block = vol_->clusterStartBlock(curCluster_) + blockOfCluster; + if (n == 512) { + // full block - don't need to use cache + // invalidate cache if block is in cache + if (SdVolume::cacheBlockNumber_ == block) { + SdVolume::cacheBlockNumber_ = 0XFFFFFFFF; + } + if (!vol_->writeBlock(block, src)) goto writeErrorReturn; + src += 512; + } else { + if (blockOffset == 0 && curPosition_ >= fileSize_) { + // start of new block don't need to read into cache + if (!SdVolume::cacheFlush()) goto writeErrorReturn; + SdVolume::cacheBlockNumber_ = block; + SdVolume::cacheSetDirty(); + } else { + // rewrite part of block + if (!SdVolume::cacheRawBlock(block, SdVolume::CACHE_FOR_WRITE)) { + goto writeErrorReturn; + } + } + uint8_t* dst = SdVolume::cacheBuffer_.data + blockOffset; + uint8_t* end = dst + n; + while (dst != end) *dst++ = *src++; + } + nToWrite -= n; + curPosition_ += n; + } + if (curPosition_ > fileSize_) { + // update fileSize and insure sync will update dir entry + fileSize_ = curPosition_; + flags_ |= F_FILE_DIR_DIRTY; + } else if (dateTime_ && nbyte) { + // insure sync will update modified date and time + flags_ |= F_FILE_DIR_DIRTY; + } + + if (flags_ & O_SYNC) { + if (!sync()) goto writeErrorReturn; + } + return nbyte; + + writeErrorReturn: + // return for write error + //writeError = true; + setWriteError(); + return 0; +} +//------------------------------------------------------------------------------ +/** + * Write a byte to a file. Required by the Arduino Print class. + * + * Use SdFile::writeError to check for errors. + */ +size_t SdFile::write(uint8_t b) { + return write(&b, 1); +} +//------------------------------------------------------------------------------ +/** + * Write a string to a file. Used by the Arduino Print class. + * + * Use SdFile::writeError to check for errors. + */ +size_t SdFile::write(const char* str) { + return write(str, strlen(str)); +} +//------------------------------------------------------------------------------ +/** + * Write a PROGMEM string to a file. + * + * Use SdFile::writeError to check for errors. + */ +void SdFile::write_P(PGM_P str) { + for (uint8_t c; (c = pgm_read_byte(str)); str++) write(c); +} +//------------------------------------------------------------------------------ +/** + * Write a PROGMEM string followed by CR/LF to a file. + * + * Use SdFile::writeError to check for errors. + */ +void SdFile::writeln_P(PGM_P str) { + write_P(str); + println(); +} diff --git a/libs/arduino-1.0/libraries/SD/utility/SdInfo.h b/libs/arduino-1.0/libraries/SD/utility/SdInfo.h new file mode 100644 index 0000000..bc4c613 --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdInfo.h @@ -0,0 +1,232 @@ +/* Arduino Sd2Card Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino Sd2Card Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino Sd2Card Library. If not, see + * . + */ +#ifndef SdInfo_h +#define SdInfo_h +#include +// Based on the document: +// +// SD Specifications +// Part 1 +// Physical Layer +// Simplified Specification +// Version 2.00 +// September 25, 2006 +// +// www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf +//------------------------------------------------------------------------------ +// SD card commands +/** GO_IDLE_STATE - init card in spi mode if CS low */ +uint8_t const CMD0 = 0X00; +/** SEND_IF_COND - verify SD Memory Card interface operating condition.*/ +uint8_t const CMD8 = 0X08; +/** SEND_CSD - read the Card Specific Data (CSD register) */ +uint8_t const CMD9 = 0X09; +/** SEND_CID - read the card identification information (CID register) */ +uint8_t const CMD10 = 0X0A; +/** SEND_STATUS - read the card status register */ +uint8_t const CMD13 = 0X0D; +/** READ_BLOCK - read a single data block from the card */ +uint8_t const CMD17 = 0X11; +/** WRITE_BLOCK - write a single data block to the card */ +uint8_t const CMD24 = 0X18; +/** WRITE_MULTIPLE_BLOCK - write blocks of data until a STOP_TRANSMISSION */ +uint8_t const CMD25 = 0X19; +/** ERASE_WR_BLK_START - sets the address of the first block to be erased */ +uint8_t const CMD32 = 0X20; +/** ERASE_WR_BLK_END - sets the address of the last block of the continuous + range to be erased*/ +uint8_t const CMD33 = 0X21; +/** ERASE - erase all previously selected blocks */ +uint8_t const CMD38 = 0X26; +/** APP_CMD - escape for application specific command */ +uint8_t const CMD55 = 0X37; +/** READ_OCR - read the OCR register of a card */ +uint8_t const CMD58 = 0X3A; +/** SET_WR_BLK_ERASE_COUNT - Set the number of write blocks to be + pre-erased before writing */ +uint8_t const ACMD23 = 0X17; +/** SD_SEND_OP_COMD - Sends host capacity support information and + activates the card's initialization process */ +uint8_t const ACMD41 = 0X29; +//------------------------------------------------------------------------------ +/** status for card in the ready state */ +uint8_t const R1_READY_STATE = 0X00; +/** status for card in the idle state */ +uint8_t const R1_IDLE_STATE = 0X01; +/** status bit for illegal command */ +uint8_t const R1_ILLEGAL_COMMAND = 0X04; +/** start data token for read or write single block*/ +uint8_t const DATA_START_BLOCK = 0XFE; +/** stop token for write multiple blocks*/ +uint8_t const STOP_TRAN_TOKEN = 0XFD; +/** start data token for write multiple blocks*/ +uint8_t const WRITE_MULTIPLE_TOKEN = 0XFC; +/** mask for data response tokens after a write block operation */ +uint8_t const DATA_RES_MASK = 0X1F; +/** write data accepted token */ +uint8_t const DATA_RES_ACCEPTED = 0X05; +//------------------------------------------------------------------------------ +typedef struct CID { + // byte 0 + uint8_t mid; // Manufacturer ID + // byte 1-2 + char oid[2]; // OEM/Application ID + // byte 3-7 + char pnm[5]; // Product name + // byte 8 + unsigned prv_m : 4; // Product revision n.m + unsigned prv_n : 4; + // byte 9-12 + uint32_t psn; // Product serial number + // byte 13 + unsigned mdt_year_high : 4; // Manufacturing date + unsigned reserved : 4; + // byte 14 + unsigned mdt_month : 4; + unsigned mdt_year_low :4; + // byte 15 + unsigned always1 : 1; + unsigned crc : 7; +}cid_t; +//------------------------------------------------------------------------------ +// CSD for version 1.00 cards +typedef struct CSDV1 { + // byte 0 + unsigned reserved1 : 6; + unsigned csd_ver : 2; + // byte 1 + uint8_t taac; + // byte 2 + uint8_t nsac; + // byte 3 + uint8_t tran_speed; + // byte 4 + uint8_t ccc_high; + // byte 5 + unsigned read_bl_len : 4; + unsigned ccc_low : 4; + // byte 6 + unsigned c_size_high : 2; + unsigned reserved2 : 2; + unsigned dsr_imp : 1; + unsigned read_blk_misalign :1; + unsigned write_blk_misalign : 1; + unsigned read_bl_partial : 1; + // byte 7 + uint8_t c_size_mid; + // byte 8 + unsigned vdd_r_curr_max : 3; + unsigned vdd_r_curr_min : 3; + unsigned c_size_low :2; + // byte 9 + unsigned c_size_mult_high : 2; + unsigned vdd_w_cur_max : 3; + unsigned vdd_w_curr_min : 3; + // byte 10 + unsigned sector_size_high : 6; + unsigned erase_blk_en : 1; + unsigned c_size_mult_low : 1; + // byte 11 + unsigned wp_grp_size : 7; + unsigned sector_size_low : 1; + // byte 12 + unsigned write_bl_len_high : 2; + unsigned r2w_factor : 3; + unsigned reserved3 : 2; + unsigned wp_grp_enable : 1; + // byte 13 + unsigned reserved4 : 5; + unsigned write_partial : 1; + unsigned write_bl_len_low : 2; + // byte 14 + unsigned reserved5: 2; + unsigned file_format : 2; + unsigned tmp_write_protect : 1; + unsigned perm_write_protect : 1; + unsigned copy : 1; + unsigned file_format_grp : 1; + // byte 15 + unsigned always1 : 1; + unsigned crc : 7; +}csd1_t; +//------------------------------------------------------------------------------ +// CSD for version 2.00 cards +typedef struct CSDV2 { + // byte 0 + unsigned reserved1 : 6; + unsigned csd_ver : 2; + // byte 1 + uint8_t taac; + // byte 2 + uint8_t nsac; + // byte 3 + uint8_t tran_speed; + // byte 4 + uint8_t ccc_high; + // byte 5 + unsigned read_bl_len : 4; + unsigned ccc_low : 4; + // byte 6 + unsigned reserved2 : 4; + unsigned dsr_imp : 1; + unsigned read_blk_misalign :1; + unsigned write_blk_misalign : 1; + unsigned read_bl_partial : 1; + // byte 7 + unsigned reserved3 : 2; + unsigned c_size_high : 6; + // byte 8 + uint8_t c_size_mid; + // byte 9 + uint8_t c_size_low; + // byte 10 + unsigned sector_size_high : 6; + unsigned erase_blk_en : 1; + unsigned reserved4 : 1; + // byte 11 + unsigned wp_grp_size : 7; + unsigned sector_size_low : 1; + // byte 12 + unsigned write_bl_len_high : 2; + unsigned r2w_factor : 3; + unsigned reserved5 : 2; + unsigned wp_grp_enable : 1; + // byte 13 + unsigned reserved6 : 5; + unsigned write_partial : 1; + unsigned write_bl_len_low : 2; + // byte 14 + unsigned reserved7: 2; + unsigned file_format : 2; + unsigned tmp_write_protect : 1; + unsigned perm_write_protect : 1; + unsigned copy : 1; + unsigned file_format_grp : 1; + // byte 15 + unsigned always1 : 1; + unsigned crc : 7; +}csd2_t; +//------------------------------------------------------------------------------ +// union of old and new style CSD register +union csd_t { + csd1_t v1; + csd2_t v2; +}; +#endif // SdInfo_h diff --git a/libs/arduino-1.0/libraries/SD/utility/SdVolume.cpp b/libs/arduino-1.0/libraries/SD/utility/SdVolume.cpp new file mode 100644 index 0000000..e053bee --- /dev/null +++ b/libs/arduino-1.0/libraries/SD/utility/SdVolume.cpp @@ -0,0 +1,295 @@ +/* Arduino SdFat Library + * Copyright (C) 2009 by William Greiman + * + * This file is part of the Arduino SdFat Library + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino SdFat Library. If not, see + * . + */ +#include +//------------------------------------------------------------------------------ +// raw block cache +// init cacheBlockNumber_to invalid SD block number +uint32_t SdVolume::cacheBlockNumber_ = 0XFFFFFFFF; +cache_t SdVolume::cacheBuffer_; // 512 byte cache for Sd2Card +Sd2Card* SdVolume::sdCard_; // pointer to SD card object +uint8_t SdVolume::cacheDirty_ = 0; // cacheFlush() will write block if true +uint32_t SdVolume::cacheMirrorBlock_ = 0; // mirror block for second FAT +//------------------------------------------------------------------------------ +// find a contiguous group of clusters +uint8_t SdVolume::allocContiguous(uint32_t count, uint32_t* curCluster) { + // start of group + uint32_t bgnCluster; + + // flag to save place to start next search + uint8_t setStart; + + // set search start cluster + if (*curCluster) { + // try to make file contiguous + bgnCluster = *curCluster + 1; + + // don't save new start location + setStart = false; + } else { + // start at likely place for free cluster + bgnCluster = allocSearchStart_; + + // save next search start if one cluster + setStart = 1 == count; + } + // end of group + uint32_t endCluster = bgnCluster; + + // last cluster of FAT + uint32_t fatEnd = clusterCount_ + 1; + + // search the FAT for free clusters + for (uint32_t n = 0;; n++, endCluster++) { + // can't find space checked all clusters + if (n >= clusterCount_) return false; + + // past end - start from beginning of FAT + if (endCluster > fatEnd) { + bgnCluster = endCluster = 2; + } + uint32_t f; + if (!fatGet(endCluster, &f)) return false; + + if (f != 0) { + // cluster in use try next cluster as bgnCluster + bgnCluster = endCluster + 1; + } else if ((endCluster - bgnCluster + 1) == count) { + // done - found space + break; + } + } + // mark end of chain + if (!fatPutEOC(endCluster)) return false; + + // link clusters + while (endCluster > bgnCluster) { + if (!fatPut(endCluster - 1, endCluster)) return false; + endCluster--; + } + if (*curCluster != 0) { + // connect chains + if (!fatPut(*curCluster, bgnCluster)) return false; + } + // return first cluster number to caller + *curCluster = bgnCluster; + + // remember possible next free cluster + if (setStart) allocSearchStart_ = bgnCluster + 1; + + return true; +} +//------------------------------------------------------------------------------ +uint8_t SdVolume::cacheFlush(void) { + if (cacheDirty_) { + if (!sdCard_->writeBlock(cacheBlockNumber_, cacheBuffer_.data)) { + return false; + } + // mirror FAT tables + if (cacheMirrorBlock_) { + if (!sdCard_->writeBlock(cacheMirrorBlock_, cacheBuffer_.data)) { + return false; + } + cacheMirrorBlock_ = 0; + } + cacheDirty_ = 0; + } + return true; +} +//------------------------------------------------------------------------------ +uint8_t SdVolume::cacheRawBlock(uint32_t blockNumber, uint8_t action) { + if (cacheBlockNumber_ != blockNumber) { + if (!cacheFlush()) return false; + if (!sdCard_->readBlock(blockNumber, cacheBuffer_.data)) return false; + cacheBlockNumber_ = blockNumber; + } + cacheDirty_ |= action; + return true; +} +//------------------------------------------------------------------------------ +// cache a zero block for blockNumber +uint8_t SdVolume::cacheZeroBlock(uint32_t blockNumber) { + if (!cacheFlush()) return false; + + // loop take less flash than memset(cacheBuffer_.data, 0, 512); + for (uint16_t i = 0; i < 512; i++) { + cacheBuffer_.data[i] = 0; + } + cacheBlockNumber_ = blockNumber; + cacheSetDirty(); + return true; +} +//------------------------------------------------------------------------------ +// return the size in bytes of a cluster chain +uint8_t SdVolume::chainSize(uint32_t cluster, uint32_t* size) const { + uint32_t s = 0; + do { + if (!fatGet(cluster, &cluster)) return false; + s += 512UL << clusterSizeShift_; + } while (!isEOC(cluster)); + *size = s; + return true; +} +//------------------------------------------------------------------------------ +// Fetch a FAT entry +uint8_t SdVolume::fatGet(uint32_t cluster, uint32_t* value) const { + if (cluster > (clusterCount_ + 1)) return false; + uint32_t lba = fatStartBlock_; + lba += fatType_ == 16 ? cluster >> 8 : cluster >> 7; + if (lba != cacheBlockNumber_) { + if (!cacheRawBlock(lba, CACHE_FOR_READ)) return false; + } + if (fatType_ == 16) { + *value = cacheBuffer_.fat16[cluster & 0XFF]; + } else { + *value = cacheBuffer_.fat32[cluster & 0X7F] & FAT32MASK; + } + return true; +} +//------------------------------------------------------------------------------ +// Store a FAT entry +uint8_t SdVolume::fatPut(uint32_t cluster, uint32_t value) { + // error if reserved cluster + if (cluster < 2) return false; + + // error if not in FAT + if (cluster > (clusterCount_ + 1)) return false; + + // calculate block address for entry + uint32_t lba = fatStartBlock_; + lba += fatType_ == 16 ? cluster >> 8 : cluster >> 7; + + if (lba != cacheBlockNumber_) { + if (!cacheRawBlock(lba, CACHE_FOR_READ)) return false; + } + // store entry + if (fatType_ == 16) { + cacheBuffer_.fat16[cluster & 0XFF] = value; + } else { + cacheBuffer_.fat32[cluster & 0X7F] = value; + } + cacheSetDirty(); + + // mirror second FAT + if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_; + return true; +} +//------------------------------------------------------------------------------ +// free a cluster chain +uint8_t SdVolume::freeChain(uint32_t cluster) { + // clear free cluster location + allocSearchStart_ = 2; + + do { + uint32_t next; + if (!fatGet(cluster, &next)) return false; + + // free cluster + if (!fatPut(cluster, 0)) return false; + + cluster = next; + } while (!isEOC(cluster)); + + return true; +} +//------------------------------------------------------------------------------ +/** + * Initialize a FAT volume. + * + * \param[in] dev The SD card where the volume is located. + * + * \param[in] part The partition to be used. Legal values for \a part are + * 1-4 to use the corresponding partition on a device formatted with + * a MBR, Master Boot Record, or zero if the device is formatted as + * a super floppy with the FAT boot sector in block zero. + * + * \return The value one, true, is returned for success and + * the value zero, false, is returned for failure. Reasons for + * failure include not finding a valid partition, not finding a valid + * FAT file system in the specified partition or an I/O error. + */ +uint8_t SdVolume::init(Sd2Card* dev, uint8_t part) { + uint32_t volumeStartBlock = 0; + sdCard_ = dev; + // if part == 0 assume super floppy with FAT boot sector in block zero + // if part > 0 assume mbr volume with partition table + if (part) { + if (part > 4)return false; + if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) return false; + part_t* p = &cacheBuffer_.mbr.part[part-1]; + if ((p->boot & 0X7F) !=0 || + p->totalSectors < 100 || + p->firstSector == 0) { + // not a valid partition + return false; + } + volumeStartBlock = p->firstSector; + } + if (!cacheRawBlock(volumeStartBlock, CACHE_FOR_READ)) return false; + bpb_t* bpb = &cacheBuffer_.fbs.bpb; + if (bpb->bytesPerSector != 512 || + bpb->fatCount == 0 || + bpb->reservedSectorCount == 0 || + bpb->sectorsPerCluster == 0) { + // not valid FAT volume + return false; + } + fatCount_ = bpb->fatCount; + blocksPerCluster_ = bpb->sectorsPerCluster; + + // determine shift that is same as multiply by blocksPerCluster_ + clusterSizeShift_ = 0; + while (blocksPerCluster_ != (1 << clusterSizeShift_)) { + // error if not power of 2 + if (clusterSizeShift_++ > 7) return false; + } + blocksPerFat_ = bpb->sectorsPerFat16 ? + bpb->sectorsPerFat16 : bpb->sectorsPerFat32; + + fatStartBlock_ = volumeStartBlock + bpb->reservedSectorCount; + + // count for FAT16 zero for FAT32 + rootDirEntryCount_ = bpb->rootDirEntryCount; + + // directory start for FAT16 dataStart for FAT32 + rootDirStart_ = fatStartBlock_ + bpb->fatCount * blocksPerFat_; + + // data start for FAT16 and FAT32 + dataStartBlock_ = rootDirStart_ + ((32 * bpb->rootDirEntryCount + 511)/512); + + // total blocks for FAT16 or FAT32 + uint32_t totalBlocks = bpb->totalSectors16 ? + bpb->totalSectors16 : bpb->totalSectors32; + // total data blocks + clusterCount_ = totalBlocks - (dataStartBlock_ - volumeStartBlock); + + // divide by cluster size to get cluster count + clusterCount_ >>= clusterSizeShift_; + + // FAT type is determined by cluster count + if (clusterCount_ < 4085) { + fatType_ = 12; + } else if (clusterCount_ < 65525) { + fatType_ = 16; + } else { + rootDirStart_ = bpb->fat32RootCluster; + fatType_ = 32; + } + return true; +} diff --git a/libs/arduino-1.0/libraries/SPI/SPI.cpp b/libs/arduino-1.0/libraries/SPI/SPI.cpp new file mode 100644 index 0000000..5e48073 --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/SPI.cpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2010 by Cristian Maglie + * SPI Master library for arduino. + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#include "pins_arduino.h" +#include "SPI.h" + +SPIClass SPI; + +void SPIClass::begin() { + + // Set SS to high so a connected chip will be "deselected" by default + digitalWrite(SS, HIGH); + + // When the SS pin is set as OUTPUT, it can be used as + // a general purpose output port (it doesn't influence + // SPI operations). + pinMode(SS, OUTPUT); + + // Warning: if the SS pin ever becomes a LOW INPUT then SPI + // automatically switches to Slave, so the data direction of + // the SS pin MUST be kept as OUTPUT. + SPCR |= _BV(MSTR); + SPCR |= _BV(SPE); + + // Set direction register for SCK and MOSI pin. + // MISO pin automatically overrides to INPUT. + // By doing this AFTER enabling SPI, we avoid accidentally + // clocking in a single bit since the lines go directly + // from "input" to SPI control. + // http://code.google.com/p/arduino/issues/detail?id=888 + pinMode(SCK, OUTPUT); + pinMode(MOSI, OUTPUT); +} + + +void SPIClass::end() { + SPCR &= ~_BV(SPE); +} + +void SPIClass::setBitOrder(uint8_t bitOrder) +{ + if(bitOrder == LSBFIRST) { + SPCR |= _BV(DORD); + } else { + SPCR &= ~(_BV(DORD)); + } +} + +void SPIClass::setDataMode(uint8_t mode) +{ + SPCR = (SPCR & ~SPI_MODE_MASK) | mode; +} + +void SPIClass::setClockDivider(uint8_t rate) +{ + SPCR = (SPCR & ~SPI_CLOCK_MASK) | (rate & SPI_CLOCK_MASK); + SPSR = (SPSR & ~SPI_2XCLOCK_MASK) | ((rate >> 2) & SPI_2XCLOCK_MASK); +} + diff --git a/libs/arduino-1.0/libraries/SPI/SPI.h b/libs/arduino-1.0/libraries/SPI/SPI.h new file mode 100644 index 0000000..f647d5c --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/SPI.h @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2010 by Cristian Maglie + * SPI Master library for arduino. + * + * This file is free software; you can redistribute it and/or modify + * it under the terms of either the GNU General Public License version 2 + * or the GNU Lesser General Public License version 2.1, both as + * published by the Free Software Foundation. + */ + +#ifndef _SPI_H_INCLUDED +#define _SPI_H_INCLUDED + +#include +#include +#include + +#define SPI_CLOCK_DIV4 0x00 +#define SPI_CLOCK_DIV16 0x01 +#define SPI_CLOCK_DIV64 0x02 +#define SPI_CLOCK_DIV128 0x03 +#define SPI_CLOCK_DIV2 0x04 +#define SPI_CLOCK_DIV8 0x05 +#define SPI_CLOCK_DIV32 0x06 +//#define SPI_CLOCK_DIV64 0x07 + +#define SPI_MODE0 0x00 +#define SPI_MODE1 0x04 +#define SPI_MODE2 0x08 +#define SPI_MODE3 0x0C + +#define SPI_MODE_MASK 0x0C // CPOL = bit 3, CPHA = bit 2 on SPCR +#define SPI_CLOCK_MASK 0x03 // SPR1 = bit 1, SPR0 = bit 0 on SPCR +#define SPI_2XCLOCK_MASK 0x01 // SPI2X = bit 0 on SPSR + +class SPIClass { +public: + inline static byte transfer(byte _data); + + // SPI Configuration methods + + inline static void attachInterrupt(); + inline static void detachInterrupt(); // Default + + static void begin(); // Default + static void end(); + + static void setBitOrder(uint8_t); + static void setDataMode(uint8_t); + static void setClockDivider(uint8_t); +}; + +extern SPIClass SPI; + +byte SPIClass::transfer(byte _data) { + SPDR = _data; + while (!(SPSR & _BV(SPIF))) + ; + return SPDR; +} + +void SPIClass::attachInterrupt() { + SPCR |= _BV(SPIE); +} + +void SPIClass::detachInterrupt() { + SPCR &= ~_BV(SPIE); +} + +#endif diff --git a/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino b/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino new file mode 100644 index 0000000..9d77a42 --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor.ino @@ -0,0 +1,143 @@ +/* + SCP1000 Barometric Pressure Sensor Display + + Shows the output of a Barometric Pressure Sensor on a + Uses the SPI library. For details on the sensor, see: + http://www.sparkfun.com/commerce/product_info.php?products_id=8161 + http://www.vti.fi/en/support/obsolete_products/pressure_sensors/ + + This sketch adapted from Nathan Seidle's SCP1000 example for PIC: + http://www.sparkfun.com/datasheets/Sensors/SCP1000-Testing.zip + + Circuit: + SCP1000 sensor attached to pins 6, 7, 10 - 13: + DRDY: pin 6 + CSB: pin 7 + MOSI: pin 11 + MISO: pin 12 + SCK: pin 13 + + created 31 July 2010 + modified 14 August 2010 + by Tom Igoe + */ + +// the sensor communicates using SPI, so include the library: +#include + +//Sensor's memory register addresses: +const int PRESSURE = 0x1F; //3 most significant bits of pressure +const int PRESSURE_LSB = 0x20; //16 least significant bits of pressure +const int TEMPERATURE = 0x21; //16 bit temperature reading +const byte READ = 0b11111100; // SCP1000's read command +const byte WRITE = 0b00000010; // SCP1000's write command + +// pins used for the connection with the sensor +// the other you need are controlled by the SPI library): +const int dataReadyPin = 6; +const int chipSelectPin = 7; + +void setup() { + Serial.begin(9600); + + // start the SPI library: + SPI.begin(); + + // initalize the data ready and chip select pins: + pinMode(dataReadyPin, INPUT); + pinMode(chipSelectPin, OUTPUT); + + //Configure SCP1000 for low noise configuration: + writeRegister(0x02, 0x2D); + writeRegister(0x01, 0x03); + writeRegister(0x03, 0x02); + // give the sensor time to set up: + delay(100); +} + +void loop() { + //Select High Resolution Mode + writeRegister(0x03, 0x0A); + + // don't do anything until the data ready pin is high: + if (digitalRead(dataReadyPin) == HIGH) { + //Read the temperature data + int tempData = readRegister(0x21, 2); + + // convert the temperature to celsius and display it: + float realTemp = (float)tempData / 20.0; + Serial.print("Temp[C]="); + Serial.print(realTemp); + + + //Read the pressure data highest 3 bits: + byte pressure_data_high = readRegister(0x1F, 1); + pressure_data_high &= 0b00000111; //you only needs bits 2 to 0 + + //Read the pressure data lower 16 bits: + unsigned int pressure_data_low = readRegister(0x20, 2); + //combine the two parts into one 19-bit number: + long pressure = ((pressure_data_high << 16) | pressure_data_low)/4; + + // display the temperature: + Serial.println("\tPressure [Pa]=" + String(pressure)); + } +} + +//Read from or write to register from the SCP1000: +unsigned int readRegister(byte thisRegister, int bytesToRead ) { + byte inByte = 0; // incoming byte from the SPI + unsigned int result = 0; // result to return + Serial.print(thisRegister, BIN); + Serial.print("\t"); + // SCP1000 expects the register name in the upper 6 bits + // of the byte. So shift the bits left by two bits: + thisRegister = thisRegister << 2; + // now combine the address and the command into one byte + byte dataToSend = thisRegister & READ; + Serial.println(thisRegister, BIN); + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + // send the device the register you want to read: + SPI.transfer(dataToSend); + // send a value of 0 to read the first byte returned: + result = SPI.transfer(0x00); + // decrement the number of bytes left to read: + bytesToRead--; + // if you still have another byte to read: + if (bytesToRead > 0) { + // shift the first byte left, then get the second byte: + result = result << 8; + inByte = SPI.transfer(0x00); + // combine the byte you just got with the previous one: + result = result | inByte; + // decrement the number of bytes left to read: + bytesToRead--; + } + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); + // return the result: + return(result); +} + + +//Sends a write command to SCP1000 + +void writeRegister(byte thisRegister, byte thisValue) { + + // SCP1000 expects the register address in the upper 6 bits + // of the byte. So shift the bits left by two bits: + thisRegister = thisRegister << 2; + // now combine the register address and the command into one byte: + byte dataToSend = thisRegister | WRITE; + + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + + SPI.transfer(dataToSend); //Send register location + SPI.transfer(thisValue); //Send value to record into register + + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); +} + diff --git a/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor/BarometricPressureSensor.ino b/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor/BarometricPressureSensor.ino new file mode 100644 index 0000000..9c9c9b6 --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/examples/BarometricPressureSensor/BarometricPressureSensor/BarometricPressureSensor.ino @@ -0,0 +1,143 @@ +/* + SCP1000 Barometric Pressure Sensor Display + + Shows the output of a Barometric Pressure Sensor on a + Uses the SPI library. For details on the sensor, see: + http://www.sparkfun.com/commerce/product_info.php?products_id=8161 + http://www.vti.fi/en/support/obsolete_products/pressure_sensors/ + + This sketch adapted from Nathan Seidle's SCP1000 example for PIC: + http://www.sparkfun.com/datasheets/Sensors/SCP1000-Testing.zip + + Circuit: + SCP1000 sensor attached to pins 6, 7, 10 - 13: + DRDY: pin 6 + CSB: pin 7 + MOSI: pin 11 + MISO: pin 12 + SCK: pin 13 + + created 31 July 2010 + modified 14 August 2010 + by Tom Igoe + */ + +// the sensor communicates using SPI, so include the library: +#include + +//Sensor's memory register addresses: +const int PRESSURE = 0x1F; //3 most significant bits of pressure +const int PRESSURE_LSB = 0x20; //16 least significant bits of pressure +const int TEMPERATURE = 0x21; //16 bit temperature reading +cont byte READ = 0b00000000; // SCP1000's read command +const byte WRITE = 0b00000010; // SCP1000's write command +// pins used for the connection with the sensor +// the other you need are controlled by the SPI library): +const int dataReadyPin = 6; +const int chipSelectPin = 7; + +void setup() { + Serial.begin(9600); + + // start the SPI library: + SPI.begin(); + + // initalize the data ready and chip select pins: + pinMode(dataReadyPin, INPUT); + pinMode(chipSelectPin, OUTPUT); + + //Configure SCP1000 for low noise configuration: + writeRegister(0x02, 0x2D); + writeRegister(0x01, 0x03); + writeRegister(0x03, 0x02); + // give the sensor time to set up: + delay(100); +} + +void loop() { + //Select High Resolution Mode + writeRegister(0x03, 0x0A); + + // don't do anything until the data ready pin is high: + if (digitalRead(dataReadyPin) == HIGH) { + //Read the temperature data + int tempData = readRegister(0x21, 2); + + // convert the temperature to celsius and display it: + float realTemp = (float)tempData / 20.0; + Serial.print("Temp[C]="); + Serial.print(realTemp); + + + //Read the pressure data highest 3 bits: + byte pressure_data_high = readRegister(0x1F, 1); + pressure_data_high &= 0b00000111; //you only needs bits 2 to 0 + + //Read the pressure data lower 16 bits: + unsigned int pressure_data_low = readRegister(0x20, 2); + //combine the two parts into one 19-bit number: + long pressure = ((pressure_data_high << 16) | pressure_data_low)/4; + + // display the temperature: + Serial.println("\tPressure [Pa]=" + String(pressure)); + } +} + +//Read from or write to register from the SCP1000: +unsigned int readRegister(byte thisRegister, int bytesToRead ) { + byte inByte = 0; // incoming byte from the SPI + unsigned int result = 0; // result to return + + // SCP1000 expects the register name in the upper 6 bits + // of the byte. So shift the bits left by two bits: + thisRegister = thisRegister << 2; + // now combine the address and the command into one byte + dataToSend = thisRegister & READ; + + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + // send the device the register you want to read: + SPI.transfer(dataToSend); + // send a value of 0 to read the first byte returned: + result = SPI.transfer(0x00); + // decrement the number of bytes left to read: + bytesToRead--; + // if you still have another byte to read: + if (bytesToRead > 0) { + // shift the first byte left, then get the second byte: + result = result << 8; + inByte = SPI.transfer(0x00); + // combine the byte you just got with the previous one: + result = result | inByte; + // decrement the number of bytes left to read: + bytesToRead--; + } + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); + // return the result: + return(result); +} + + +//Sends a write command to SCP1000 + +void writeRegister(byte thisRegister, byte thisValue) { + + // SCP1000 expects the register address in the upper 6 bits + // of the byte. So shift the bits left by two bits: + thisRegister = thisRegister << 2; + // now combine the register address and the command into one byte: + dataToSend = thisRegister | WRITE; + + // take the chip select low to select the device: + digitalWrite(chipSelectPin, LOW); + + SPI.transfer(dataToSend); //Send register location + SPI.transfer(thisValue); //Send value to record into register + + // take the chip select high to de-select: + digitalWrite(chipSelectPin, HIGH); +} + + + diff --git a/libs/arduino-1.0/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino b/libs/arduino-1.0/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino new file mode 100644 index 0000000..ef97dae --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino @@ -0,0 +1,71 @@ +/* + Digital Pot Control + + This example controls an Analog Devices AD5206 digital potentiometer. + The AD5206 has 6 potentiometer channels. Each channel's pins are labeled + A - connect this to voltage + W - this is the pot's wiper, which changes when you set it + B - connect this to ground. + + The AD5206 is SPI-compatible,and to command it, you send two bytes, + one with the channel number (0 - 5) and one with the resistance value for the + channel (0 - 255). + + The circuit: + * All A pins of AD5206 connected to +5V + * All B pins of AD5206 connected to ground + * An LED and a 220-ohm resisor in series connected from each W pin to ground + * CS - to digital pin 10 (SS pin) + * SDI - to digital pin 11 (MOSI pin) + * CLK - to digital pin 13 (SCK pin) + + created 10 Aug 2010 + by Tom Igoe + + Thanks to Heather Dewey-Hagborg for the original tutorial, 2005 + +*/ + + +// inslude the SPI library: +#include + + +// set pin 10 as the slave select for the digital pot: +const int slaveSelectPin = 10; + +void setup() { + // set the slaveSelectPin as an output: + pinMode (slaveSelectPin, OUTPUT); + // initialize SPI: + SPI.begin(); +} + +void loop() { + // go through the six channels of the digital pot: + for (int channel = 0; channel < 6; channel++) { + // change the resistance on this channel from min to max: + for (int level = 0; level < 255; level++) { + digitalPotWrite(channel, level); + delay(10); + } + // wait a second at the top: + delay(100); + // change the resistance on this channel from max to min: + for (int level = 0; level < 255; level++) { + digitalPotWrite(channel, 255 - level); + delay(10); + } + } + +} + +int digitalPotWrite(int address, int value) { + // take the SS pin low to select the chip: + digitalWrite(slaveSelectPin,LOW); + // send in the address and value via SPI: + SPI.transfer(address); + SPI.transfer(value); + // take the SS pin high to de-select the chip: + digitalWrite(slaveSelectPin,HIGH); +} \ No newline at end of file diff --git a/libs/arduino-1.0/libraries/SPI/keywords.txt b/libs/arduino-1.0/libraries/SPI/keywords.txt new file mode 100644 index 0000000..fa76165 --- /dev/null +++ b/libs/arduino-1.0/libraries/SPI/keywords.txt @@ -0,0 +1,36 @@ +####################################### +# Syntax Coloring Map SPI +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +SPI KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### +begin KEYWORD2 +end KEYWORD2 +transfer KEYWORD2 +setBitOrder KEYWORD2 +setDataMode KEYWORD2 +setClockDivider KEYWORD2 + + +####################################### +# Constants (LITERAL1) +####################################### +SPI_CLOCK_DIV4 LITERAL1 +SPI_CLOCK_DIV16 LITERAL1 +SPI_CLOCK_DIV64 LITERAL1 +SPI_CLOCK_DIV128 LITERAL1 +SPI_CLOCK_DIV2 LITERAL1 +SPI_CLOCK_DIV8 LITERAL1 +SPI_CLOCK_DIV32 LITERAL1 +SPI_CLOCK_DIV64 LITERAL1 +SPI_MODE0 LITERAL1 +SPI_MODE1 LITERAL1 +SPI_MODE2 LITERAL1 +SPI_MODE3 LITERAL1 \ No newline at end of file diff --git a/libs/arduino-1.0/libraries/Servo/Servo.cpp b/libs/arduino-1.0/libraries/Servo/Servo.cpp new file mode 100644 index 0000000..2c0fd71 --- /dev/null +++ b/libs/arduino-1.0/libraries/Servo/Servo.cpp @@ -0,0 +1,337 @@ +/* + Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + Copyright (c) 2009 Michael Margolis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/* + + A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. + The servos are pulsed in the background using the value most recently written using the write() method + + Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. + Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. + + The methods are: + + Servo - Class for manipulating servo motors connected to Arduino pins. + + attach(pin ) - Attaches a servo motor to an i/o pin. + attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds + default min is 544, max is 2400 + + write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) + writeMicroseconds() - Sets the servo pulse width in microseconds + read() - Gets the last written servo pulse width as an angle between 0 and 180. + readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) + attached() - Returns true if there is a servo attached. + detach() - Stops an attached servos from pulsing its i/o pin. + +*/ + +#include +#include + +#include "Servo.h" + +#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 +#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds + + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 + +//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) + +static servo_t servos[MAX_SERVOS]; // static array of servo structures +static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) + +uint8_t ServoCount = 0; // the total number of attached servos + + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel +#define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel + +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo + +/************ static functions common to all instances ***********************/ + +static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) +{ + if( Channel[timer] < 0 ) + *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer + else{ + if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) + digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated + } + + Channel[timer]++; // increment to the next channel + if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; + if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated + digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high + } + else { + // finished all channels so wait for the refresh period to expire before starting over + if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); + else + *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed + Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + } +} + +#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform +// Interrupt handlers for Arduino +#if defined(_useTimer1) +SIGNAL (TIMER1_COMPA_vect) +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif + +#if defined(_useTimer3) +SIGNAL (TIMER3_COMPA_vect) +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif + +#if defined(_useTimer4) +SIGNAL (TIMER4_COMPA_vect) +{ + handle_interrupts(_timer4, &TCNT4, &OCR4A); +} +#endif + +#if defined(_useTimer5) +SIGNAL (TIMER5_COMPA_vect) +{ + handle_interrupts(_timer5, &TCNT5, &OCR5A); +} +#endif + +#elif defined WIRING +// Interrupt handlers for Wiring +#if defined(_useTimer1) +void Timer1Service() +{ + handle_interrupts(_timer1, &TCNT1, &OCR1A); +} +#endif +#if defined(_useTimer3) +void Timer3Service() +{ + handle_interrupts(_timer3, &TCNT3, &OCR3A); +} +#endif +#endif + + +static void initISR(timer16_Sequence_t timer) +{ +#if defined (_useTimer1) + if(timer == _timer1) { + TCCR1A = 0; // normal counting mode + TCCR1B = _BV(CS11); // set prescaler of 8 + TCNT1 = 0; // clear the timer count +#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) + TIFR |= _BV(OCF1A); // clear any pending interrupts; + TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt +#else + // here if not ATmega8 or ATmega128 + TIFR1 |= _BV(OCF1A); // clear any pending interrupts; + TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); +#endif + } +#endif + +#if defined (_useTimer3) + if(timer == _timer3) { + TCCR3A = 0; // normal counting mode + TCCR3B = _BV(CS31); // set prescaler of 8 + TCNT3 = 0; // clear the timer count +#if defined(__AVR_ATmega128__) + TIFR |= _BV(OCF3A); // clear any pending interrupts; + ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt +#else + TIFR3 = _BV(OCF3A); // clear any pending interrupts; + TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt +#endif +#if defined(WIRING) + timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only +#endif + } +#endif + +#if defined (_useTimer4) + if(timer == _timer4) { + TCCR4A = 0; // normal counting mode + TCCR4B = _BV(CS41); // set prescaler of 8 + TCNT4 = 0; // clear the timer count + TIFR4 = _BV(OCF4A); // clear any pending interrupts; + TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt + } +#endif + +#if defined (_useTimer5) + if(timer == _timer5) { + TCCR5A = 0; // normal counting mode + TCCR5B = _BV(CS51); // set prescaler of 8 + TCNT5 = 0; // clear the timer count + TIFR5 = _BV(OCF5A); // clear any pending interrupts; + TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt + } +#endif +} + +static void finISR(timer16_Sequence_t timer) +{ + //disable use of the given timer +#if defined WIRING // Wiring + if(timer == _timer1) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #else + TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt + #endif + timerDetach(TIMER1OUTCOMPAREA_INT); + } + else if(timer == _timer3) { + #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) + TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #else + ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + #endif + timerDetach(TIMER3OUTCOMPAREA_INT); + } +#else + //For arduino - in future: call here to a currently undefined function to reset the timer +#endif +} + +static boolean isTimerActive(timer16_Sequence_t timer) +{ + // returns true if any servo is active on this timer + for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { + if(SERVO(timer,channel).Pin.isActive == true) + return true; + } + return false; +} + + +/****************** end of static functions ******************************/ + +Servo::Servo() +{ + if( ServoCount < MAX_SERVOS) { + this->servoIndex = ServoCount++; // assign a servo index to this instance + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 + } + else + this->servoIndex = INVALID_SERVO ; // too many servos +} + +uint8_t Servo::attach(int pin) +{ + return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +uint8_t Servo::attach(int pin, int min, int max) +{ + if(this->servoIndex < MAX_SERVOS ) { + pinMode( pin, OUTPUT) ; // set servo pin to output + servos[this->servoIndex].Pin.nbr = pin; + // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 + this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS + this->max = (MAX_PULSE_WIDTH - max)/4; + // initialize the timer if it has not already been initialized + timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) + initISR(timer); + servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + } + return this->servoIndex ; +} + +void Servo::detach() +{ + servos[this->servoIndex].Pin.isActive = false; + timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if(isTimerActive(timer) == false) { + finISR(timer); + } +} + +void Servo::write(int value) +{ + if(value < MIN_PULSE_WIDTH) + { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + if(value < 0) value = 0; + if(value > 180) value = 180; + value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); + } + this->writeMicroseconds(value); +} + +void Servo::writeMicroseconds(int value) +{ + // calculate and store the values for the given channel + byte channel = this->servoIndex; + if( (channel < MAX_SERVOS) ) // ensure channel is valid + { + if( value < SERVO_MIN() ) // ensure pulse width is valid + value = SERVO_MIN(); + else if( value > SERVO_MAX() ) + value = SERVO_MAX(); + + value = value - TRIM_DURATION; + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009 + + uint8_t oldSREG = SREG; + cli(); + servos[channel].ticks = value; + SREG = oldSREG; + } +} + +int Servo::read() // return the value as degrees +{ + return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); +} + +int Servo::readMicroseconds() +{ + unsigned int pulsewidth; + if( this->servoIndex != INVALID_SERVO ) + pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009 + else + pulsewidth = 0; + + return pulsewidth; +} + +bool Servo::attached() +{ + return servos[this->servoIndex].Pin.isActive ; +} diff --git a/libs/arduino-1.0/libraries/Servo/Servo.h b/libs/arduino-1.0/libraries/Servo/Servo.h new file mode 100644 index 0000000..8168494 --- /dev/null +++ b/libs/arduino-1.0/libraries/Servo/Servo.h @@ -0,0 +1,126 @@ +/* + Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + Copyright (c) 2009 Michael Margolis. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +/* + + A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. + The servos are pulsed in the background using the value most recently written using the write() method + + Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. + Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. + The sequence used to sieze timers is defined in timers.h + + The methods are: + + Servo - Class for manipulating servo motors connected to Arduino pins. + + attach(pin ) - Attaches a servo motor to an i/o pin. + attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds + default min is 544, max is 2400 + + write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) + writeMicroseconds() - Sets the servo pulse width in microseconds + read() - Gets the last written servo pulse width as an angle between 0 and 180. + readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) + attached() - Returns true if there is a servo attached. + detach() - Stops an attached servos from pulsing its i/o pin. + */ + +#ifndef Servo_h +#define Servo_h + +#include + +/* + * Defines for 16 bit timers used with Servo library + * + * If _useTimerX is defined then TimerX is a 16 bit timer on the curent board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + * + */ + +// Say which 16 bit timers can be used and in what order +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) +#define _useTimer5 +#define _useTimer1 +#define _useTimer3 +#define _useTimer4 +typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; + +#elif defined(__AVR_ATmega32U4__) +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; + +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; + +#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) +#define _useTimer3 +#define _useTimer1 +typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; + +#else // everything else +#define _useTimer1 +typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; +#endif + +#define Servo_VERSION 2 // software version of this library + +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds + +#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer +#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + +typedef struct { + uint8_t nbr :6 ; // a pin number from 0 to 63 + uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t ; + +typedef struct { + ServoPin_t Pin; + unsigned int ticks; +} servo_t; + +class Servo +{ +public: + Servo(); + uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure + uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. + void detach(); + void write(int value); // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds + void writeMicroseconds(int value); // Write pulse width in microseconds + int read(); // returns current pulse width as an angle between 0 and 180 degrees + int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) + bool attached(); // return true if this servo is attached, otherwise false +private: + uint8_t servoIndex; // index into the channel data for this servo + int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH + int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH +}; + +#endif diff --git a/libs/arduino-1.0/libraries/Servo/examples/Knob/Knob.ino b/libs/arduino-1.0/libraries/Servo/examples/Knob/Knob.ino new file mode 100644 index 0000000..886e107 --- /dev/null +++ b/libs/arduino-1.0/libraries/Servo/examples/Knob/Knob.ino @@ -0,0 +1,22 @@ +// Controlling a servo position using a potentiometer (variable resistor) +// by Michal Rinott + +#include + +Servo myservo; // create servo object to control a servo + +int potpin = 0; // analog pin used to connect the potentiometer +int val; // variable to read the value from the analog pin + +void setup() +{ + myservo.attach(9); // attaches the servo on pin 9 to the servo object +} + +void loop() +{ + val = analogRead(potpin); // reads the value of the potentiometer (value between 0 and 1023) + val = map(val, 0, 1023, 0, 179); // scale it to use it with the servo (value between 0 and 180) + myservo.write(val); // sets the servo position according to the scaled value + delay(15); // waits for the servo to get there +} diff --git a/libs/arduino-1.0/libraries/Servo/examples/Sweep/Sweep.ino b/libs/arduino-1.0/libraries/Servo/examples/Sweep/Sweep.ino new file mode 100644 index 0000000..fb326e7 --- /dev/null +++ b/libs/arduino-1.0/libraries/Servo/examples/Sweep/Sweep.ino @@ -0,0 +1,31 @@ +// Sweep +// by BARRAGAN +// This example code is in the public domain. + + +#include + +Servo myservo; // create servo object to control a servo + // a maximum of eight servo objects can be created + +int pos = 0; // variable to store the servo position + +void setup() +{ + myservo.attach(9); // attaches the servo on pin 9 to the servo object +} + + +void loop() +{ + for(pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees + { // in steps of 1 degree + myservo.write(pos); // tell servo to go to position in variable 'pos' + delay(15); // waits 15ms for the servo to reach the position + } + for(pos = 180; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees + { + myservo.write(pos); // tell servo to go to position in variable 'pos' + delay(15); // waits 15ms for the servo to reach the position + } +} diff --git a/libs/arduino-1.0/libraries/Servo/keywords.txt b/libs/arduino-1.0/libraries/Servo/keywords.txt new file mode 100644 index 0000000..ca5ba79 --- /dev/null +++ b/libs/arduino-1.0/libraries/Servo/keywords.txt @@ -0,0 +1,24 @@ +####################################### +# Syntax Coloring Map Servo +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +Servo KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### +attach KEYWORD2 +detach KEYWORD2 +write KEYWORD2 +read KEYWORD2 +attached KEYWORD2 +writeMicroseconds KEYWORD2 +readMicroseconds KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### diff --git a/libs/arduino-1.0/libraries/SoftwareSerial/SoftwareSerial.cpp b/libs/arduino-1.0/libraries/SoftwareSerial/SoftwareSerial.cpp new file mode 100644 index 0000000..c2c2390 --- /dev/null +++ b/libs/arduino-1.0/libraries/SoftwareSerial/SoftwareSerial.cpp @@ -0,0 +1,518 @@ +/* +SoftwareSerial.cpp (formerly NewSoftSerial.cpp) - +Multi-instance software serial library for Arduino/Wiring +-- Interrupt-driven receive and other improvements by ladyada + (http://ladyada.net) +-- Tuning, circular buffer, derivation from class Print/Stream, + multi-instance support, porting to 8MHz processors, + various optimizations, PROGMEM delay tables, inverse logic and + direct port writing by Mikal Hart (http://www.arduiniana.org) +-- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com) +-- 20MHz processor support by Garrett Mace (http://www.macetech.com) +-- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/) + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +The latest version of this library can always be found at +http://arduiniana.org. +*/ + +// When set, _DEBUG co-opts pins 11 and 13 for debugging with an +// oscilloscope or logic analyzer. Beware: it also slightly modifies +// the bit times, so don't rely on it too much at high baud rates +#define _DEBUG 0 +#define _DEBUG_PIN1 11 +#define _DEBUG_PIN2 13 +// +// Includes +// +#include +#include +#include +#include +// +// Lookup table +// +typedef struct _DELAY_TABLE +{ + long baud; + unsigned short rx_delay_centering; + unsigned short rx_delay_intrabit; + unsigned short rx_delay_stopbit; + unsigned short tx_delay; +} DELAY_TABLE; + +#if F_CPU == 16000000 + +static const DELAY_TABLE PROGMEM table[] = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 1, 17, 17, 12, }, + { 57600, 10, 37, 37, 33, }, + { 38400, 25, 57, 57, 54, }, + { 31250, 31, 70, 70, 68, }, + { 28800, 34, 77, 77, 74, }, + { 19200, 54, 117, 117, 114, }, + { 14400, 74, 156, 156, 153, }, + { 9600, 114, 236, 236, 233, }, + { 4800, 233, 474, 474, 471, }, + { 2400, 471, 950, 950, 947, }, + { 1200, 947, 1902, 1902, 1899, }, + { 600, 1902, 3804, 3804, 3800, }, + { 300, 3804, 7617, 7617, 7614, }, +}; + +const int XMIT_START_ADJUSTMENT = 5; + +#elif F_CPU == 8000000 + +static const DELAY_TABLE table[] PROGMEM = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 1, 5, 5, 3, }, + { 57600, 1, 15, 15, 13, }, + { 38400, 2, 25, 26, 23, }, + { 31250, 7, 32, 33, 29, }, + { 28800, 11, 35, 35, 32, }, + { 19200, 20, 55, 55, 52, }, + { 14400, 30, 75, 75, 72, }, + { 9600, 50, 114, 114, 112, }, + { 4800, 110, 233, 233, 230, }, + { 2400, 229, 472, 472, 469, }, + { 1200, 467, 948, 948, 945, }, + { 600, 948, 1895, 1895, 1890, }, + { 300, 1895, 3805, 3805, 3802, }, +}; + +const int XMIT_START_ADJUSTMENT = 4; + +#elif F_CPU == 20000000 + +// 20MHz support courtesy of the good people at macegr.com. +// Thanks, Garrett! + +static const DELAY_TABLE PROGMEM table[] = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 3, 21, 21, 18, }, + { 57600, 20, 43, 43, 41, }, + { 38400, 37, 73, 73, 70, }, + { 31250, 45, 89, 89, 88, }, + { 28800, 46, 98, 98, 95, }, + { 19200, 71, 148, 148, 145, }, + { 14400, 96, 197, 197, 194, }, + { 9600, 146, 297, 297, 294, }, + { 4800, 296, 595, 595, 592, }, + { 2400, 592, 1189, 1189, 1186, }, + { 1200, 1187, 2379, 2379, 2376, }, + { 600, 2379, 4759, 4759, 4755, }, + { 300, 4759, 9523, 9523, 9520, }, +}; + +const int XMIT_START_ADJUSTMENT = 6; + +#else + +#error This version of SoftwareSerial supports only 20, 16 and 8MHz processors + +#endif + +// +// Statics +// +SoftwareSerial *SoftwareSerial::active_object = 0; +char SoftwareSerial::_receive_buffer[_SS_MAX_RX_BUFF]; +volatile uint8_t SoftwareSerial::_receive_buffer_tail = 0; +volatile uint8_t SoftwareSerial::_receive_buffer_head = 0; + +// +// Debugging +// +// This function generates a brief pulse +// for debugging or measuring on an oscilloscope. +inline void DebugPulse(uint8_t pin, uint8_t count) +{ +#if _DEBUG + volatile uint8_t *pport = portOutputRegister(digitalPinToPort(pin)); + + uint8_t val = *pport; + while (count--) + { + *pport = val | digitalPinToBitMask(pin); + *pport = val; + } +#endif +} + +// +// Private methods +// + +/* static */ +inline void SoftwareSerial::tunedDelay(uint16_t delay) { + uint8_t tmp=0; + + asm volatile("sbiw %0, 0x01 \n\t" + "ldi %1, 0xFF \n\t" + "cpi %A0, 0xFF \n\t" + "cpc %B0, %1 \n\t" + "brne .-10 \n\t" + : "+r" (delay), "+a" (tmp) + : "0" (delay) + ); +} + +// This function sets the current object as the "listening" +// one and returns true if it replaces another +bool SoftwareSerial::listen() +{ + if (active_object != this) + { + _buffer_overflow = false; + uint8_t oldSREG = SREG; + cli(); + _receive_buffer_head = _receive_buffer_tail = 0; + active_object = this; + SREG = oldSREG; + return true; + } + + return false; +} + +// +// The receive routine called by the interrupt handler +// +void SoftwareSerial::recv() +{ + +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Preserve the registers that the compiler misses +// (courtesy of Arduino forum user *etracer*) + asm volatile( + "push r18 \n\t" + "push r19 \n\t" + "push r20 \n\t" + "push r21 \n\t" + "push r22 \n\t" + "push r23 \n\t" + "push r26 \n\t" + "push r27 \n\t" + ::); +#endif + + uint8_t d = 0; + + // If RX line is high, then we don't see any start bit + // so interrupt is probably not for us + if (_inverse_logic ? rx_pin_read() : !rx_pin_read()) + { + // Wait approximately 1/2 of a bit width to "center" the sample + tunedDelay(_rx_delay_centering); + DebugPulse(_DEBUG_PIN2, 1); + + // Read each of the 8 bits + for (uint8_t i=0x1; i; i <<= 1) + { + tunedDelay(_rx_delay_intrabit); + DebugPulse(_DEBUG_PIN2, 1); + uint8_t noti = ~i; + if (rx_pin_read()) + d |= i; + else // else clause added to ensure function timing is ~balanced + d &= noti; + } + + // skip the stop bit + tunedDelay(_rx_delay_stopbit); + DebugPulse(_DEBUG_PIN2, 1); + + if (_inverse_logic) + d = ~d; + + // if buffer full, set the overflow flag and return + if ((_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF != _receive_buffer_head) + { + // save new data in buffer: tail points to where byte goes + _receive_buffer[_receive_buffer_tail] = d; // save new byte + _receive_buffer_tail = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF; + } + else + { +#if _DEBUG // for scope: pulse pin as overflow indictator + DebugPulse(_DEBUG_PIN1, 1); +#endif + _buffer_overflow = true; + } + } + +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Restore the registers that the compiler misses + asm volatile( + "pop r27 \n\t" + "pop r26 \n\t" + "pop r23 \n\t" + "pop r22 \n\t" + "pop r21 \n\t" + "pop r20 \n\t" + "pop r19 \n\t" + "pop r18 \n\t" + ::); +#endif +} + +void SoftwareSerial::tx_pin_write(uint8_t pin_state) +{ + if (pin_state == LOW) + *_transmitPortRegister &= ~_transmitBitMask; + else + *_transmitPortRegister |= _transmitBitMask; +} + +uint8_t SoftwareSerial::rx_pin_read() +{ + return *_receivePortRegister & _receiveBitMask; +} + +// +// Interrupt handling +// + +/* static */ +inline void SoftwareSerial::handle_interrupt() +{ + if (active_object) + { + active_object->recv(); + } +} + +#if defined(PCINT0_vect) +ISR(PCINT0_vect) +{ + SoftwareSerial::handle_interrupt(); +} +#endif + +#if defined(PCINT1_vect) +ISR(PCINT1_vect) +{ + SoftwareSerial::handle_interrupt(); +} +#endif + +#if defined(PCINT2_vect) +ISR(PCINT2_vect) +{ + SoftwareSerial::handle_interrupt(); +} +#endif + +#if defined(PCINT3_vect) +ISR(PCINT3_vect) +{ + SoftwareSerial::handle_interrupt(); +} +#endif + +// +// Constructor +// +SoftwareSerial::SoftwareSerial(uint8_t receivePin, uint8_t transmitPin, bool inverse_logic /* = false */) : + _rx_delay_centering(0), + _rx_delay_intrabit(0), + _rx_delay_stopbit(0), + _tx_delay(0), + _buffer_overflow(false), + _inverse_logic(inverse_logic) +{ + setTX(transmitPin); + setRX(receivePin); +} + +// +// Destructor +// +SoftwareSerial::~SoftwareSerial() +{ + end(); +} + +void SoftwareSerial::setTX(uint8_t tx) +{ + pinMode(tx, OUTPUT); + digitalWrite(tx, HIGH); + _transmitBitMask = digitalPinToBitMask(tx); + uint8_t port = digitalPinToPort(tx); + _transmitPortRegister = portOutputRegister(port); +} + +void SoftwareSerial::setRX(uint8_t rx) +{ + pinMode(rx, INPUT); + if (!_inverse_logic) + digitalWrite(rx, HIGH); // pullup for normal logic! + _receivePin = rx; + _receiveBitMask = digitalPinToBitMask(rx); + uint8_t port = digitalPinToPort(rx); + _receivePortRegister = portInputRegister(port); +} + +// +// Public methods +// + +void SoftwareSerial::begin(long speed) +{ + _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = 0; + + for (unsigned i=0; i +#include + +/****************************************************************************** +* Definitions +******************************************************************************/ + +#define _SS_MAX_RX_BUFF 64 // RX buffer size +#ifndef GCC_VERSION +#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) +#endif + +class SoftwareSerial : public Stream +{ +private: + // per object data + uint8_t _receivePin; + uint8_t _receiveBitMask; + volatile uint8_t *_receivePortRegister; + uint8_t _transmitBitMask; + volatile uint8_t *_transmitPortRegister; + + uint16_t _rx_delay_centering; + uint16_t _rx_delay_intrabit; + uint16_t _rx_delay_stopbit; + uint16_t _tx_delay; + + uint16_t _buffer_overflow:1; + uint16_t _inverse_logic:1; + + // static data + static char _receive_buffer[_SS_MAX_RX_BUFF]; + static volatile uint8_t _receive_buffer_tail; + static volatile uint8_t _receive_buffer_head; + static SoftwareSerial *active_object; + + // private methods + void recv(); + uint8_t rx_pin_read(); + void tx_pin_write(uint8_t pin_state); + void setTX(uint8_t transmitPin); + void setRX(uint8_t receivePin); + + // private static method for timing + static inline void tunedDelay(uint16_t delay); + +public: + // public methods + SoftwareSerial(uint8_t receivePin, uint8_t transmitPin, bool inverse_logic = false); + ~SoftwareSerial(); + void begin(long speed); + bool listen(); + void end(); + bool isListening() { return this == active_object; } + bool overflow() { bool ret = _buffer_overflow; _buffer_overflow = false; return ret; } + int peek(); + + virtual size_t write(uint8_t byte); + virtual int read(); + virtual int available(); + virtual void flush(); + + using Print::write; + + // public only for easy access by interrupt handlers + static inline void handle_interrupt(); +}; + +// Arduino 0012 workaround +#undef int +#undef char +#undef long +#undef byte +#undef float +#undef abs +#undef round + +#endif diff --git a/libs/arduino-1.0/libraries/SoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino b/libs/arduino-1.0/libraries/SoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino new file mode 100644 index 0000000..6101bb1 --- /dev/null +++ b/libs/arduino-1.0/libraries/SoftwareSerial/examples/SoftwareSerialExample/SoftwareSerialExample.ino @@ -0,0 +1,55 @@ +/* + Software serial multple serial test + + Receives from the hardware serial, sends to software serial. + Receives from software serial, sends to hardware serial. + + The circuit: + * RX is digital pin 10 (connect to TX of other device) + * TX is digital pin 11 (connect to RX of other device) + + Note: + Not all pins on the Mega and Mega 2560 support change interrupts, + so only the following can be used for RX: + 10, 11, 12, 13, 50, 51, 52, 53, 62, 63, 64, 65, 66, 67, 68, 69 + + Not all pins on the Leonardo support change interrupts, + so only the following can be used for RX: + 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI). + + created back in the mists of time + modified 25 May 2012 + by Tom Igoe + based on Mikal Hart's example + + This example code is in the public domain. + + */ +#include + +SoftwareSerial mySerial(10, 11); // RX, TX + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(57600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + Serial.println("Goodnight moon!"); + + // set the data rate for the SoftwareSerial port + mySerial.begin(4800); + mySerial.println("Hello, world?"); +} + +void loop() // run over and over +{ + if (mySerial.available()) + Serial.write(mySerial.read()); + if (Serial.available()) + mySerial.write(Serial.read()); +} + diff --git a/libs/arduino-1.0/libraries/SoftwareSerial/examples/TwoPortReceive/TwoPortReceive.ino b/libs/arduino-1.0/libraries/SoftwareSerial/examples/TwoPortReceive/TwoPortReceive.ino new file mode 100644 index 0000000..d607ee6 --- /dev/null +++ b/libs/arduino-1.0/libraries/SoftwareSerial/examples/TwoPortReceive/TwoPortReceive.ino @@ -0,0 +1,93 @@ +/* + Software serial multple serial test + + Receives from the two software serial ports, + sends to the hardware serial port. + + In order to listen on a software port, you call port.listen(). + When using two software serial ports, you have to switch ports + by listen()ing on each one in turn. Pick a logical time to switch + ports, like the end of an expected transmission, or when the + buffer is empty. This example switches ports when there is nothing + more to read from a port + + The circuit: + Two devices which communicate serially are needed. + * First serial device's TX attached to digital pin 2, RX to pin 3 + * Second serial device's TX attached to digital pin 4, RX to pin 5 + + Note: + Not all pins on the Mega and Mega 2560 support change interrupts, + so only the following can be used for RX: + 10, 11, 12, 13, 50, 51, 52, 53, 62, 63, 64, 65, 66, 67, 68, 69 + + Not all pins on the Leonardo support change interrupts, + so only the following can be used for RX: + 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI). + + created 18 Apr. 2011 + modified 25 May 2012 + by Tom Igoe + based on Mikal Hart's twoPortRXExample + + This example code is in the public domain. + + */ + +#include +// software serial #1: TX = digital pin 10, RX = digital pin 11 +SoftwareSerial portOne(10,11); + +// software serial #2: TX = digital pin 8, RX = digital pin 9 +// on the Mega, use other pins instead, since 8 and 9 don't work on the Mega +SoftwareSerial portTwo(8,9); + +void setup() +{ + // Open serial communications and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + + // Start each software serial port + portOne.begin(9600); + portTwo.begin(9600); +} + +void loop() +{ + // By default, the last intialized port is listening. + // when you want to listen on a port, explicitly select it: + portOne.listen(); + Serial.println("Data from port one:"); + // while there is data coming in, read it + // and send to the hardware serial port: + while (portOne.available() > 0) { + char inByte = portOne.read(); + Serial.write(inByte); + } + + // blank line to separate data from the two ports: + Serial.println(); + + // Now listen on the second port + portTwo.listen(); + // while there is data coming in, read it + // and send to the hardware serial port: + Serial.println("Data from port two:"); + while (portTwo.available() > 0) { + char inByte = portTwo.read(); + Serial.write(inByte); + } + + // blank line to separate data from the two ports: + Serial.println(); +} + + + + + + diff --git a/libs/arduino-1.0/libraries/SoftwareSerial/keywords.txt b/libs/arduino-1.0/libraries/SoftwareSerial/keywords.txt new file mode 100644 index 0000000..90d4c15 --- /dev/null +++ b/libs/arduino-1.0/libraries/SoftwareSerial/keywords.txt @@ -0,0 +1,27 @@ +####################################### +# Syntax Coloring Map for NewSoftSerial +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +NewSoftSerial KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +end KEYWORD2 +read KEYWORD2 +available KEYWORD2 +isListening KEYWORD2 +overflow KEYWORD2 +flush KEYWORD2 +listen KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/Stepper/Stepper.cpp b/libs/arduino-1.0/libraries/Stepper/Stepper.cpp new file mode 100644 index 0000000..5d6b5e5 --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/Stepper.cpp @@ -0,0 +1,220 @@ +/* + Stepper.cpp - - Stepper library for Wiring/Arduino - Version 0.4 + + Original library (0.1) by Tom Igoe. + Two-wire modifications (0.2) by Sebastian Gassner + Combination version (0.3) by Tom Igoe and David Mellis + Bug fix for four-wire (0.4) by Tom Igoe, bug fix from Noah Shibley + + Drives a unipolar or bipolar stepper motor using 2 wires or 4 wires + + When wiring multiple stepper motors to a microcontroller, + you quickly run out of output pins, with each motor requiring 4 connections. + + By making use of the fact that at any time two of the four motor + coils are the inverse of the other two, the number of + control connections can be reduced from 4 to 2. + + A slightly modified circuit around a Darlington transistor array or an L293 H-bridge + connects to only 2 microcontroler pins, inverts the signals received, + and delivers the 4 (2 plus 2 inverted ones) output signals required + for driving a stepper motor. + + The sequence of control signals for 4 control wires is as follows: + + Step C0 C1 C2 C3 + 1 1 0 1 0 + 2 0 1 1 0 + 3 0 1 0 1 + 4 1 0 0 1 + + The sequence of controls signals for 2 control wires is as follows + (columns C1 and C2 from above): + + Step C0 C1 + 1 0 1 + 2 1 1 + 3 1 0 + 4 0 0 + + The circuits can be found at + +http://www.arduino.cc/en/Tutorial/Stepper + + + */ + + +#include "Arduino.h" +#include "Stepper.h" + +/* + * two-wire constructor. + * Sets which wires should control the motor. + */ +Stepper::Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2) +{ + this->step_number = 0; // which step the motor is on + this->speed = 0; // the motor speed, in revolutions per minute + this->direction = 0; // motor direction + this->last_step_time = 0; // time stamp in ms of the last step taken + this->number_of_steps = number_of_steps; // total number of steps for this motor + + // Arduino pins for the motor control connection: + this->motor_pin_1 = motor_pin_1; + this->motor_pin_2 = motor_pin_2; + + // setup the pins on the microcontroller: + pinMode(this->motor_pin_1, OUTPUT); + pinMode(this->motor_pin_2, OUTPUT); + + // When there are only 2 pins, set the other two to 0: + this->motor_pin_3 = 0; + this->motor_pin_4 = 0; + + // pin_count is used by the stepMotor() method: + this->pin_count = 2; +} + + +/* + * constructor for four-pin version + * Sets which wires should control the motor. + */ + +Stepper::Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2, int motor_pin_3, int motor_pin_4) +{ + this->step_number = 0; // which step the motor is on + this->speed = 0; // the motor speed, in revolutions per minute + this->direction = 0; // motor direction + this->last_step_time = 0; // time stamp in ms of the last step taken + this->number_of_steps = number_of_steps; // total number of steps for this motor + + // Arduino pins for the motor control connection: + this->motor_pin_1 = motor_pin_1; + this->motor_pin_2 = motor_pin_2; + this->motor_pin_3 = motor_pin_3; + this->motor_pin_4 = motor_pin_4; + + // setup the pins on the microcontroller: + pinMode(this->motor_pin_1, OUTPUT); + pinMode(this->motor_pin_2, OUTPUT); + pinMode(this->motor_pin_3, OUTPUT); + pinMode(this->motor_pin_4, OUTPUT); + + // pin_count is used by the stepMotor() method: + this->pin_count = 4; +} + +/* + Sets the speed in revs per minute + +*/ +void Stepper::setSpeed(long whatSpeed) +{ + this->step_delay = 60L * 1000L / this->number_of_steps / whatSpeed; +} + +/* + Moves the motor steps_to_move steps. If the number is negative, + the motor moves in the reverse direction. + */ +void Stepper::step(int steps_to_move) +{ + int steps_left = abs(steps_to_move); // how many steps to take + + // determine direction based on whether steps_to_mode is + or -: + if (steps_to_move > 0) {this->direction = 1;} + if (steps_to_move < 0) {this->direction = 0;} + + + // decrement the number of steps, moving one step each time: + while(steps_left > 0) { + // move only if the appropriate delay has passed: + if (millis() - this->last_step_time >= this->step_delay) { + // get the timeStamp of when you stepped: + this->last_step_time = millis(); + // increment or decrement the step number, + // depending on direction: + if (this->direction == 1) { + this->step_number++; + if (this->step_number == this->number_of_steps) { + this->step_number = 0; + } + } + else { + if (this->step_number == 0) { + this->step_number = this->number_of_steps; + } + this->step_number--; + } + // decrement the steps left: + steps_left--; + // step the motor to step number 0, 1, 2, or 3: + stepMotor(this->step_number % 4); + } + } +} + +/* + * Moves the motor forward or backwards. + */ +void Stepper::stepMotor(int thisStep) +{ + if (this->pin_count == 2) { + switch (thisStep) { + case 0: /* 01 */ + digitalWrite(motor_pin_1, LOW); + digitalWrite(motor_pin_2, HIGH); + break; + case 1: /* 11 */ + digitalWrite(motor_pin_1, HIGH); + digitalWrite(motor_pin_2, HIGH); + break; + case 2: /* 10 */ + digitalWrite(motor_pin_1, HIGH); + digitalWrite(motor_pin_2, LOW); + break; + case 3: /* 00 */ + digitalWrite(motor_pin_1, LOW); + digitalWrite(motor_pin_2, LOW); + break; + } + } + if (this->pin_count == 4) { + switch (thisStep) { + case 0: // 1010 + digitalWrite(motor_pin_1, HIGH); + digitalWrite(motor_pin_2, LOW); + digitalWrite(motor_pin_3, HIGH); + digitalWrite(motor_pin_4, LOW); + break; + case 1: // 0110 + digitalWrite(motor_pin_1, LOW); + digitalWrite(motor_pin_2, HIGH); + digitalWrite(motor_pin_3, HIGH); + digitalWrite(motor_pin_4, LOW); + break; + case 2: //0101 + digitalWrite(motor_pin_1, LOW); + digitalWrite(motor_pin_2, HIGH); + digitalWrite(motor_pin_3, LOW); + digitalWrite(motor_pin_4, HIGH); + break; + case 3: //1001 + digitalWrite(motor_pin_1, HIGH); + digitalWrite(motor_pin_2, LOW); + digitalWrite(motor_pin_3, LOW); + digitalWrite(motor_pin_4, HIGH); + break; + } + } +} + +/* + version() returns the version of the library: +*/ +int Stepper::version(void) +{ + return 4; +} diff --git a/libs/arduino-1.0/libraries/Stepper/Stepper.h b/libs/arduino-1.0/libraries/Stepper/Stepper.h new file mode 100644 index 0000000..4094aee --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/Stepper.h @@ -0,0 +1,83 @@ +/* + Stepper.h - - Stepper library for Wiring/Arduino - Version 0.4 + + Original library (0.1) by Tom Igoe. + Two-wire modifications (0.2) by Sebastian Gassner + Combination version (0.3) by Tom Igoe and David Mellis + Bug fix for four-wire (0.4) by Tom Igoe, bug fix from Noah Shibley + + Drives a unipolar or bipolar stepper motor using 2 wires or 4 wires + + When wiring multiple stepper motors to a microcontroller, + you quickly run out of output pins, with each motor requiring 4 connections. + + By making use of the fact that at any time two of the four motor + coils are the inverse of the other two, the number of + control connections can be reduced from 4 to 2. + + A slightly modified circuit around a Darlington transistor array or an L293 H-bridge + connects to only 2 microcontroler pins, inverts the signals received, + and delivers the 4 (2 plus 2 inverted ones) output signals required + for driving a stepper motor. + + The sequence of control signals for 4 control wires is as follows: + + Step C0 C1 C2 C3 + 1 1 0 1 0 + 2 0 1 1 0 + 3 0 1 0 1 + 4 1 0 0 1 + + The sequence of controls signals for 2 control wires is as follows + (columns C1 and C2 from above): + + Step C0 C1 + 1 0 1 + 2 1 1 + 3 1 0 + 4 0 0 + + The circuits can be found at + http://www.arduino.cc/en/Tutorial/Stepper +*/ + +// ensure this library description is only included once +#ifndef Stepper_h +#define Stepper_h + +// library interface description +class Stepper { + public: + // constructors: + Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2); + Stepper(int number_of_steps, int motor_pin_1, int motor_pin_2, int motor_pin_3, int motor_pin_4); + + // speed setter method: + void setSpeed(long whatSpeed); + + // mover method: + void step(int number_of_steps); + + int version(void); + + private: + void stepMotor(int this_step); + + int direction; // Direction of rotation + int speed; // Speed in RPMs + unsigned long step_delay; // delay between steps, in ms, based on speed + int number_of_steps; // total number of steps this motor can take + int pin_count; // whether you're driving the motor with 2 or 4 pins + int step_number; // which step the motor is on + + // motor pin numbers: + int motor_pin_1; + int motor_pin_2; + int motor_pin_3; + int motor_pin_4; + + long last_step_time; // time stamp in ms of when the last step was taken +}; + +#endif + diff --git a/libs/arduino-1.0/libraries/Stepper/examples/MotorKnob/MotorKnob.ino b/libs/arduino-1.0/libraries/Stepper/examples/MotorKnob/MotorKnob.ino new file mode 100644 index 0000000..d428186 --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/examples/MotorKnob/MotorKnob.ino @@ -0,0 +1,41 @@ +/* + * MotorKnob + * + * A stepper motor follows the turns of a potentiometer + * (or other sensor) on analog input 0. + * + * http://www.arduino.cc/en/Reference/Stepper + * This example code is in the public domain. + */ + +#include + +// change this to the number of steps on your motor +#define STEPS 100 + +// create an instance of the stepper class, specifying +// the number of steps of the motor and the pins it's +// attached to +Stepper stepper(STEPS, 8, 9, 10, 11); + +// the previous reading from the analog input +int previous = 0; + +void setup() +{ + // set the speed of the motor to 30 RPMs + stepper.setSpeed(30); +} + +void loop() +{ + // get the sensor value + int val = analogRead(0); + + // move a number of steps equal to the change in the + // sensor reading + stepper.step(val - previous); + + // remember the previous value of the sensor + previous = val; +} \ No newline at end of file diff --git a/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneRevolution/stepper_oneRevolution.ino b/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneRevolution/stepper_oneRevolution.ino new file mode 100644 index 0000000..2dbb57d --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneRevolution/stepper_oneRevolution.ino @@ -0,0 +1,44 @@ + +/* + Stepper Motor Control - one revolution + + This program drives a unipolar or bipolar stepper motor. + The motor is attached to digital pins 8 - 11 of the Arduino. + + The motor should revolve one revolution in one direction, then + one revolution in the other direction. + + + Created 11 Mar. 2007 + Modified 30 Nov. 2009 + by Tom Igoe + + */ + +#include + +const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution + // for your motor + +// initialize the stepper library on pins 8 through 11: +Stepper myStepper(stepsPerRevolution, 8,9,10,11); + +void setup() { + // set the speed at 60 rpm: + myStepper.setSpeed(60); + // initialize the serial port: + Serial.begin(9600); +} + +void loop() { + // step one revolution in one direction: + Serial.println("clockwise"); + myStepper.step(stepsPerRevolution); + delay(500); + + // step one revolution in the other direction: + Serial.println("counterclockwise"); + myStepper.step(-stepsPerRevolution); + delay(500); +} + diff --git a/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneStepAtATime/stepper_oneStepAtATime.ino b/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneStepAtATime/stepper_oneStepAtATime.ino new file mode 100644 index 0000000..36d3299 --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/examples/stepper_oneStepAtATime/stepper_oneStepAtATime.ino @@ -0,0 +1,44 @@ + +/* + Stepper Motor Control - one step at a time + + This program drives a unipolar or bipolar stepper motor. + The motor is attached to digital pins 8 - 11 of the Arduino. + + The motor will step one step at a time, very slowly. You can use this to + test that you've got the four wires of your stepper wired to the correct + pins. If wired correctly, all steps should be in the same direction. + + Use this also to count the number of steps per revolution of your motor, + if you don't know it. Then plug that number into the oneRevolution + example to see if you got it right. + + Created 30 Nov. 2009 + by Tom Igoe + + */ + +#include + +const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution + // for your motor + +// initialize the stepper library on pins 8 through 11: +Stepper myStepper(stepsPerRevolution, 8,9,10,11); + +int stepCount = 0; // number of steps the motor has taken + +void setup() { + // initialize the serial port: + Serial.begin(9600); +} + +void loop() { + // step one step: + myStepper.step(1); + Serial.print("steps:" ); + Serial.println(stepCount); + stepCount++; + delay(500); +} + diff --git a/libs/arduino-1.0/libraries/Stepper/examples/stepper_speedControl/stepper_speedControl.ino b/libs/arduino-1.0/libraries/Stepper/examples/stepper_speedControl/stepper_speedControl.ino new file mode 100644 index 0000000..1a67a55 --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/examples/stepper_speedControl/stepper_speedControl.ino @@ -0,0 +1,48 @@ + +/* + Stepper Motor Control - speed control + + This program drives a unipolar or bipolar stepper motor. + The motor is attached to digital pins 8 - 11 of the Arduino. + A potentiometer is connected to analog input 0. + + The motor will rotate in a clockwise direction. The higher the potentiometer value, + the faster the motor speed. Because setSpeed() sets the delay between steps, + you may notice the motor is less responsive to changes in the sensor value at + low speeds. + + Created 30 Nov. 2009 + Modified 28 Oct 2010 + by Tom Igoe + + */ + +#include + +const int stepsPerRevolution = 200; // change this to fit the number of steps per revolution +// for your motor + + +// initialize the stepper library on pins 8 through 11: +Stepper myStepper(stepsPerRevolution, 8,9,10,11); + +int stepCount = 0; // number of steps the motor has taken + +void setup() { + // nothing to do inside the setup +} + +void loop() { + // read the sensor value: + int sensorReading = analogRead(A0); + // map it to a range from 0 to 100: + int motorSpeed = map(sensorReading, 0, 1023, 0, 100); + // set the motor speed: + if (motorSpeed > 0) { + myStepper.setSpeed(motorSpeed); + // step 1/100 of a revolution: + myStepper.step(stepsPerRevolution/100); + } +} + + diff --git a/libs/arduino-1.0/libraries/Stepper/keywords.txt b/libs/arduino-1.0/libraries/Stepper/keywords.txt new file mode 100644 index 0000000..19a0fad --- /dev/null +++ b/libs/arduino-1.0/libraries/Stepper/keywords.txt @@ -0,0 +1,28 @@ +####################################### +# Syntax Coloring Map For Test +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +Stepper KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +step KEYWORD2 +setSpeed KEYWORD2 +version KEYWORD2 + +###################################### +# Instances (KEYWORD2) +####################################### +direction KEYWORD2 +speed KEYWORD2 + + +####################################### +# Constants (LITERAL1) +####################################### diff --git a/libs/arduino-1.0/libraries/WiFi/WiFi.cpp b/libs/arduino-1.0/libraries/WiFi/WiFi.cpp new file mode 100644 index 0000000..c0cb001 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFi.cpp @@ -0,0 +1,199 @@ +#include "wifi_drv.h" +#include "WiFi.h" + +extern "C" { + #include "utility/wl_definitions.h" + #include "utility/wl_types.h" + #include "debug.h" +} + +// XXX: don't make assumptions about the value of MAX_SOCK_NUM. +int16_t WiFiClass::_state[MAX_SOCK_NUM] = { 0, 0, 0, 0 }; +uint16_t WiFiClass::_server_port[MAX_SOCK_NUM] = { 0, 0, 0, 0 }; + +WiFiClass::WiFiClass() +{ + // Driver initialization + init(); +} + +void WiFiClass::init() +{ + WiFiDrv::wifiDriverInit(); +} + +uint8_t WiFiClass::getSocket() +{ + for (uint8_t i = 0; i < MAX_SOCK_NUM; ++i) + { + if (WiFiClass::_server_port[i] == 0) + { + return i; + } + } + return NO_SOCKET_AVAIL; +} + +char* WiFiClass::firmwareVersion() +{ + return WiFiDrv::getFwVersion(); +} + +int WiFiClass::begin(char* ssid) +{ + uint8_t status = WL_IDLE_STATUS; + uint8_t attempts = WL_MAX_ATTEMPT_CONNECTION; + + if (WiFiDrv::wifiSetNetwork(ssid, strlen(ssid)) != WL_FAILURE) + { + do + { + delay(WL_DELAY_START_CONNECTION); + status = WiFiDrv::getConnectionStatus(); + } + while ((( status == WL_IDLE_STATUS)||(status == WL_SCAN_COMPLETED))&&(--attempts>0)); + }else + { + status = WL_CONNECT_FAILED; + } + return status; +} + +int WiFiClass::begin(char* ssid, uint8_t key_idx, const char *key) +{ + uint8_t status = WL_IDLE_STATUS; + uint8_t attempts = WL_MAX_ATTEMPT_CONNECTION; + + // set encryption key + if (WiFiDrv::wifiSetKey(ssid, strlen(ssid), key_idx, key, strlen(key)) != WL_FAILURE) + { + do + { + delay(WL_DELAY_START_CONNECTION); + status = WiFiDrv::getConnectionStatus(); + } + while ((( status == WL_IDLE_STATUS)||(status == WL_SCAN_COMPLETED))&&(--attempts>0)); + }else{ + status = WL_CONNECT_FAILED; + } + return status; +} + +int WiFiClass::begin(char* ssid, const char *passphrase) +{ + uint8_t status = WL_IDLE_STATUS; + uint8_t attempts = WL_MAX_ATTEMPT_CONNECTION; + + // set passphrase + if (WiFiDrv::wifiSetPassphrase(ssid, strlen(ssid), passphrase, strlen(passphrase))!= WL_FAILURE) + { + do + { + delay(WL_DELAY_START_CONNECTION); + status = WiFiDrv::getConnectionStatus(); + } + while ((( status == WL_IDLE_STATUS)||(status == WL_SCAN_COMPLETED))&&(--attempts>0)); + }else{ + status = WL_CONNECT_FAILED; + } + return status; +} + +int WiFiClass::disconnect() +{ + return WiFiDrv::disconnect(); +} + +uint8_t* WiFiClass::macAddress(uint8_t* mac) +{ + uint8_t* _mac = WiFiDrv::getMacAddress(); + memcpy(mac, _mac, WL_MAC_ADDR_LENGTH); + return mac; +} + +IPAddress WiFiClass::localIP() +{ + IPAddress ret; + WiFiDrv::getIpAddress(ret); + return ret; +} + +IPAddress WiFiClass::subnetMask() +{ + IPAddress ret; + WiFiDrv::getSubnetMask(ret); + return ret; +} + +IPAddress WiFiClass::gatewayIP() +{ + IPAddress ret; + WiFiDrv::getGatewayIP(ret); + return ret; +} + +char* WiFiClass::SSID() +{ + return WiFiDrv::getCurrentSSID(); +} + +uint8_t* WiFiClass::BSSID(uint8_t* bssid) +{ + uint8_t* _bssid = WiFiDrv::getCurrentBSSID(); + memcpy(bssid, _bssid, WL_MAC_ADDR_LENGTH); + return bssid; +} + +int32_t WiFiClass::RSSI() +{ + return WiFiDrv::getCurrentRSSI(); +} + +uint8_t WiFiClass::encryptionType() +{ + return WiFiDrv::getCurrentEncryptionType(); +} + + +int8_t WiFiClass::scanNetworks() +{ + uint8_t attempts = 10; + uint8_t numOfNetworks = 0; + + if (WiFiDrv::startScanNetworks() == WL_FAILURE) + return WL_FAILURE; + do + { + delay(2000); + numOfNetworks = WiFiDrv::getScanNetworks(); + } + while (( numOfNetworks == 0)&&(--attempts>0)); + return numOfNetworks; +} + +char* WiFiClass::SSID(uint8_t networkItem) +{ + return WiFiDrv::getSSIDNetoworks(networkItem); +} + +int32_t WiFiClass::RSSI(uint8_t networkItem) +{ + return WiFiDrv::getRSSINetoworks(networkItem); +} + +uint8_t WiFiClass::encryptionType(uint8_t networkItem) +{ + return WiFiDrv::getEncTypeNetowrks(networkItem); +} + +uint8_t WiFiClass::status() +{ + return WiFiDrv::getConnectionStatus(); +} + +int WiFiClass::hostByName(const char* aHostname, IPAddress& aResult) +{ + return WiFiDrv::getHostByName(aHostname, aResult); +} + +WiFiClass WiFi; diff --git a/libs/arduino-1.0/libraries/WiFi/WiFi.h b/libs/arduino-1.0/libraries/WiFi/WiFi.h new file mode 100644 index 0000000..9a86701 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFi.h @@ -0,0 +1,183 @@ +#ifndef WiFi_h +#define WiFi_h + +#include + +extern "C" { + #include "utility/wl_definitions.h" + #include "utility/wl_types.h" +} + +#include "IPAddress.h" +#include "WiFiClient.h" +#include "WiFiServer.h" + +class WiFiClass +{ +private: + + static void init(); +public: + static int16_t _state[MAX_SOCK_NUM]; + static uint16_t _server_port[MAX_SOCK_NUM]; + + WiFiClass(); + + /* + * Get the first socket available + */ + static uint8_t getSocket(); + + /* + * Get firmware version + */ + static char* firmwareVersion(); + + + /* Start Wifi connection for OPEN networks + * + * param ssid: Pointer to the SSID string. + */ + int begin(char* ssid); + + /* Start Wifi connection with WEP encryption. + * Configure a key into the device. The key type (WEP-40, WEP-104) + * is determined by the size of the key (5 bytes for WEP-40, 13 bytes for WEP-104). + * + * param ssid: Pointer to the SSID string. + * param key_idx: The key index to set. Valid values are 0-3. + * param key: Key input buffer. + */ + int begin(char* ssid, uint8_t key_idx, const char* key); + + /* Start Wifi connection with passphrase + * the most secure supported mode will be automatically selected + * + * param ssid: Pointer to the SSID string. + * param passphrase: Passphrase. Valid characters in a passphrase + * must be between ASCII 32-126 (decimal). + */ + int begin(char* ssid, const char *passphrase); + + /* + * Disconnect from the network + * + * return: one value of wl_status_t enum + */ + int disconnect(void); + + /* + * Get the interface MAC address. + * + * return: pointer to uint8_t array with length WL_MAC_ADDR_LENGTH + */ + uint8_t* macAddress(uint8_t* mac); + + /* + * Get the interface IP address. + * + * return: Ip address value + */ + IPAddress localIP(); + + /* + * Get the interface subnet mask address. + * + * return: subnet mask address value + */ + IPAddress subnetMask(); + + /* + * Get the gateway ip address. + * + * return: gateway ip address value + */ + IPAddress gatewayIP(); + + /* + * Return the current SSID associated with the network + * + * return: ssid string + */ + char* SSID(); + + /* + * Return the current BSSID associated with the network. + * It is the MAC address of the Access Point + * + * return: pointer to uint8_t array with length WL_MAC_ADDR_LENGTH + */ + uint8_t* BSSID(uint8_t* bssid); + + /* + * Return the current RSSI /Received Signal Strength in dBm) + * associated with the network + * + * return: signed value + */ + int32_t RSSI(); + + /* + * Return the Encryption Type associated with the network + * + * return: one value of wl_enc_type enum + */ + uint8_t encryptionType(); + + /* + * Start scan WiFi networks available + * + * return: Number of discovered networks + */ + int8_t scanNetworks(); + + /* + * Return the SSID discovered during the network scan. + * + * param networkItem: specify from which network item want to get the information + * + * return: ssid string of the specified item on the networks scanned list + */ + char* SSID(uint8_t networkItem); + + /* + * Return the encryption type of the networks discovered during the scanNetworks + * + * param networkItem: specify from which network item want to get the information + * + * return: encryption type (enum wl_enc_type) of the specified item on the networks scanned list + */ + uint8_t encryptionType(uint8_t networkItem); + + /* + * Return the RSSI of the networks discovered during the scanNetworks + * + * param networkItem: specify from which network item want to get the information + * + * return: signed value of RSSI of the specified item on the networks scanned list + */ + int32_t RSSI(uint8_t networkItem); + + /* + * Return Connection status. + * + * return: one of the value defined in wl_status_t + */ + uint8_t status(); + + /* + * Resolve the given hostname to an IP address. + * param aHostname: Name to be resolved + * param aResult: IPAddress structure to store the returned IP address + * result: 1 if aIPAddrString was successfully converted to an IP address, + * else error code + */ + int hostByName(const char* aHostname, IPAddress& aResult); + + friend class WiFiClient; + friend class WiFiServer; +}; + +extern WiFiClass WiFi; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/WiFiClient.cpp b/libs/arduino-1.0/libraries/WiFi/WiFiClient.cpp new file mode 100644 index 0000000..83c0d10 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFiClient.cpp @@ -0,0 +1,179 @@ +extern "C" { + #include "utility/wl_definitions.h" + #include "utility/wl_types.h" + #include "socket.h" + #include "string.h" + #include "utility/debug.h" +} + +#include "WiFi.h" +#include "WiFiClient.h" +#include "WiFiServer.h" +#include "server_drv.h" + + +uint16_t WiFiClient::_srcport = 1024; + +WiFiClient::WiFiClient() : _sock(MAX_SOCK_NUM) { +} + +WiFiClient::WiFiClient(uint8_t sock) : _sock(sock) { +} + +int WiFiClient::connect(const char* host, uint16_t port) { + IPAddress remote_addr; + if (WiFi.hostByName(host, remote_addr)) + { + return connect(remote_addr, port); + } + return 0; +} + +int WiFiClient::connect(IPAddress ip, uint16_t port) { + _sock = getFirstSocket(); + if (_sock != NO_SOCKET_AVAIL) + { + ServerDrv::startClient(uint32_t(ip), port, _sock); + WiFiClass::_state[_sock] = _sock; + + unsigned long start = millis(); + + // wait 4 second for the connection to close + while (!connected() && millis() - start < 10000) + delay(1); + + if (!connected()) + { + return 0; + } + }else{ + Serial.println("No Socket available"); + return 0; + } + return 1; +} + +size_t WiFiClient::write(uint8_t b) { + return write(&b, 1); +} + +size_t WiFiClient::write(const uint8_t *buf, size_t size) { + if (_sock >= MAX_SOCK_NUM) + { + setWriteError(); + return 0; + } + if (size==0) + { + setWriteError(); + return 0; + } + + + if (!ServerDrv::sendData(_sock, buf, size)) + { + setWriteError(); + return 0; + } + if (!ServerDrv::checkDataSent(_sock)) + { + setWriteError(); + return 0; + } + + return size; +} + +int WiFiClient::available() { + if (_sock != 255) + { + return ServerDrv::availData(_sock); + } + + return 0; +} + +int WiFiClient::read() { + uint8_t b; + if (!available()) + return -1; + + ServerDrv::getData(_sock, &b); + return b; +} + + +int WiFiClient::read(uint8_t* buf, size_t size) { + if (!ServerDrv::getDataBuf(_sock, buf, &size)) + return -1; + return 0; +} + +int WiFiClient::peek() { + uint8_t b; + if (!available()) + return -1; + + ServerDrv::getData(_sock, &b, 1); + return b; +} + +void WiFiClient::flush() { + while (available()) + read(); +} + +void WiFiClient::stop() { + + if (_sock == 255) + return; + + ServerDrv::stopClient(_sock); + + unsigned long start = millis(); + + + // wait a second for the connection to close + while (status() != CLOSED && millis() - start < 1000) + delay(1); + _sock = 255; +} + +uint8_t WiFiClient::connected() { + + if (_sock == 255) { + return 0; + } else { + uint8_t s = status(); + + return !(s == LISTEN || s == CLOSED || s == FIN_WAIT_1 || + s == FIN_WAIT_2 || s == TIME_WAIT || + s == SYN_SENT || s== SYN_RCVD || + (s == CLOSE_WAIT && !available())); + } +} + +uint8_t WiFiClient::status() { + if (_sock == 255) { + return CLOSED; + } else { + return ServerDrv::getClientState(_sock); + } +} + +WiFiClient::operator bool() { + return _sock != 255; +} + +// Private Methods +uint8_t WiFiClient::getFirstSocket() +{ + for (int i = 0; i < MAX_SOCK_NUM; i++) { + if (WiFiClass::_state[i] == 0) + { + return i; + } + } + return SOCK_NOT_AVAIL; +} + diff --git a/libs/arduino-1.0/libraries/WiFi/WiFiClient.h b/libs/arduino-1.0/libraries/WiFi/WiFiClient.h new file mode 100644 index 0000000..5a7f0f3 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFiClient.h @@ -0,0 +1,40 @@ +#ifndef wificlient_h +#define wificlient_h +#include "Arduino.h" +#include "Print.h" +#include "Client.h" +#include "IPAddress.h" + +class WiFiClient : public Client { + +public: + WiFiClient(); + WiFiClient(uint8_t sock); + + uint8_t status(); + virtual int connect(IPAddress ip, uint16_t port); + virtual int connect(const char *host, uint16_t port); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *buf, size_t size); + virtual int available(); + virtual int read(); + virtual int read(uint8_t *buf, size_t size); + virtual int peek(); + virtual void flush(); + virtual void stop(); + virtual uint8_t connected(); + virtual operator bool(); + + friend class WiFiServer; + + using Print::write; + +private: + static uint16_t _srcport; + uint8_t _sock; //not used + uint16_t _socket; + + uint8_t getFirstSocket(); +}; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/WiFiServer.cpp b/libs/arduino-1.0/libraries/WiFi/WiFiServer.cpp new file mode 100644 index 0000000..77dbac0 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFiServer.cpp @@ -0,0 +1,88 @@ +#include +#include "server_drv.h" + +extern "C" { + #include "utility/debug.h" +} + +#include "WiFi.h" +#include "WiFiClient.h" +#include "WiFiServer.h" + +WiFiServer::WiFiServer(uint16_t port) +{ + _port = port; +} + +void WiFiServer::begin() +{ + uint8_t _sock = WiFiClass::getSocket(); + if (_sock != NO_SOCKET_AVAIL) + { + ServerDrv::startServer(_port, _sock); + WiFiClass::_server_port[_sock] = _port; + } +} + +WiFiClient WiFiServer::available(byte* status) +{ + static int cycle_server_down = 0; + const int TH_SERVER_DOWN = 50; + + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) + { + if (WiFiClass::_server_port[sock] == _port) + { + WiFiClient client(sock); + uint8_t _status = client.status(); + uint8_t _ser_status = this->status(); + + if (status != NULL) + *status = _status; + + //server not in listen state, restart it + if ((_ser_status == 0)&&(cycle_server_down++ > TH_SERVER_DOWN)) + { + ServerDrv::startServer(_port, sock); + cycle_server_down = 0; + } + + if (_status == ESTABLISHED) + { + return client; //TODO + } + } + } + + return WiFiClient(255); +} + +uint8_t WiFiServer::status() { + return ServerDrv::getServerState(0); +} + + +size_t WiFiServer::write(uint8_t b) +{ + return write(&b, 1); +} + +size_t WiFiServer::write(const uint8_t *buffer, size_t size) +{ + size_t n = 0; + + for (int sock = 0; sock < MAX_SOCK_NUM; sock++) + { + if (WiFiClass::_server_port[sock] != 0) + { + WiFiClient client(sock); + + if (WiFiClass::_server_port[sock] == _port && + client.status() == ESTABLISHED) + { + n+=client.write(buffer, size); + } + } + } + return n; +} diff --git a/libs/arduino-1.0/libraries/WiFi/WiFiServer.h b/libs/arduino-1.0/libraries/WiFi/WiFiServer.h new file mode 100644 index 0000000..68b574c --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/WiFiServer.h @@ -0,0 +1,27 @@ +#ifndef wifiserver_h +#define wifiserver_h + +extern "C" { + #include "utility/wl_definitions.h" +} + +#include "Server.h" + +class WiFiClient; + +class WiFiServer : public Server { +private: + uint16_t _port; + void* pcb; +public: + WiFiServer(uint16_t); + WiFiClient available(uint8_t* status = NULL); + void begin(); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *buf, size_t size); + uint8_t status(); + + using Print::write; +}; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/examples/ConnectNoEncryption/ConnectNoEncryption.ino b/libs/arduino-1.0/libraries/WiFi/examples/ConnectNoEncryption/ConnectNoEncryption.ino new file mode 100644 index 0000000..f42a7f3 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/ConnectNoEncryption/ConnectNoEncryption.ino @@ -0,0 +1,121 @@ +/* + + This example connects to an unencrypted Wifi network. + Then it prints the MAC address of the Wifi shield, + the IP address obtained, and other network details. + + Circuit: + * WiFi shield attached + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 31 May 2012 + by Tom Igoe + */ + #include + +char ssid[] = "yourNetwork"; // the name of your network +int status = WL_IDLE_STATUS; // the Wifi radio's status + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to open SSID: "); + Serial.println(ssid); + status = WiFi.begin(ssid); + + // wait 10 seconds for connection: + delay(10000); + } + + // you're connected now, so print out the data: + Serial.print("You're connected to the network"); + printCurrentNet(); + printWifiData(); +} + +void loop() { + // check the network connection once every 10 seconds: + delay(10000); + printCurrentNet(); +} + +void printWifiData() { + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + Serial.println(ip); + + // print your MAC address: + byte mac[6]; + WiFi.macAddress(mac); + Serial.print("MAC address: "); + Serial.print(mac[5],HEX); + Serial.print(":"); + Serial.print(mac[4],HEX); + Serial.print(":"); + Serial.print(mac[3],HEX); + Serial.print(":"); + Serial.print(mac[2],HEX); + Serial.print(":"); + Serial.print(mac[1],HEX); + Serial.print(":"); + Serial.println(mac[0],HEX); + + // print your subnet mask: + IPAddress subnet = WiFi.subnetMask(); + Serial.print("NetMask: "); + Serial.println(subnet); + + // print your gateway address: + IPAddress gateway = WiFi.gatewayIP(); + Serial.print("Gateway: "); + Serial.println(gateway); +} + +void printCurrentNet() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print the MAC address of the router you're attached to: + byte bssid[6]; + WiFi.BSSID(bssid); + Serial.print("BSSID: "); + Serial.print(bssid[5],HEX); + Serial.print(":"); + Serial.print(bssid[4],HEX); + Serial.print(":"); + Serial.print(bssid[3],HEX); + Serial.print(":"); + Serial.print(bssid[2],HEX); + Serial.print(":"); + Serial.print(bssid[1],HEX); + Serial.print(":"); + Serial.println(bssid[0],HEX); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.println(rssi); + + // print the encryption type: + byte encryption = WiFi.encryptionType(); + Serial.print("Encryption Type:"); + Serial.println(encryption,HEX); +} + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWEP/ConnectWithWEP.ino b/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWEP/ConnectWithWEP.ino new file mode 100644 index 0000000..19736b5 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWEP/ConnectWithWEP.ino @@ -0,0 +1,126 @@ +/* + + This example connects to a WEP-encrypted Wifi network. + Then it prints the MAC address of the Wifi shield, + the IP address obtained, and other network details. + + If you use 40-bit WEP, you need a key that is 10 characters long, + and the characters must be hexadecimal (0-9 or A-F). + e.g. for 40-bit, ABBADEAF01 will work, but ABBADEAF won't work + (too short) and ABBAISDEAF won't work (I and S are not + hexadecimal characters). + + For 128-bit, you need a string that is 26 characters long. + D0D0DEADF00DABBADEAFBEADED will work because it's 26 characters, + all in the 0-9, A-F range. + + Circuit: + * WiFi shield attached + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 31 May 2012 + by Tom Igoe + */ +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char key[] = "D0D0DEADF00DABBADEAFBEADED"; // your network key +int keyIndex = 0; // your network key Index number +int status = WL_IDLE_STATUS; // the Wifi radio's status + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to WEP network, SSID: "); + Serial.println(ssid); + status = WiFi.begin(ssid, keyIndex, key); + + // wait 10 seconds for connection: + delay(10000); + } + + // once you are connected : + Serial.print("You're connected to the network"); + printCurrentNet(); + printWifiData(); +} + +void loop() { + // check the network connection once every 10 seconds: + delay(10000); + printCurrentNet(); +} + +void printWifiData() { + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + Serial.println(ip); + + // print your MAC address: + byte mac[6]; + WiFi.macAddress(mac); + Serial.print("MAC address: "); + Serial.print(mac[5],HEX); + Serial.print(":"); + Serial.print(mac[4],HEX); + Serial.print(":"); + Serial.print(mac[3],HEX); + Serial.print(":"); + Serial.print(mac[2],HEX); + Serial.print(":"); + Serial.print(mac[1],HEX); + Serial.print(":"); + Serial.println(mac[0],HEX); +} + +void printCurrentNet() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print the MAC address of the router you're attached to: + byte bssid[6]; + WiFi.BSSID(bssid); + Serial.print("BSSID: "); + Serial.print(bssid[5],HEX); + Serial.print(":"); + Serial.print(bssid[4],HEX); + Serial.print(":"); + Serial.print(bssid[3],HEX); + Serial.print(":"); + Serial.print(bssid[2],HEX); + Serial.print(":"); + Serial.print(bssid[1],HEX); + Serial.print(":"); + Serial.println(bssid[0],HEX); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.println(rssi); + + // print the encryption type: + byte encryption = WiFi.encryptionType(); + Serial.print("Encryption Type:"); + Serial.println(encryption,HEX); + Serial.println(); +} + + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWPA/ConnectWithWPA.ino b/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWPA/ConnectWithWPA.ino new file mode 100644 index 0000000..fcc33ec --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/ConnectWithWPA/ConnectWithWPA.ino @@ -0,0 +1,116 @@ +/* + + This example connects to an unencrypted Wifi network. + Then it prints the MAC address of the Wifi shield, + the IP address obtained, and other network details. + + Circuit: + * WiFi shield attached + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 31 May 2012 + by Tom Igoe + */ + #include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password +int status = WL_IDLE_STATUS; // the Wifi radio's status + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to WPA SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + + // you're connected now, so print out the data: + Serial.print("You're connected to the network"); + printCurrentNet(); + printWifiData(); + +} + +void loop() { + // check the network connection once every 10 seconds: + delay(10000); + printCurrentNet(); +} + +void printWifiData() { + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + Serial.println(ip); + + // print your MAC address: + byte mac[6]; + WiFi.macAddress(mac); + Serial.print("MAC address: "); + Serial.print(mac[5],HEX); + Serial.print(":"); + Serial.print(mac[4],HEX); + Serial.print(":"); + Serial.print(mac[3],HEX); + Serial.print(":"); + Serial.print(mac[2],HEX); + Serial.print(":"); + Serial.print(mac[1],HEX); + Serial.print(":"); + Serial.println(mac[0],HEX); + +} + +void printCurrentNet() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print the MAC address of the router you're attached to: + byte bssid[6]; + WiFi.BSSID(bssid); + Serial.print("BSSID: "); + Serial.print(bssid[5],HEX); + Serial.print(":"); + Serial.print(bssid[4],HEX); + Serial.print(":"); + Serial.print(bssid[3],HEX); + Serial.print(":"); + Serial.print(bssid[2],HEX); + Serial.print(":"); + Serial.print(bssid[1],HEX); + Serial.print(":"); + Serial.println(bssid[0],HEX); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.println(rssi); + + // print the encryption type: + byte encryption = WiFi.encryptionType(); + Serial.print("Encryption Type:"); + Serial.println(encryption,HEX); + Serial.println(); +} + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/ScanNetworks/ScanNetworks.ino b/libs/arduino-1.0/libraries/WiFi/examples/ScanNetworks/ScanNetworks.ino new file mode 100644 index 0000000..93b3000 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/ScanNetworks/ScanNetworks.ino @@ -0,0 +1,119 @@ +/* + + This example prints the Wifi shield's MAC address, and + scans for available Wifi networks using the Wifi shield. + Every ten seconds, it scans again. It doesn't actually + connect to any network, so no encryption scheme is specified. + + Circuit: + * WiFi shield attached + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 21 Junn 2012 + by Tom Igoe and Jaymes Dec + */ + + +#include +#include + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // Print WiFi MAC address: + printMacAddress(); + + // scan for existing networks: + Serial.println("Scanning available networks..."); + listNetworks(); +} + +void loop() { + delay(10000); + // scan for existing networks: + Serial.println("Scanning available networks..."); + listNetworks(); +} + +void printMacAddress() { + // the MAC address of your Wifi shield + byte mac[6]; + + // print your MAC address: + WiFi.macAddress(mac); + Serial.print("MAC: "); + Serial.print(mac[5],HEX); + Serial.print(":"); + Serial.print(mac[4],HEX); + Serial.print(":"); + Serial.print(mac[3],HEX); + Serial.print(":"); + Serial.print(mac[2],HEX); + Serial.print(":"); + Serial.print(mac[1],HEX); + Serial.print(":"); + Serial.println(mac[0],HEX); +} + +void listNetworks() { + // scan for nearby networks: + Serial.println("** Scan Networks **"); + int numSsid = WiFi.scanNetworks(); + if (numSsid == -1) + { + Serial.println("Couldn't get a wifi connection"); + while(true); + } + + // print the list of networks seen: + Serial.print("number of available networks:"); + Serial.println(numSsid); + + // print the network number and name for each network found: + for (int thisNet = 0; thisNet +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; +WiFiServer server(80); + +void setup() { + Serial.begin(9600); // initialize serial communication + pinMode(9, OUTPUT); // set the LED pin mode + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + while(true); // don't continue + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to Network named: "); + Serial.println(ssid); // print the network name (SSID); + + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + // wait 10 seconds for connection: + delay(10000); + } + server.begin(); // start the web server on port 80 + printWifiStatus(); // you're connected now, so print out the status +} + + +void loop() { + WiFiClient client = server.available(); // listen for incoming clients + + if (client) { // if you get a client, + Serial.println("new client"); // print a message out the serial port + String currentLine = ""; // make a String to hold incoming data from the client + while (client.connected()) { // loop while the client's connected + if (client.available()) { // if there's bytes to read from the client, + char c = client.read(); // read a byte, then + Serial.write(c); // print it out the serial monitor + if (c == '\n') { // if the byte is a newline character + + // if the current line is blank, you got two newline characters in a row. + // that's the end of the client HTTP request, so send a response: + if (currentLine.length() == 0) { + // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK) + // and a content-type so the client knows what's coming, then a blank line: + client.println("HTTP/1.1 200 OK"); + client.println("Content-type:text/html"); + client.println(); + + // the content of the HTTP response follows the header: + client.print("Click here turn the LED on pin 9 on
"); + client.print("Click here turn the LED on pin 9 off
"); + + // The HTTP response ends with another blank line: + client.println(); + // break out of the while loop: + break; + } + else { // if you got a newline, then clear currentLine: + currentLine = ""; + } + } + else if (c != '\r') { // if you got anything else but a carriage return character, + currentLine += c; // add it to the end of the currentLine + } + + // Check to see if the client request was "GET /H" or "GET /L": + if (currentLine.endsWith("GET /H")) { + digitalWrite(9, HIGH); // GET /H turns the LED on + } + if (currentLine.endsWith("GET /L")) { + digitalWrite(9, LOW); // GET /L turns the LED off + } + } + } + // close the connection: + client.stop(); + Serial.println("client disonnected"); + } +} + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); + // print where to go in a browser: + Serial.print("To see this page in action, open a browser to http://"); + Serial.println(ip); +} diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiChatServer/WifiChatServer.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiChatServer/WifiChatServer.ino new file mode 100644 index 0000000..e4b1d1a --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiChatServer/WifiChatServer.ino @@ -0,0 +1,111 @@ +/* + Chat Server + + A simple server that distributes any incoming messages to all + connected clients. To use telnet to your device's IP address and type. + You can see the client's input in the serial monitor as well. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + + Circuit: + * WiFi shield attached + + created 18 Dec 2009 + by David A. Mellis + modified 31 May 2012 + by Tom Igoe + + */ + +#include +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password (use for WPA, or use as key for WEP) + +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; + +WiFiServer server(23); + +boolean alreadyConnected = false; // whether or not the client was connected previously + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + // start the server: + server.begin(); + // you're connected now, so print out the status: + printWifiStatus(); + } + + +void loop() { + // wait for a new client: + WiFiClient client = server.available(); + + + // when the client sends the first byte, say hello: + if (client) { + if (!alreadyConnected) { + // clead out the input buffer: + client.flush(); + Serial.println("We have a new client"); + client.println("Hello, client!"); + alreadyConnected = true; + } + + if (client.available() > 0) { + // read the bytes incoming from the client: + char thisChar = client.read(); + // echo the bytes back to the client: + server.write(thisChar); + // echo the bytes to the server as well: + Serial.write(thisChar); + } + } +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClient/WifiPachubeClient.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClient/WifiPachubeClient.ino new file mode 100644 index 0000000..f8ffc07 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClient/WifiPachubeClient.ino @@ -0,0 +1,190 @@ +/* + Wifi Pachube sensor client + + This sketch connects an analog sensor to Pachube (http://www.pachube.com) + using an Arduino Wifi shield. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + This example has been updated to use version 2.0 of the Pachube API. + To make it work, create a feed with a datastream, and give it the ID + sensor1. Or change the code below to match your feed. + + Circuit: + * Analog sensor attached to analog in 0 + * Wifi shield attached to pins 10, 11, 12, 13 + + created 13 Mar 2012 + modified 31 May 2012 + by Tom Igoe + modified 8 Sept 2012 + by Scott Fitzgerald + + This code is in the public domain. + + */ +#include +#include + +#define APIKEY "YOUR API KEY GOES HERE" // replace your pachube api key here +#define FEEDID 00000 // replace your feed ID +#define USERAGENT "My Arduino Project" // user agent is the project name + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password + +int status = WL_IDLE_STATUS; + +// initialize the library instance: +WiFiClient client; +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +IPAddress server(216,52,233,121); // numeric IP for api.pachube.com +//char server[] = "api.pachube.com"; // name address for pachube API + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 10*1000; //delay between updates to pachube.com + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + // you're connected now, so print out the status: + printWifiStatus(); +} + + +void loop() { + // read the analog sensor: + int sensorReading = analogRead(A0); + + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + while (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + sendData(sensorReading); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void sendData(int thisData) { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.print("PUT /v2/feeds/"); + client.print(FEEDID); + client.println(".csv HTTP/1.1"); + client.println("Host: api.pachube.com"); + client.print("X-ApiKey: "); + client.println(APIKEY); + client.print("User-Agent: "); + client.println(USERAGENT); + client.print("Content-Length: "); + + // calculate the length of the sensor reading in bytes: + // 8 bytes for "sensor1," + number of digits of the data: + int thisLength = 8 + getLength(thisData); + client.println(thisLength); + + // last pieces of the HTTP PUT request: + client.println("Content-Type: text/csv"); + client.println("Connection: close"); + client.println(); + + // here's the actual content of the PUT request: + client.print("sensor1,"); + client.println(thisData); + + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + // note the time that the connection was made or attempted: + lastConnectionTime = millis(); +} + + +// This method calculates the number of digits in the +// sensor reading. Since each digit of the ASCII decimal +// representation is a byte, the number of digits equals +// the number of bytes: + +int getLength(int someValue) { + // there's at least one byte: + int digits = 1; + // continually divide the value by ten, + // adding one to the digit count for each + // time you divide, until you're at 0: + int dividend = someValue /10; + while (dividend > 0) { + dividend = dividend /10; + digits++; + } + // return the number of digits: + return digits; +} + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClientString/WifiPachubeClientString.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClientString/WifiPachubeClientString.ino new file mode 100644 index 0000000..243fe83 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiPachubeClientString/WifiPachubeClientString.ino @@ -0,0 +1,177 @@ +/* + Wifi Pachube sensor client with Strings + + This sketch connects an analog sensor to Pachube (http://www.pachube.com) + using a Arduino Wifi shield. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + This example has been updated to use version 2.0 of the pachube.com API. + To make it work, create a feed with a datastream, and give it the ID + sensor1. Or change the code below to match your feed. + + This example uses the String library, which is part of the Arduino core from + version 0019. + + Circuit: + * Analog sensor attached to analog in 0 + * Wifi shield attached to pins 10, 11, 12, 13 + + created 16 Mar 2012 + modified 31 May 2012 + by Tom Igoe + modified 8 Sept 2012 + by Scott Fitzgerald + + This code is in the public domain. + + */ + +#include +#include + +#define APIKEY "YOUR API KEY GOES HERE" // replace your pachube api key here +#define FEEDID 00000 // replace your feed ID +#define USERAGENT "My Arduino Project" // user agent is the project name + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password + +int status = WL_IDLE_STATUS; + +// initialize the library instance: +WiFiClient client; + +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +//IPAddress server(216,52,233,121); // numeric IP for api.pachube.com +char server[] = "api.pachube.com"; // name address for pachube API + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 10*1000; //delay between updates to pachube.com + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + // you're connected now, so print out the status: + printWifiStatus(); +} + +void loop() { + // read the analog sensor: + int sensorReading = analogRead(A0); + // convert the data to a String to send it: + + String dataString = "sensor1,"; + dataString += sensorReading; + + // you can append multiple readings to this String if your + // pachube feed is set up to handle multiple values: + int otherSensorReading = analogRead(A1); + dataString += "\nsensor2,"; + dataString += otherSensorReading; + + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + while (client.available()) { + char c = client.read(); + Serial.print(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + sendData(dataString); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void sendData(String thisData) { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.print("PUT /v2/feeds/"); + client.print(FEEDID); + client.println(".csv HTTP/1.1"); + client.println("Host: api.pachube.com"); + client.print("X-ApiKey: "); + client.println(APIKEY); + client.print("User-Agent: "); + client.println(USERAGENT); + client.print("Content-Length: "); + client.println(thisData.length()); + + // last pieces of the HTTP PUT request: + client.println("Content-Type: text/csv"); + client.println("Connection: close"); + client.println(); + + // here's the actual content of the PUT request: + client.println(thisData); + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + // note the time that the connection was made or attempted: + lastConnectionTime = millis(); +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiTwitterClient/WifiTwitterClient.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiTwitterClient/WifiTwitterClient.ino new file mode 100644 index 0000000..3dc2c8d --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiTwitterClient/WifiTwitterClient.ino @@ -0,0 +1,163 @@ +/* + Wifi Twitter Client with Strings + + This sketch connects to Twitter using using an Arduino WiFi shield. + It parses the XML returned, and looks for this is a tweet + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + This example uses the String library, which is part of the Arduino core from + version 0019. + + Circuit: + * WiFi shield attached to pins 10, 11, 12, 13 + + created 23 apr 2012 + modified 31 May 2012 + by Tom Igoe + + This code is in the public domain. + + */ +#include +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "password"; // your network password (use for WPA, or use as key for WEP) +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; // status of the wifi connection + +// initialize the library instance: +WiFiClient client; + +const unsigned long requestInterval = 30*1000; // delay between requests; 30 seconds + +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +//IPAddress server(199,59,149,200); // numeric IP for api.twitter.com +char server[] = "api.twitter.com"; // name address for twitter API + +boolean requested; // whether you've made a request since connecting +unsigned long lastAttemptTime = 0; // last time you connected to the server, in milliseconds + +String currentLine = ""; // string to hold the text from server +String tweet = ""; // string to hold the tweet +boolean readingTweet = false; // if you're currently reading the tweet + +void setup() { + // reserve space for the strings: + currentLine.reserve(256); + tweet.reserve(150); + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + // you're connected now, so print out the status: + printWifiStatus(); + connectToServer(); +} + +void loop() +{ + if (client.connected()) { + if (client.available()) { + // read incoming bytes: + char inChar = client.read(); + + // add incoming byte to end of line: + currentLine += inChar; + + // if you get a newline, clear the line: + if (inChar == '\n') { + currentLine = ""; + } + // if the current line ends with , it will + // be followed by the tweet: + if ( currentLine.endsWith("")) { + // tweet is beginning. Clear the tweet string: + readingTweet = true; + tweet = ""; + // break out of the loop so this character isn't added to the tweet: + return; + } + // if you're currently reading the bytes of a tweet, + // add them to the tweet String: + if (readingTweet) { + if (inChar != '<') { + tweet += inChar; + } + else { + // if you got a "<" character, + // you've reached the end of the tweet: + readingTweet = false; + Serial.println(tweet); + // close the connection to the server: + client.stop(); + } + } + } + } + else if (millis() - lastAttemptTime > requestInterval) { + // if you're not connected, and two minutes have passed since + // your last connection, then attempt to connect again: + connectToServer(); + } +} + +void connectToServer() { + // attempt to connect, and wait a millisecond: + Serial.println("connecting to server..."); + if (client.connect(server, 80)) { + Serial.println("making HTTP request..."); + // make HTTP GET request to twitter: + client.println("GET /1/statuses/user_timeline.xml?screen_name=arduino HTTP/1.1"); + client.println("Host:api.twitter.com"); + client.println("Connection:close"); + client.println(); + } + // note the time of this connect attempt: + lastAttemptTime = millis(); +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClient/WifiWebClient.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClient/WifiWebClient.ino new file mode 100644 index 0000000..17f44a3 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClient/WifiWebClient.ino @@ -0,0 +1,121 @@ + +/* + Web client + + This sketch connects to a website (http://www.google.com) + using a WiFi shield. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + Circuit: + * WiFi shield attached + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 31 May 2012 + by Tom Igoe + */ + + +#include +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password (use for WPA, or use as key for WEP) +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; +// if you don't want to use DNS (and reduce your sketch size) +// use the numeric IP instead of the name for the server: +IPAddress server(173,194,73,105); // numeric IP for Google (no DNS) +//char server[] = "www.google.com"; // name address for Google (using DNS) + +// Initialize the Ethernet client library +// with the IP address and port of the server +// that you want to connect to (port 80 is default for HTTP): +WiFiClient client; + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + Serial.println("Connected to wifi"); + printWifiStatus(); + + Serial.println("\nStarting connection to server..."); + // if you get a connection, report back via serial: + if (client.connect(server, 80)) { + Serial.println("connected to server"); + // Make a HTTP request: + client.println("GET /search?q=arduino HTTP/1.1"); + client.println("Host:www.google.com"); + client.println("Connection: close"); + client.println(); + } +} + +void loop() { + // if there are incoming bytes available + // from the server, read them and print them: + while (client.available()) { + char c = client.read(); + Serial.write(c); + } + + // if the server's disconnected, stop the client: + if (!client.connected()) { + Serial.println(); + Serial.println("disconnecting from server."); + client.stop(); + + // do nothing forevermore: + while(true); + } +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + + + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClientRepeating/WifiWebClientRepeating.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClientRepeating/WifiWebClientRepeating.ino new file mode 100644 index 0000000..96eb628 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebClientRepeating/WifiWebClientRepeating.ino @@ -0,0 +1,138 @@ +/* + Repeating Wifi Web client + + This sketch connects to a a web server and makes a request + using an Arduino Wifi shield. + + Circuit: + * Wifi shield attached to pins 10, 11, 12, 13 + + created 23 April 2012 + modifide 31 May 2012 + by Tom Igoe + + http://arduino.cc/en/Tutorial/WifiWebClientRepeating + This code is in the public domain. + */ + +#include +#include + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; + +// Initialize the Wifi client library +WiFiClient client; + +// server address: +char server[] = "www.arduino.cc"; +//IPAddress server(64,131,82,241); + +unsigned long lastConnectionTime = 0; // last time you connected to the server, in milliseconds +boolean lastConnected = false; // state of the connection last time through the main loop +const unsigned long postingInterval = 10*1000; // delay between updates, in milliseconds + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + // you're connected now, so print out the status: + printWifiStatus(); +} + +void loop() { + // if there's incoming data from the net connection. + // send it out the serial port. This is for debugging + // purposes only: + while (client.available()) { + char c = client.read(); + Serial.write(c); + } + + // if there's no net connection, but there was one last time + // through the loop, then stop the client: + if (!client.connected() && lastConnected) { + Serial.println(); + Serial.println("disconnecting."); + client.stop(); + } + + // if you're not connected, and ten seconds have passed since + // your last connection, then connect again and send data: + if(!client.connected() && (millis() - lastConnectionTime > postingInterval)) { + httpRequest(); + } + // store the state of the connection for next time through + // the loop: + lastConnected = client.connected(); +} + +// this method makes a HTTP connection to the server: +void httpRequest() { + // if there's a successful connection: + if (client.connect(server, 80)) { + Serial.println("connecting..."); + // send the HTTP PUT request: + client.println("GET /latest.txt HTTP/1.1"); + client.println("Host: www.arduino.cc"); + client.println("User-Agent: arduino-ethernet"); + client.println("Connection: close"); + client.println(); + + // note the time that the connection was made: + lastConnectionTime = millis(); + } + else { + // if you couldn't make a connection: + Serial.println("connection failed"); + Serial.println("disconnecting."); + client.stop(); + } +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + + + + + + diff --git a/libs/arduino-1.0/libraries/WiFi/examples/WifiWebServer/WifiWebServer.ino b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebServer/WifiWebServer.ino new file mode 100644 index 0000000..ac5f056 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/examples/WifiWebServer/WifiWebServer.ino @@ -0,0 +1,132 @@ +/* + Web Server + + A simple web server that shows the value of the analog input pins. + using a WiFi shield. + + This example is written for a network using WPA encryption. For + WEP or WPA, change the Wifi.begin() call accordingly. + + Circuit: + * WiFi shield attached + * Analog inputs attached to pins A0 through A5 (optional) + + created 13 July 2010 + by dlf (Metodo2 srl) + modified 31 May 2012 + by Tom Igoe + */ +#include +#include + + +char ssid[] = "yourNetwork"; // your network SSID (name) +char pass[] = "secretPassword"; // your network password +int keyIndex = 0; // your network key Index number (needed only for WEP) + +int status = WL_IDLE_STATUS; + +WiFiServer server(80); + +void setup() { + //Initialize serial and wait for port to open: + Serial.begin(9600); + while (!Serial) { + ; // wait for serial port to connect. Needed for Leonardo only + } + + // check for the presence of the shield: + if (WiFi.status() == WL_NO_SHIELD) { + Serial.println("WiFi shield not present"); + // don't continue: + while(true); + } + + // attempt to connect to Wifi network: + while ( status != WL_CONNECTED) { + Serial.print("Attempting to connect to SSID: "); + Serial.println(ssid); + // Connect to WPA/WPA2 network. Change this line if using open or WEP network: + status = WiFi.begin(ssid, pass); + + // wait 10 seconds for connection: + delay(10000); + } + server.begin(); + // you're connected now, so print out the status: + printWifiStatus(); +} + + +void loop() { + // listen for incoming clients + WiFiClient client = server.available(); + if (client) { + Serial.println("new client"); + // an http request ends with a blank line + boolean currentLineIsBlank = true; + while (client.connected()) { + if (client.available()) { + char c = client.read(); + Serial.write(c); + // if you've gotten to the end of the line (received a newline + // character) and the line is blank, the http request has ended, + // so you can send a reply + if (c == '\n' && currentLineIsBlank) { + // send a standard http response header + client.println("HTTP/1.1 200 OK"); + client.println("Content-Type: text/html"); + client.println("Connnection: close"); + client.println(); + client.println(""); + client.println(""); + // add a meta refresh tag, so the browser pulls again every 5 seconds: + client.println(""); + // output the value of each analog input pin + for (int analogChannel = 0; analogChannel < 6; analogChannel++) { + int sensorReading = analogRead(analogChannel); + client.print("analog input "); + client.print(analogChannel); + client.print(" is "); + client.print(sensorReading); + client.println("
"); + } + client.println(""); + break; + } + if (c == '\n') { + // you're starting a new line + currentLineIsBlank = true; + } + else if (c != '\r') { + // you've gotten a character on the current line + currentLineIsBlank = false; + } + } + } + // give the web browser time to receive the data + delay(1); + // close the connection: + client.stop(); + Serial.println("client disonnected"); + } +} + + +void printWifiStatus() { + // print the SSID of the network you're attached to: + Serial.print("SSID: "); + Serial.println(WiFi.SSID()); + + // print your WiFi shield's IP address: + IPAddress ip = WiFi.localIP(); + Serial.print("IP Address: "); + Serial.println(ip); + + // print the received signal strength: + long rssi = WiFi.RSSI(); + Serial.print("signal strength (RSSI):"); + Serial.print(rssi); + Serial.println(" dBm"); +} + diff --git a/libs/arduino-1.0/libraries/WiFi/keywords.txt b/libs/arduino-1.0/libraries/WiFi/keywords.txt new file mode 100644 index 0000000..47704cd --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/keywords.txt @@ -0,0 +1,43 @@ +####################################### +# Syntax Coloring Map For WiFi +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +WiFi KEYWORD1 +Client KEYWORD1 +Server KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +status KEYWORD2 +connect KEYWORD2 +write KEYWORD2 +available KEYWORD2 +read KEYWORD2 +flush KEYWORD2 +stop KEYWORD2 +connected KEYWORD2 +begin KEYWORD2 +disconnect KEYWORD2 +macAddress KEYWORD2 +localIP KEYWORD2 +subnetMask KEYWORD2 +gatewayIP KEYWORD2 +SSID KEYWORD2 +BSSID KEYWORD2 +RSSI KEYWORD2 +encryptionType KEYWORD2 +getResult KEYWORD2 +getSocket KEYWORD2 +WiFiClient KEYWORD2 +WiFiServer KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/WiFi/utility/debug.h b/libs/arduino-1.0/libraries/WiFi/utility/debug.h new file mode 100644 index 0000000..9f71055 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/debug.h @@ -0,0 +1,77 @@ +//*********************************************/ +// +// File: debug.h +// +// Author: dlf (Metodo2 srl) +// +//********************************************/ + + +#ifndef Debug_H +#define Debug_H + +#include +#include + +#define PRINT_FILE_LINE() do { \ + Serial.print("[");Serial.print(__FILE__); \ + Serial.print("::");Serial.print(__LINE__);Serial.print("]");\ +}while (0); + +#ifdef _DEBUG_ + +#define INFO(format, args...) do { \ + char buf[250]; \ + sprintf(buf, format, args); \ + Serial.println(buf); \ +} while(0); + +#define INFO1(x) do { PRINT_FILE_LINE() Serial.print("-I-");\ + Serial.println(x); \ +}while (0); + + +#define INFO2(x,y) do { PRINT_FILE_LINE() Serial.print("-I-");\ + Serial.print(x,16);Serial.print(",");Serial.println(y,16); \ +}while (0); + + +#else +#define INFO1(x) do {} while(0); +#define INFO2(x,y) do {} while(0); +#define INFO(format, args...) do {} while(0); +#endif + +#if 0 +#define WARN(args) do { PRINT_FILE_LINE() \ + Serial.print("-W-"); Serial.println(args); \ +}while (0); +#else +#define WARN(args) do {} while (0); +#endif + +#if _DEBUG_SPI_ +#define DBG_PIN2 5 +#define DBG_PIN 4 + +#define START() digitalWrite(DBG_PIN2, HIGH); +#define END() digitalWrite(DBG_PIN2, LOW); +#define SET_TRIGGER() digitalWrite(DBG_PIN, HIGH); +#define RST_TRIGGER() digitalWrite(DBG_PIN, LOW); + +#define INIT_TRIGGER() pinMode(DBG_PIN, OUTPUT); \ + pinMode(DBG_PIN2, OUTPUT); \ + RST_TRIGGER() +#define TOGGLE_TRIGGER() SET_TRIGGER() \ + delayMicroseconds(2); \ + RST_TRIGGER() +#else +#define START() +#define END() +#define SET_TRIGGER() +#define RST_TRIGGER() +#define INIT_TRIGGER() +#define TOGGLE_TRIGGER() +#endif + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/utility/server_drv.cpp b/libs/arduino-1.0/libraries/WiFi/utility/server_drv.cpp new file mode 100644 index 0000000..05c3a17 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/server_drv.cpp @@ -0,0 +1,260 @@ +//#define _DEBUG_ + +#include "server_drv.h" + +#include "Arduino.h" +#include "spi_drv.h" + +extern "C" { +#include "wl_types.h" +#include "debug.h" +} + + +// Start server TCP on port specified +void ServerDrv::startServer(uint16_t port, uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(START_SERVER_TCP_CMD, PARAM_NUMS_2); + SpiDrv::sendParam(port); + SpiDrv::sendParam(&sock, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(START_SERVER_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); +} + +// Start server TCP on port specified +void ServerDrv::startClient(uint32_t ipAddress, uint16_t port, uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(START_CLIENT_TCP_CMD, PARAM_NUMS_3); + SpiDrv::sendParam((uint8_t*)&ipAddress, sizeof(ipAddress)); + SpiDrv::sendParam(port); + SpiDrv::sendParam(&sock, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(START_CLIENT_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); +} + +// Start server TCP on port specified +void ServerDrv::stopClient(uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(STOP_CLIENT_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendParam(&sock, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(STOP_CLIENT_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); +} + + +uint8_t ServerDrv::getServerState(uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_STATE_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendParam(&sock, sizeof(sock), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(GET_STATE_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + return _data; +} + +uint8_t ServerDrv::getClientState(uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_CLIENT_STATE_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendParam(&sock, sizeof(sock), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(GET_CLIENT_STATE_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + return _data; +} + +uint8_t ServerDrv::availData(uint8_t sock) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(AVAIL_DATA_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendParam(&sock, sizeof(sock), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(AVAIL_DATA_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + + if (_dataLen!=0) + { + return (_data == 1); + } + return false; +} + +bool ServerDrv::getData(uint8_t sock, uint8_t *data, uint8_t peek) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_DATA_TCP_CMD, PARAM_NUMS_2); + SpiDrv::sendParam(&sock, sizeof(sock)); + SpiDrv::sendParam(peek, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseData8(GET_DATA_TCP_CMD, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + if (_dataLen!=0) + { + *data = _data; + return true; + } + return false; +} + +bool ServerDrv::getDataBuf(uint8_t sock, uint8_t *_data, uint16_t *_dataLen) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_DATABUF_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendBuffer(&sock, sizeof(sock), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + if (!SpiDrv::waitResponseData16(GET_DATABUF_TCP_CMD, _data, _dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + if (*_dataLen!=0) + { + return true; + } + return false; +} + + +bool ServerDrv::sendData(uint8_t sock, const uint8_t *data, uint16_t len) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(SEND_DATA_TCP_CMD, PARAM_NUMS_2); + SpiDrv::sendBuffer(&sock, sizeof(sock)); + SpiDrv::sendBuffer((uint8_t *)data, len, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseData8(SEND_DATA_TCP_CMD, &_data, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + if (_dataLen!=0) + { + return (_data == 1); + } + return false; +} + + +uint8_t ServerDrv::checkDataSent(uint8_t sock) +{ + const uint16_t TIMEOUT_DATA_SENT = 25; + uint16_t timeout = 0; + uint8_t _data = 0; + uint8_t _dataLen = 0; + + do { + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(DATA_SENT_TCP_CMD, PARAM_NUMS_1); + SpiDrv::sendParam(&sock, sizeof(sock), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + if (!SpiDrv::waitResponseCmd(DATA_SENT_TCP_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse isDataSent"); + } + SpiDrv::spiSlaveDeselect(); + + if (_data) timeout = 0; + else{ + ++timeout; + delay(100); + } + + }while((_data==0)&&(timeout +#include "wifi_spi.h" + +class ServerDrv +{ +public: + // Start server TCP on port specified + static void startServer(uint16_t port, uint8_t sock); + + static void startClient(uint32_t ipAddress, uint16_t port, uint8_t sock); + + static void stopClient(uint8_t sock); + + static uint8_t getServerState(uint8_t sock); + + static uint8_t getClientState(uint8_t sock); + + static bool getData(uint8_t sock, uint8_t *data, uint8_t peek = 0); + + static bool getDataBuf(uint8_t sock, uint8_t *data, uint16_t *len); + + static bool sendData(uint8_t sock, const uint8_t *data, uint16_t len); + + static uint8_t availData(uint8_t sock); + + static uint8_t checkDataSent(uint8_t sock); +}; + +extern ServerDrv serverDrv; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/utility/socket.c b/libs/arduino-1.0/libraries/WiFi/utility/socket.c new file mode 100644 index 0000000..0eddd4a --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/socket.c @@ -0,0 +1,20 @@ +/* +* +@file socket.c +@brief define function of socket API +* +*/ +#include +#include "socket.h" + +SOCKET socket(uint8 protocol) {return 0;} // Opens a socket(TCP or UDP or IP_RAW mode) +void close(SOCKET s) {} // Close socket +uint8 connect(SOCKET s, uint8 * addr, uint16 port) {return 0;} // Establish TCP connection (Active connection) +void disconnect(SOCKET s) {} // disconnect the connection +uint8 listen(SOCKET s) { return 0;} // Establish TCP connection (Passive connection) +uint16 send(SOCKET s, const uint8 * buf, uint16 len) { return 0;} // Send data (TCP) +uint16 recv(SOCKET s, uint8 * buf, uint16 len) {return 0;} // Receive data (TCP) +uint16 sendto(SOCKET s, const uint8 * buf, uint16 len, uint8 * addr, uint16 port) {return 0;} // Send data (UDP/IP RAW) +uint16 recvfrom(SOCKET s, uint8 * buf, uint16 len, uint8 * addr, uint16 *port) {return 0;} // Receive data (UDP/IP RAW) + +uint16 igmpsend(SOCKET s, const uint8 * buf, uint16 len) {return 0;} diff --git a/libs/arduino-1.0/libraries/WiFi/utility/socket.h b/libs/arduino-1.0/libraries/WiFi/utility/socket.h new file mode 100644 index 0000000..8e7f6b6 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/socket.h @@ -0,0 +1,87 @@ +/* +* +@file socket.h +@brief define function of socket API +* +*/ + +#ifndef _SOCKET_H_ +#define _SOCKET_H_ + +#define TCP_SOCKET 1 +#define UDP_SOCKET 2 +#define RAW_SOCKET 3 + +#define SOCK_NOT_AVAIL 255 + +#include "wl_definitions.h" +/** + * The 8-bit signed data type. + */ +typedef char int8; +/** + * The volatile 8-bit signed data type. + */ +typedef volatile char vint8; +/** + * The 8-bit unsigned data type. + */ +typedef unsigned char uint8; +/** + * The volatile 8-bit unsigned data type. + */ +typedef volatile unsigned char vuint8; + +/** + * The 16-bit signed data type. + */ +typedef int int16; +/** + * The volatile 16-bit signed data type. + */ +typedef volatile int vint16; +/** + * The 16-bit unsigned data type. + */ +typedef unsigned int uint16; +/** + * The volatile 16-bit unsigned data type. + */ +typedef volatile unsigned int vuint16; +/** + * The 32-bit signed data type. + */ +typedef long int32; +/** + * The volatile 32-bit signed data type. + */ +typedef volatile long vint32; +/** + * The 32-bit unsigned data type. + */ +typedef unsigned long uint32; +/** + * The volatile 32-bit unsigned data type. + */ +typedef volatile unsigned long vuint32; + +/* bsd */ +typedef uint8 u_char; /**< 8-bit value */ +typedef uint16_t SOCKET; +typedef uint16 u_short; /**< 16-bit value */ +typedef uint16 u_int; /**< 16-bit value */ +typedef uint32 u_long; /**< 32-bit value */ + +extern SOCKET socket(uint8 protocol); // Opens a socket(TCP or UDP or IP_RAW mode) +extern void close(SOCKET s); // Close socket +extern uint8 connect(SOCKET s, uint8 * addr, uint16 port); // Establish TCP connection (Active connection) +extern void disconnect(SOCKET s); // disconnect the connection +extern uint8 listen(SOCKET s); // Establish TCP connection (Passive connection) +extern uint16 send(SOCKET s, const uint8 * buf, uint16 len); // Send data (TCP) +extern uint16 recv(SOCKET s, uint8 * buf, uint16 len); // Receive data (TCP) +extern uint16 sendto(SOCKET s, const uint8 * buf, uint16 len, uint8 * addr, uint16 port); // Send data (UDP/IP RAW) +extern uint16 recvfrom(SOCKET s, uint8 * buf, uint16 len, uint8 * addr, uint16 *port); // Receive data (UDP/IP RAW) + +extern uint16 igmpsend(SOCKET s, const uint8 * buf, uint16 len); +#endif +/* _SOCKET_H_ */ diff --git a/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.cpp b/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.cpp new file mode 100644 index 0000000..12a320b --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.cpp @@ -0,0 +1,506 @@ + +#include "Arduino.h" +#include "spi_drv.h" +#include "pins_arduino.h" +//#define _DEBUG_ +extern "C" { +#include "debug.h" +} + +#define DATAOUT 11 // MOSI +#define DATAIN 12 // MISO +#define SPICLOCK 13 // sck +#define SLAVESELECT 10 // ss +#define SLAVEREADY 7 // handshake pin +#define WIFILED 9 // led on wifi shield + +#define DELAY_100NS do { asm volatile("nop"); }while(0); +#define DELAY_SPI(X) { int ii=0; do { asm volatile("nop"); }while(++ii 0) && (_readChar != waitChar)); + return (_readChar == waitChar); +} + +int SpiDrv::readAndCheckChar(char checkChar, char* readChar) +{ + getParam((uint8_t*)readChar); + + return (*readChar == checkChar); +} + +char SpiDrv::readChar() +{ + uint8_t readChar = 0; + getParam(&readChar); + return readChar; +} + +#define WAIT_START_CMD(x) waitSpiChar(START_CMD) + +#define IF_CHECK_START_CMD(x) \ + if (!WAIT_START_CMD(_data)) \ + { \ + TOGGLE_TRIGGER() \ + WARN("Error waiting START_CMD"); \ + return 0; \ + }else \ + +#define CHECK_DATA(check, x) \ + if (!readAndCheckChar(check, &x)) \ + { \ + TOGGLE_TRIGGER() \ + WARN("Reply error"); \ + INFO2(check, (uint8_t)x); \ + return 0; \ + }else \ + +#define waitSlaveReady() (digitalRead(SLAVEREADY) == LOW) +#define waitSlaveSign() (digitalRead(SLAVEREADY) == HIGH) +#define waitSlaveSignalH() while(digitalRead(SLAVEREADY) != HIGH){} +#define waitSlaveSignalL() while(digitalRead(SLAVEREADY) != LOW){} + +void SpiDrv::waitForSlaveSign() +{ + while (!waitSlaveSign()); +} + +void SpiDrv::waitForSlaveReady() +{ + while (!waitSlaveReady()); +} + +void SpiDrv::getParam(uint8_t* param) +{ + // Get Params data + *param = spiTransfer(DUMMY_DATA); + DELAY_TRANSFER(); +} + +int SpiDrv::waitResponseCmd(uint8_t cmd, uint8_t numParam, uint8_t* param, uint8_t* param_len) +{ + char _data = 0; + int ii = 0; + + IF_CHECK_START_CMD(_data) + { + CHECK_DATA(cmd | REPLY_FLAG, _data){}; + + CHECK_DATA(numParam, _data); + { + readParamLen8(param_len); + for (ii=0; ii<(*param_len); ++ii) + { + // Get Params data + //param[ii] = spiTransfer(DUMMY_DATA); + getParam(¶m[ii]); + } + } + + readAndCheckChar(END_CMD, &_data); + } + + return 1; +} +/* +int SpiDrv::waitResponse(uint8_t cmd, uint8_t numParam, uint8_t* param, uint16_t* param_len) +{ + char _data = 0; + int i =0, ii = 0; + + IF_CHECK_START_CMD(_data) + { + CHECK_DATA(cmd | REPLY_FLAG, _data){}; + + CHECK_DATA(numParam, _data); + { + readParamLen16(param_len); + for (ii=0; ii<(*param_len); ++ii) + { + // Get Params data + param[ii] = spiTransfer(DUMMY_DATA); + } + } + + readAndCheckChar(END_CMD, &_data); + } + + return 1; +} +*/ + +int SpiDrv::waitResponseData16(uint8_t cmd, uint8_t* param, uint16_t* param_len) +{ + char _data = 0; + uint16_t ii = 0; + + IF_CHECK_START_CMD(_data) + { + CHECK_DATA(cmd | REPLY_FLAG, _data){}; + + uint8_t numParam = readChar(); + if (numParam != 0) + { + readParamLen16(param_len); + for (ii=0; ii<(*param_len); ++ii) + { + // Get Params data + param[ii] = spiTransfer(DUMMY_DATA); + } + } + + readAndCheckChar(END_CMD, &_data); + } + + return 1; +} + +int SpiDrv::waitResponseData8(uint8_t cmd, uint8_t* param, uint8_t* param_len) +{ + char _data = 0; + int ii = 0; + + IF_CHECK_START_CMD(_data) + { + CHECK_DATA(cmd | REPLY_FLAG, _data){}; + + uint8_t numParam = readChar(); + if (numParam != 0) + { + readParamLen8(param_len); + for (ii=0; ii<(*param_len); ++ii) + { + // Get Params data + param[ii] = spiTransfer(DUMMY_DATA); + } + } + + readAndCheckChar(END_CMD, &_data); + } + + return 1; +} + +int SpiDrv::waitResponseParams(uint8_t cmd, uint8_t numParam, tParam* params) +{ + char _data = 0; + int i =0, ii = 0; + + + IF_CHECK_START_CMD(_data) + { + CHECK_DATA(cmd | REPLY_FLAG, _data){}; + + uint8_t _numParam = readChar(); + if (_numParam != 0) + { + for (i=0; i<_numParam; ++i) + { + params[i].paramLen = readParamLen8(); + for (ii=0; ii maxNumParams) + { + numParam = maxNumParams; + } + *numParamRead = numParam; + if (numParam != 0) + { + for (i=0; i maxNumParams) + { + numParam = maxNumParams; + } + *numParamRead = numParam; + if (numParam != 0) + { + for (i=0; i>8)); + spiTransfer((uint8_t)(param_len & 0xff)); +} + +uint8_t SpiDrv::readParamLen8(uint8_t* param_len) +{ + uint8_t _param_len = spiTransfer(DUMMY_DATA); + if (param_len != NULL) + { + *param_len = _param_len; + } + return _param_len; +} + +uint16_t SpiDrv::readParamLen16(uint16_t* param_len) +{ + uint16_t _param_len = spiTransfer(DUMMY_DATA)<<8 | (spiTransfer(DUMMY_DATA)& 0xff); + if (param_len != NULL) + { + *param_len = _param_len; + } + return _param_len; +} + + +void SpiDrv::sendBuffer(uint8_t* param, uint16_t param_len, uint8_t lastParam) +{ + uint16_t i = 0; + + // Send Spi paramLen + sendParamLen16(param_len); + + // Send Spi param data + for (i=0; i>8)); + spiTransfer((uint8_t)(param & 0xff)); + + // if lastParam==1 Send Spi END CMD + if (lastParam == 1) + spiTransfer(END_CMD); +} + +/* Cmd Struct Message */ +/* _________________________________________________________________________________ */ +/*| START CMD | C/R | CMD |[TOT LEN]| N.PARAM | PARAM LEN | PARAM | .. | END CMD | */ +/*|___________|______|______|_________|_________|___________|________|____|_________| */ +/*| 8 bit | 1bit | 7bit | 8bit | 8bit | 8bit | nbytes | .. | 8bit | */ +/*|___________|______|______|_________|_________|___________|________|____|_________| */ + +void SpiDrv::sendCmd(uint8_t cmd, uint8_t numParam) +{ + // Send Spi START CMD + spiTransfer(START_CMD); + + //waitForSlaveSign(); + //wait the interrupt trigger on slave + delayMicroseconds(SPI_START_CMD_DELAY); + + // Send Spi C + cmd + spiTransfer(cmd & ~(REPLY_FLAG)); + + // Send Spi totLen + //spiTransfer(totLen); + + // Send Spi numParam + spiTransfer(numParam); + + // If numParam == 0 send END CMD + if (numParam == 0) + spiTransfer(END_CMD); + +} + +SpiDrv spiDrv; diff --git a/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.h b/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.h new file mode 100644 index 0000000..8cafb1b --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/spi_drv.h @@ -0,0 +1,83 @@ +#ifndef SPI_Drv_h +#define SPI_Drv_h + +#include +#include "wifi_spi.h" + +#define SPI_START_CMD_DELAY 12 + +#define NO_LAST_PARAM 0 +#define LAST_PARAM 1 + +#define DUMMY_DATA 0xFF + +#define WAIT_FOR_SLAVE_SELECT() \ + SpiDrv::waitForSlaveReady(); \ + SpiDrv::spiSlaveSelect(); + + + +class SpiDrv +{ +private: + //static bool waitSlaveReady(); + static void waitForSlaveSign(); + static void getParam(uint8_t* param); +public: + + static void begin(); + + static void end(); + + static void spiDriverInit(); + + static void spiSlaveSelect(); + + static void spiSlaveDeselect(); + + static char spiTransfer(volatile char data); + + static void waitForSlaveReady(); + + //static int waitSpiChar(char waitChar, char* readChar); + + static int waitSpiChar(unsigned char waitChar); + + static int readAndCheckChar(char checkChar, char* readChar); + + static char readChar(); + + static int waitResponseParams(uint8_t cmd, uint8_t numParam, tParam* params); + + static int waitResponseCmd(uint8_t cmd, uint8_t numParam, uint8_t* param, uint8_t* param_len); + + static int waitResponseData8(uint8_t cmd, uint8_t* param, uint8_t* param_len); + + static int waitResponseData16(uint8_t cmd, uint8_t* param, uint16_t* param_len); + /* + static int waitResponse(uint8_t cmd, tParam* params, uint8_t* numParamRead, uint8_t maxNumParams); + + static int waitResponse(uint8_t cmd, uint8_t numParam, uint8_t* param, uint16_t* param_len); +*/ + static int waitResponse(uint8_t cmd, uint8_t* numParamRead, uint8_t** params, uint8_t maxNumParams); + + static void sendParam(uint8_t* param, uint8_t param_len, uint8_t lastParam = NO_LAST_PARAM); + + static void sendParamLen8(uint8_t param_len); + + static void sendParamLen16(uint16_t param_len); + + static uint8_t readParamLen8(uint8_t* param_len = NULL); + + static uint16_t readParamLen16(uint16_t* param_len = NULL); + + static void sendBuffer(uint8_t* param, uint16_t param_len, uint8_t lastParam = NO_LAST_PARAM); + + static void sendParam(uint16_t param, uint8_t lastParam = NO_LAST_PARAM); + + static void sendCmd(uint8_t cmd, uint8_t numParam); +}; + +extern SpiDrv spiDrv; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.cpp b/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.cpp new file mode 100644 index 0000000..01023d7 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.cpp @@ -0,0 +1,491 @@ +#include +#include +#include + +#include "Arduino.h" +#include "spi_drv.h" +#include "wifi_drv.h" + +#define _DEBUG_ + +extern "C" { +#include "wifi_spi.h" +#include "wl_types.h" +#include "debug.h" +} + +// Array of data to cache the information related to the networks discovered +char WiFiDrv::_networkSsid[][WL_SSID_MAX_LENGTH] = {{"1"},{"2"},{"3"},{"4"},{"5"}}; +int32_t WiFiDrv::_networkRssi[WL_NETWORKS_LIST_MAXNUM] = { 0 }; +uint8_t WiFiDrv::_networkEncr[WL_NETWORKS_LIST_MAXNUM] = { 0 }; + +// Cached values of retrieved data +char WiFiDrv::_ssid[] = {0}; +uint8_t WiFiDrv::_bssid[] = {0}; +uint8_t WiFiDrv::_mac[] = {0}; +uint8_t WiFiDrv::_localIp[] = {0}; +uint8_t WiFiDrv::_subnetMask[] = {0}; +uint8_t WiFiDrv::_gatewayIp[] = {0}; +// Firmware version +char WiFiDrv::fwVersion[] = {0}; + + +// Private Methods + +void WiFiDrv::getNetworkData(uint8_t *ip, uint8_t *mask, uint8_t *gwip) +{ + tParam params[PARAM_NUMS_3] = { {0, (char*)ip}, {0, (char*)mask}, {0, (char*)gwip}}; + + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_IPADDR_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, sizeof(_dummy), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + SpiDrv::waitResponseParams(GET_IPADDR_CMD, PARAM_NUMS_3, params); + + SpiDrv::spiSlaveDeselect(); +} + +// Public Methods + + +void WiFiDrv::wifiDriverInit() +{ + SpiDrv::begin(); +} + +int8_t WiFiDrv::wifiSetNetwork(char* ssid, uint8_t ssid_len) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(SET_NET_CMD, PARAM_NUMS_1); + SpiDrv::sendParam((uint8_t*)ssid, ssid_len, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(SET_NET_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + _data = WL_FAILURE; + } + SpiDrv::spiSlaveDeselect(); + + return(_data == WIFI_SPI_ACK) ? WL_SUCCESS : WL_FAILURE; +} + +int8_t WiFiDrv::wifiSetPassphrase(char* ssid, uint8_t ssid_len, const char *passphrase, const uint8_t len) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(SET_PASSPHRASE_CMD, PARAM_NUMS_2); + SpiDrv::sendParam((uint8_t*)ssid, ssid_len, NO_LAST_PARAM); + SpiDrv::sendParam((uint8_t*)passphrase, len, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(SET_PASSPHRASE_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + _data = WL_FAILURE; + } + SpiDrv::spiSlaveDeselect(); + return _data; +} + + +int8_t WiFiDrv::wifiSetKey(char* ssid, uint8_t ssid_len, uint8_t key_idx, const void *key, const uint8_t len) +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(SET_KEY_CMD, PARAM_NUMS_3); + SpiDrv::sendParam((uint8_t*)ssid, ssid_len, NO_LAST_PARAM); + SpiDrv::sendParam(&key_idx, KEY_IDX_LEN, NO_LAST_PARAM); + SpiDrv::sendParam((uint8_t*)key, len, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(SET_KEY_CMD, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + _data = WL_FAILURE; + } + SpiDrv::spiSlaveDeselect(); + return _data; +} + +int8_t WiFiDrv::disconnect() +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(DISCONNECT_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + int8_t result = SpiDrv::waitResponseCmd(DISCONNECT_CMD, PARAM_NUMS_1, &_data, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return result; +} + +uint8_t WiFiDrv::getConnectionStatus() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_CONN_STATUS_CMD, PARAM_NUMS_0); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = -1; + uint8_t _dataLen = 0; + SpiDrv::waitResponseCmd(GET_CONN_STATUS_CMD, PARAM_NUMS_1, &_data, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return _data; +} + +uint8_t* WiFiDrv::getMacAddress() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_MACADDR_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + SpiDrv::waitResponseCmd(GET_MACADDR_CMD, PARAM_NUMS_1, _mac, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return _mac; +} + +void WiFiDrv::getIpAddress(IPAddress& ip) +{ + getNetworkData(_localIp, _subnetMask, _gatewayIp); + ip = _localIp; +} + + void WiFiDrv::getSubnetMask(IPAddress& mask) + { + getNetworkData(_localIp, _subnetMask, _gatewayIp); + mask = _subnetMask; + } + + void WiFiDrv::getGatewayIP(IPAddress& ip) + { + getNetworkData(_localIp, _subnetMask, _gatewayIp); + ip = _gatewayIp; + } + +char* WiFiDrv::getCurrentSSID() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_CURR_SSID_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + SpiDrv::waitResponseCmd(GET_CURR_SSID_CMD, PARAM_NUMS_1, (uint8_t*)_ssid, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return _ssid; +} + +uint8_t* WiFiDrv::getCurrentBSSID() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_CURR_BSSID_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + SpiDrv::waitResponseCmd(GET_CURR_BSSID_CMD, PARAM_NUMS_1, _bssid, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return _bssid; +} + +int32_t WiFiDrv::getCurrentRSSI() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_CURR_RSSI_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + int32_t rssi = 0; + SpiDrv::waitResponseCmd(GET_CURR_RSSI_CMD, PARAM_NUMS_1, (uint8_t*)&rssi, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return rssi; +} + +uint8_t WiFiDrv::getCurrentEncryptionType() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_CURR_ENCT_CMD, PARAM_NUMS_1); + + uint8_t _dummy = DUMMY_DATA; + SpiDrv::sendParam(&_dummy, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t dataLen = 0; + uint8_t encType = 0; + SpiDrv::waitResponseCmd(GET_CURR_ENCT_CMD, PARAM_NUMS_1, (uint8_t*)&encType, &dataLen); + + SpiDrv::spiSlaveDeselect(); + + return encType; +} + +int8_t WiFiDrv::startScanNetworks() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(START_SCAN_NETWORKS, PARAM_NUMS_0); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + + if (!SpiDrv::waitResponseCmd(START_SCAN_NETWORKS, PARAM_NUMS_1, &_data, &_dataLen)) + { + WARN("error waitResponse"); + _data = WL_FAILURE; + } + + SpiDrv::spiSlaveDeselect(); + + return (_data == WL_FAILURE)? _data : WL_SUCCESS; +} + + +uint8_t WiFiDrv::getScanNetworks() +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(SCAN_NETWORKS, PARAM_NUMS_0); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t ssidListNum = 0; + SpiDrv::waitResponse(SCAN_NETWORKS, &ssidListNum, (uint8_t**)_networkSsid, WL_NETWORKS_LIST_MAXNUM); + + SpiDrv::spiSlaveDeselect(); + + return ssidListNum; +} + +char* WiFiDrv::getSSIDNetoworks(uint8_t networkItem) +{ + if (networkItem >= WL_NETWORKS_LIST_MAXNUM) + return NULL; + + return _networkSsid[networkItem]; +} + +uint8_t WiFiDrv::getEncTypeNetowrks(uint8_t networkItem) +{ + if (networkItem >= WL_NETWORKS_LIST_MAXNUM) + return NULL; + + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_IDX_ENCT_CMD, PARAM_NUMS_1); + + SpiDrv::sendParam(&networkItem, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t dataLen = 0; + uint8_t encType = 0; + SpiDrv::waitResponseCmd(GET_IDX_ENCT_CMD, PARAM_NUMS_1, (uint8_t*)&encType, &dataLen); + + SpiDrv::spiSlaveDeselect(); + + return encType; +} + +int32_t WiFiDrv::getRSSINetoworks(uint8_t networkItem) +{ + if (networkItem >= WL_NETWORKS_LIST_MAXNUM) + return NULL; + int32_t networkRssi = 0; + + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(GET_IDX_RSSI_CMD, PARAM_NUMS_1); + + SpiDrv::sendParam(&networkItem, 1, LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t dataLen = 0; + SpiDrv::waitResponseCmd(GET_IDX_RSSI_CMD, PARAM_NUMS_1, (uint8_t*)&networkRssi, &dataLen); + + SpiDrv::spiSlaveDeselect(); + + return networkRssi; +} + +uint8_t WiFiDrv::reqHostByName(const char* aHostname) +{ + WAIT_FOR_SLAVE_SELECT(); + + // Send Command + SpiDrv::sendCmd(REQ_HOST_BY_NAME_CMD, PARAM_NUMS_1); + SpiDrv::sendParam((uint8_t*)aHostname, strlen(aHostname), LAST_PARAM); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _data = 0; + uint8_t _dataLen = 0; + uint8_t result = SpiDrv::waitResponseCmd(REQ_HOST_BY_NAME_CMD, PARAM_NUMS_1, &_data, &_dataLen); + + SpiDrv::spiSlaveDeselect(); + + return result; +} + +int WiFiDrv::getHostByName(IPAddress& aResult) +{ + uint8_t _ipAddr[WL_IPV4_LENGTH]; + IPAddress dummy(0xFF,0xFF,0xFF,0xFF); + int result = 0; + + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_HOST_BY_NAME_CMD, PARAM_NUMS_0); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(GET_HOST_BY_NAME_CMD, PARAM_NUMS_1, _ipAddr, &_dataLen)) + { + WARN("error waitResponse"); + }else{ + aResult = _ipAddr; + result = (aResult != dummy); + } + SpiDrv::spiSlaveDeselect(); + return result; +} + +int WiFiDrv::getHostByName(const char* aHostname, IPAddress& aResult) +{ + uint8_t retry = 10; + if (reqHostByName(aHostname)) + { + while(!getHostByName(aResult) && --retry > 0) + { + delay(1000); + } + }else{ + return 0; + } + return (retry>0); +} + +char* WiFiDrv::getFwVersion() +{ + WAIT_FOR_SLAVE_SELECT(); + // Send Command + SpiDrv::sendCmd(GET_FW_VERSION_CMD, PARAM_NUMS_0); + + //Wait the reply elaboration + SpiDrv::waitForSlaveReady(); + + // Wait for reply + uint8_t _dataLen = 0; + if (!SpiDrv::waitResponseCmd(GET_FW_VERSION_CMD, PARAM_NUMS_1, (uint8_t*)fwVersion, &_dataLen)) + { + WARN("error waitResponse"); + } + SpiDrv::spiSlaveDeselect(); + return fwVersion; +} + +WiFiDrv wiFiDrv; diff --git a/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.h b/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.h new file mode 100644 index 0000000..8aeb8ae --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/wifi_drv.h @@ -0,0 +1,219 @@ +#ifndef WiFi_Drv_h +#define WiFi_Drv_h + +#include +#include "wifi_spi.h" +#include "IPAddress.h" + +// Key index length +#define KEY_IDX_LEN 1 +// 5 secs of delay to have the connection established +#define WL_DELAY_START_CONNECTION 5000 +// firmware version string length +#define WL_FW_VER_LENGTH 6 + +class WiFiDrv +{ +private: + // settings of requested network + static char _networkSsid[WL_NETWORKS_LIST_MAXNUM][WL_SSID_MAX_LENGTH]; + static int32_t _networkRssi[WL_NETWORKS_LIST_MAXNUM]; + static uint8_t _networkEncr[WL_NETWORKS_LIST_MAXNUM]; + + // firmware version string in the format a.b.c + static char fwVersion[WL_FW_VER_LENGTH]; + + // settings of current selected network + static char _ssid[WL_SSID_MAX_LENGTH]; + static uint8_t _bssid[WL_MAC_ADDR_LENGTH]; + static uint8_t _mac[WL_MAC_ADDR_LENGTH]; + static uint8_t _localIp[WL_IPV4_LENGTH]; + static uint8_t _subnetMask[WL_IPV4_LENGTH]; + static uint8_t _gatewayIp[WL_IPV4_LENGTH]; + + /* + * Get network Data information + */ + static void getNetworkData(uint8_t *ip, uint8_t *mask, uint8_t *gwip); + + static uint8_t reqHostByName(const char* aHostname); + + static int getHostByName(IPAddress& aResult); + +public: + + /* + * Driver initialization + */ + static void wifiDriverInit(); + + /* + * Set the desired network which the connection manager should try to + * connect to. + * + * The ssid of the desired network should be specified. + * + * param ssid: The ssid of the desired network. + * param ssid_len: Lenght of ssid string. + * return: WL_SUCCESS or WL_FAILURE + */ + static int8_t wifiSetNetwork(char* ssid, uint8_t ssid_len); + + /* Start Wifi connection with passphrase + * the most secure supported mode will be automatically selected + * + * param ssid: Pointer to the SSID string. + * param ssid_len: Lenght of ssid string. + * param passphrase: Passphrase. Valid characters in a passphrase + * must be between ASCII 32-126 (decimal). + * param len: Lenght of passphrase string. + * return: WL_SUCCESS or WL_FAILURE + */ + static int8_t wifiSetPassphrase(char* ssid, uint8_t ssid_len, const char *passphrase, const uint8_t len); + + /* Start Wifi connection with WEP encryption. + * Configure a key into the device. The key type (WEP-40, WEP-104) + * is determined by the size of the key (5 bytes for WEP-40, 13 bytes for WEP-104). + * + * param ssid: Pointer to the SSID string. + * param ssid_len: Lenght of ssid string. + * param key_idx: The key index to set. Valid values are 0-3. + * param key: Key input buffer. + * param len: Lenght of key string. + * return: WL_SUCCESS or WL_FAILURE + */ + static int8_t wifiSetKey(char* ssid, uint8_t ssid_len, uint8_t key_idx, const void *key, const uint8_t len); + + /* + * Disconnect from the network + * + * return: WL_SUCCESS or WL_FAILURE + */ + static int8_t disconnect(); + + /* + * Disconnect from the network + * + * return: one value of wl_status_t enum + */ + static uint8_t getConnectionStatus(); + + /* + * Get the interface MAC address. + * + * return: pointer to uint8_t array with length WL_MAC_ADDR_LENGTH + */ + static uint8_t* getMacAddress(); + + /* + * Get the interface IP address. + * + * return: copy the ip address value in IPAddress object + */ + static void getIpAddress(IPAddress& ip); + + /* + * Get the interface subnet mask address. + * + * return: copy the subnet mask address value in IPAddress object + */ + static void getSubnetMask(IPAddress& mask); + + /* + * Get the gateway ip address. + * + * return: copy the gateway ip address value in IPAddress object + */ + static void getGatewayIP(IPAddress& ip); + + /* + * Return the current SSID associated with the network + * + * return: ssid string + */ + static char* getCurrentSSID(); + + /* + * Return the current BSSID associated with the network. + * It is the MAC address of the Access Point + * + * return: pointer to uint8_t array with length WL_MAC_ADDR_LENGTH + */ + static uint8_t* getCurrentBSSID(); + + /* + * Return the current RSSI /Received Signal Strength in dBm) + * associated with the network + * + * return: signed value + */ + static int32_t getCurrentRSSI(); + + /* + * Return the Encryption Type associated with the network + * + * return: one value of wl_enc_type enum + */ + static uint8_t getCurrentEncryptionType(); + + /* + * Start scan WiFi networks available + * + * return: Number of discovered networks + */ + static int8_t startScanNetworks(); + + /* + * Get the networks available + * + * return: Number of discovered networks + */ + static uint8_t getScanNetworks(); + + /* + * Return the SSID discovered during the network scan. + * + * param networkItem: specify from which network item want to get the information + * + * return: ssid string of the specified item on the networks scanned list + */ + static char* getSSIDNetoworks(uint8_t networkItem); + + /* + * Return the RSSI of the networks discovered during the scanNetworks + * + * param networkItem: specify from which network item want to get the information + * + * return: signed value of RSSI of the specified item on the networks scanned list + */ + static int32_t getRSSINetoworks(uint8_t networkItem); + + /* + * Return the encryption type of the networks discovered during the scanNetworks + * + * param networkItem: specify from which network item want to get the information + * + * return: encryption type (enum wl_enc_type) of the specified item on the networks scanned list + */ + static uint8_t getEncTypeNetowrks(uint8_t networkItem); + + /* + * Resolve the given hostname to an IP address. + * param aHostname: Name to be resolved + * param aResult: IPAddress structure to store the returned IP address + * result: 1 if aIPAddrString was successfully converted to an IP address, + * else error code + */ + static int getHostByName(const char* aHostname, IPAddress& aResult); + + /* + * Get the firmware version + * result: version as string with this format a.b.c + */ + static char* getFwVersion(); + +}; + +extern WiFiDrv wiFiDrv; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/utility/wifi_spi.h b/libs/arduino-1.0/libraries/WiFi/utility/wifi_spi.h new file mode 100644 index 0000000..adf8bef --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/wifi_spi.h @@ -0,0 +1,144 @@ +#ifndef WiFi_Spi_h +#define WiFi_Spi_h + +#include "wl_definitions.h" + +#define CMD_FLAG 0 +#define REPLY_FLAG 1<<7 +#define DATA_FLAG 0x40 + +#define WIFI_SPI_ACK 1 +#define WIFI_SPI_ERR 0xFF + +#define TIMEOUT_CHAR 1000 + +//#define MAX_SOCK_NUM 4 /**< Maxmium number of socket */ +#define NO_SOCKET_AVAIL 255 + +#define START_CMD 0xE0 +#define END_CMD 0xEE +#define ERR_CMD 0xEF + +enum { + SET_NET_CMD = 0x10, + SET_PASSPHRASE_CMD = 0x11, + SET_KEY_CMD = 0x12, + TEST_CMD = 0x13, + + GET_CONN_STATUS_CMD = 0x20, + GET_IPADDR_CMD = 0x21, + GET_MACADDR_CMD = 0x22, + GET_CURR_SSID_CMD = 0x23, + GET_CURR_BSSID_CMD = 0x24, + GET_CURR_RSSI_CMD = 0x25, + GET_CURR_ENCT_CMD = 0x26, + SCAN_NETWORKS = 0x27, + START_SERVER_TCP_CMD= 0x28, + GET_STATE_TCP_CMD = 0x29, + DATA_SENT_TCP_CMD = 0x2A, + AVAIL_DATA_TCP_CMD = 0x2B, + GET_DATA_TCP_CMD = 0x2C, + START_CLIENT_TCP_CMD= 0x2D, + STOP_CLIENT_TCP_CMD = 0x2E, + GET_CLIENT_STATE_TCP_CMD= 0x2F, + DISCONNECT_CMD = 0x30, + GET_IDX_SSID_CMD = 0x31, + GET_IDX_RSSI_CMD = 0x32, + GET_IDX_ENCT_CMD = 0x33, + REQ_HOST_BY_NAME_CMD= 0x34, + GET_HOST_BY_NAME_CMD= 0x35, + START_SCAN_NETWORKS = 0x36, + GET_FW_VERSION_CMD = 0x37, + + // All command with DATA_FLAG 0x40 send a 16bit Len + + SEND_DATA_TCP_CMD = 0x44, + GET_DATABUF_TCP_CMD = 0x45, +}; + + +enum wl_tcp_state { + CLOSED = 0, + LISTEN = 1, + SYN_SENT = 2, + SYN_RCVD = 3, + ESTABLISHED = 4, + FIN_WAIT_1 = 5, + FIN_WAIT_2 = 6, + CLOSE_WAIT = 7, + CLOSING = 8, + LAST_ACK = 9, + TIME_WAIT = 10 +}; + + +enum numParams{ + PARAM_NUMS_0, + PARAM_NUMS_1, + PARAM_NUMS_2, + PARAM_NUMS_3, + PARAM_NUMS_4, + PARAM_NUMS_5, + MAX_PARAM_NUMS +}; + +#define MAX_PARAMS MAX_PARAM_NUMS-1 +#define PARAM_LEN_SIZE 1 + +typedef struct __attribute__((__packed__)) +{ + uint8_t paramLen; + char* param; +}tParam; + +typedef struct __attribute__((__packed__)) +{ + uint16_t dataLen; + char* data; +}tDataParam; + + +typedef struct __attribute__((__packed__)) +{ + unsigned char cmd; + unsigned char tcmd; + unsigned char nParam; + tParam params[MAX_PARAMS]; +}tSpiMsg; + +typedef struct __attribute__((__packed__)) +{ + unsigned char cmd; + unsigned char tcmd; + unsigned char nParam; + tDataParam params[MAX_PARAMS]; +}tSpiMsgData; + + +typedef struct __attribute__((__packed__)) +{ + unsigned char cmd; + unsigned char tcmd; + //unsigned char totLen; + unsigned char nParam; +}tSpiHdr; + +typedef struct __attribute__((__packed__)) +{ + uint8_t paramLen; + uint32_t param; +}tLongParam; + +typedef struct __attribute__((__packed__)) +{ + uint8_t paramLen; + uint16_t param; +}tIntParam; + +typedef struct __attribute__((__packed__)) +{ + uint8_t paramLen; + uint8_t param; +}tByteParam; + +#endif diff --git a/libs/arduino-1.0/libraries/WiFi/utility/wl_definitions.h b/libs/arduino-1.0/libraries/WiFi/utility/wl_definitions.h new file mode 100644 index 0000000..15de781 --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/wl_definitions.h @@ -0,0 +1,50 @@ +/* + * wl_definitions.h + * + * Created on: Mar 6, 2011 + * Author: dlafauci + */ + +#ifndef WL_DEFINITIONS_H_ +#define WL_DEFINITIONS_H_ + +// Maximum size of a SSID +#define WL_SSID_MAX_LENGTH 32 +// Length of passphrase. Valid lengths are 8-63. +#define WL_WPA_KEY_MAX_LENGTH 63 +// Length of key in bytes. Valid values are 5 and 13. +#define WL_WEP_KEY_MAX_LENGTH 13 +// Size of a MAC-address or BSSID +#define WL_MAC_ADDR_LENGTH 6 +// Size of a MAC-address or BSSID +#define WL_IPV4_LENGTH 4 +// Maximum size of a SSID list +#define WL_NETWORKS_LIST_MAXNUM 10 +// Maxmium number of socket +#define MAX_SOCK_NUM 4 +//Maximum number of attempts to establish wifi connection +#define WL_MAX_ATTEMPT_CONNECTION 10 + +typedef enum { + WL_NO_SHIELD = 255, + WL_IDLE_STATUS = 0, + WL_NO_SSID_AVAIL, + WL_SCAN_COMPLETED, + WL_CONNECTED, + WL_CONNECT_FAILED, + WL_CONNECTION_LOST, + WL_DISCONNECTED +} wl_status_t; + +/* Encryption modes */ +enum wl_enc_type { /* Values map to 802.11 encryption suites... */ + ENC_TYPE_WEP = 5, + ENC_TYPE_TKIP = 2, + ENC_TYPE_CCMP = 4, + /* ... except these two, 7 and 8 are reserved in 802.11-2007 */ + ENC_TYPE_NONE = 7, + ENC_TYPE_AUTO = 8 +}; + + +#endif /* WL_DEFINITIONS_H_ */ diff --git a/libs/arduino-1.0/libraries/WiFi/utility/wl_types.h b/libs/arduino-1.0/libraries/WiFi/utility/wl_types.h new file mode 100644 index 0000000..5eed7ee --- /dev/null +++ b/libs/arduino-1.0/libraries/WiFi/utility/wl_types.h @@ -0,0 +1,31 @@ +/* + * wl_types.h + * + * Created on: Jul 30, 2010 + * Author: dlafauci + */ + + +#ifndef _WL_TYPES_H_ +#define _WL_TYPES_H_ + +#include + +typedef enum { + WL_FAILURE = -1, + WL_SUCCESS = 1, +} wl_error_code_t; + +/* Authentication modes */ +enum wl_auth_mode { + AUTH_MODE_INVALID, + AUTH_MODE_AUTO, + AUTH_MODE_OPEN_SYSTEM, + AUTH_MODE_SHARED_KEY, + AUTH_MODE_WPA, + AUTH_MODE_WPA2, + AUTH_MODE_WPA_PSK, + AUTH_MODE_WPA2_PSK +}; + +#endif //_WL_TYPES_H_ diff --git a/libs/arduino-1.0/libraries/Wire/Wire.cpp b/libs/arduino-1.0/libraries/Wire/Wire.cpp new file mode 100644 index 0000000..4e7a17c --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/Wire.cpp @@ -0,0 +1,298 @@ +/* + TwoWire.cpp - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +extern "C" { + #include + #include + #include + #include "twi.h" +} + +#include "Wire.h" + +// Initialize Class Variables ////////////////////////////////////////////////// + +uint8_t TwoWire::rxBuffer[BUFFER_LENGTH]; +uint8_t TwoWire::rxBufferIndex = 0; +uint8_t TwoWire::rxBufferLength = 0; + +uint8_t TwoWire::txAddress = 0; +uint8_t TwoWire::txBuffer[BUFFER_LENGTH]; +uint8_t TwoWire::txBufferIndex = 0; +uint8_t TwoWire::txBufferLength = 0; + +uint8_t TwoWire::transmitting = 0; +void (*TwoWire::user_onRequest)(void); +void (*TwoWire::user_onReceive)(int); + +// Constructors //////////////////////////////////////////////////////////////// + +TwoWire::TwoWire() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void TwoWire::begin(void) +{ + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); +} + +void TwoWire::begin(uint8_t address) +{ + twi_setAddress(address); + twi_attachSlaveTxEvent(onRequestService); + twi_attachSlaveRxEvent(onReceiveService); + begin(); +} + +void TwoWire::begin(int address) +{ + begin((uint8_t)address); +} + +uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) +{ + // clamp to buffer length + if(quantity > BUFFER_LENGTH){ + quantity = BUFFER_LENGTH; + } + // perform blocking read into buffer + uint8_t read = twi_readFrom(address, rxBuffer, quantity, sendStop); + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; +} + +uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true); +} + +uint8_t TwoWire::requestFrom(int address, int quantity) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true); +} + +uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop) +{ + return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)sendStop); +} + +void TwoWire::beginTransmission(uint8_t address) +{ + // indicate that we are transmitting + transmitting = 1; + // set address of targeted slave + txAddress = address; + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; +} + +void TwoWire::beginTransmission(int address) +{ + beginTransmission((uint8_t)address); +} + +// +// Originally, 'endTransmission' was an f(void) function. +// It has been modified to take one parameter indicating +// whether or not a STOP should be performed on the bus. +// Calling endTransmission(false) allows a sketch to +// perform a repeated start. +// +// WARNING: Nothing in the library keeps track of whether +// the bus tenure has been properly ended with a STOP. It +// is very possible to leave the bus in a hung state if +// no call to endTransmission(true) is made. Some I2C +// devices will behave oddly if they do not see a STOP. +// +uint8_t TwoWire::endTransmission(uint8_t sendStop) +{ + // transmit buffer (blocking) + int8_t ret = twi_writeTo(txAddress, txBuffer, txBufferLength, 1, sendStop); + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + // indicate that we are done transmitting + transmitting = 0; + return ret; +} + +// This provides backwards compatibility with the original +// definition, and expected behaviour, of endTransmission +// +uint8_t TwoWire::endTransmission(void) +{ + return endTransmission(true); +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +size_t TwoWire::write(uint8_t data) +{ + if(transmitting){ + // in master transmitter mode + // don't bother if buffer is full + if(txBufferLength >= BUFFER_LENGTH){ + setWriteError(); + return 0; + } + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + // update amount in buffer + txBufferLength = txBufferIndex; + }else{ + // in slave send mode + // reply to master + twi_transmit(&data, 1); + } + return 1; +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +size_t TwoWire::write(const uint8_t *data, size_t quantity) +{ + if(transmitting){ + // in master transmitter mode + for(size_t i = 0; i < quantity; ++i){ + write(data[i]); + } + }else{ + // in slave send mode + // reply to master + twi_transmit(data, quantity); + } + return quantity; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::available(void) +{ + return rxBufferLength - rxBufferIndex; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::read(void) +{ + int value = -1; + + // get each successive byte on each call + if(rxBufferIndex < rxBufferLength){ + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; +} + +// must be called in: +// slave rx event callback +// or after requestFrom(address, numBytes) +int TwoWire::peek(void) +{ + int value = -1; + + if(rxBufferIndex < rxBufferLength){ + value = rxBuffer[rxBufferIndex]; + } + + return value; +} + +void TwoWire::flush(void) +{ + // XXX: to be implemented. +} + +// behind the scenes function that is called when data is received +void TwoWire::onReceiveService(uint8_t* inBytes, int numBytes) +{ + // don't bother if user hasn't registered a callback + if(!user_onReceive){ + return; + } + // don't bother if rx buffer is in use by a master requestFrom() op + // i know this drops data, but it allows for slight stupidity + // meaning, they may not have read all the master requestFrom() data yet + if(rxBufferIndex < rxBufferLength){ + return; + } + // copy twi rx buffer into local read buffer + // this enables new reads to happen in parallel + for(uint8_t i = 0; i < numBytes; ++i){ + rxBuffer[i] = inBytes[i]; + } + // set rx iterator vars + rxBufferIndex = 0; + rxBufferLength = numBytes; + // alert user program + user_onReceive(numBytes); +} + +// behind the scenes function that is called when data is requested +void TwoWire::onRequestService(void) +{ + // don't bother if user hasn't registered a callback + if(!user_onRequest){ + return; + } + // reset tx buffer iterator vars + // !!! this will kill any pending pre-master sendTo() activity + txBufferIndex = 0; + txBufferLength = 0; + // alert user program + user_onRequest(); +} + +// sets function called on slave write +void TwoWire::onReceive( void (*function)(int) ) +{ + user_onReceive = function; +} + +// sets function called on slave read +void TwoWire::onRequest( void (*function)(void) ) +{ + user_onRequest = function; +} + +// Preinstantiate Objects ////////////////////////////////////////////////////// + +TwoWire Wire = TwoWire(); + diff --git a/libs/arduino-1.0/libraries/Wire/Wire.h b/libs/arduino-1.0/libraries/Wire/Wire.h new file mode 100644 index 0000000..a93d0f5 --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/Wire.h @@ -0,0 +1,79 @@ +/* + TwoWire.h - TWI/I2C library for Arduino & Wiring + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +#ifndef TwoWire_h +#define TwoWire_h + +#include +#include "Stream.h" + +#define BUFFER_LENGTH 32 + +class TwoWire : public Stream +{ + private: + static uint8_t rxBuffer[]; + static uint8_t rxBufferIndex; + static uint8_t rxBufferLength; + + static uint8_t txAddress; + static uint8_t txBuffer[]; + static uint8_t txBufferIndex; + static uint8_t txBufferLength; + + static uint8_t transmitting; + static void (*user_onRequest)(void); + static void (*user_onReceive)(int); + static void onRequestService(void); + static void onReceiveService(uint8_t*, int); + public: + TwoWire(); + void begin(); + void begin(uint8_t); + void begin(int); + void beginTransmission(uint8_t); + void beginTransmission(int); + uint8_t endTransmission(void); + uint8_t endTransmission(uint8_t); + uint8_t requestFrom(uint8_t, uint8_t); + uint8_t requestFrom(uint8_t, uint8_t, uint8_t); + uint8_t requestFrom(int, int); + uint8_t requestFrom(int, int, int); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t *, size_t); + virtual int available(void); + virtual int read(void); + virtual int peek(void); + virtual void flush(void); + void onReceive( void (*)(int) ); + void onRequest( void (*)(void) ); + + inline size_t write(unsigned long n) { return write((uint8_t)n); } + inline size_t write(long n) { return write((uint8_t)n); } + inline size_t write(unsigned int n) { return write((uint8_t)n); } + inline size_t write(int n) { return write((uint8_t)n); } + using Print::write; +}; + +extern TwoWire Wire; + +#endif + diff --git a/libs/arduino-1.0/libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino b/libs/arduino-1.0/libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino new file mode 100644 index 0000000..9c41c18 --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/SFRRanger_reader/SFRRanger_reader.ino @@ -0,0 +1,87 @@ +// I2C SRF10 or SRF08 Devantech Ultrasonic Ranger Finder +// by Nicholas Zambetti +// and James Tichenor + +// Demonstrates use of the Wire library reading data from the +// Devantech Utrasonic Rangers SFR08 and SFR10 + +// Created 29 April 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) + Serial.begin(9600); // start serial communication at 9600bps +} + +int reading = 0; + +void loop() +{ + // step 1: instruct sensor to read echoes + Wire.beginTransmission(112); // transmit to device #112 (0x70) + // the address specified in the datasheet is 224 (0xE0) + // but i2c adressing uses the high 7 bits so it's 112 + Wire.write(byte(0x00)); // sets register pointer to the command register (0x00) + Wire.write(byte(0x50)); // command sensor to measure in "inches" (0x50) + // use 0x51 for centimeters + // use 0x52 for ping microseconds + Wire.endTransmission(); // stop transmitting + + // step 2: wait for readings to happen + delay(70); // datasheet suggests at least 65 milliseconds + + // step 3: instruct sensor to return a particular echo reading + Wire.beginTransmission(112); // transmit to device #112 + Wire.write(byte(0x02)); // sets register pointer to echo #1 register (0x02) + Wire.endTransmission(); // stop transmitting + + // step 4: request reading from sensor + Wire.requestFrom(112, 2); // request 2 bytes from slave device #112 + + // step 5: receive reading from sensor + if(2 <= Wire.available()) // if two bytes were received + { + reading = Wire.read(); // receive high byte (overwrites previous reading) + reading = reading << 8; // shift high byte to be high 8 bits + reading |= Wire.read(); // receive low byte as lower 8 bits + Serial.println(reading); // print the reading + } + + delay(250); // wait a bit since people have to read the output :) +} + + +/* + +// The following code changes the address of a Devantech Ultrasonic Range Finder (SRF10 or SRF08) +// usage: changeAddress(0x70, 0xE6); + +void changeAddress(byte oldAddress, byte newAddress) +{ + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xA0)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xAA)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(byte(0xA5)); + Wire.endTransmission(); + + Wire.beginTransmission(oldAddress); + Wire.write(byte(0x00)); + Wire.write(newAddress); + Wire.endTransmission(); +} + +*/ diff --git a/libs/arduino-1.0/libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino b/libs/arduino-1.0/libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino new file mode 100644 index 0000000..38da1c5 --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino @@ -0,0 +1,39 @@ +// I2C Digital Potentiometer +// by Nicholas Zambetti +// and Shawn Bonkowski + +// Demonstrates use of the Wire library +// Controls AD5171 digital potentiometer via I2C/TWI + +// Created 31 March 2006 + +// This example code is in the public domain. + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) +} + +byte val = 0; + +void loop() +{ + Wire.beginTransmission(44); // transmit to device #44 (0x2c) + // device address is specified in datasheet + Wire.write(byte(0x00)); // sends instruction byte + Wire.write(val); // sends potentiometer value byte + Wire.endTransmission(); // stop transmitting + + val++; // increment value + if(val == 64) // if reached 64th position (max) + { + val = 0; // start over from lowest value + } + delay(500); +} + diff --git a/libs/arduino-1.0/libraries/Wire/examples/master_reader/master_reader.ino b/libs/arduino-1.0/libraries/Wire/examples/master_reader/master_reader.ino new file mode 100644 index 0000000..4124d7d --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/master_reader/master_reader.ino @@ -0,0 +1,32 @@ +// Wire Master Reader +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Reads data from an I2C/TWI slave device +// Refer to the "Wire Slave Sender" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) + Serial.begin(9600); // start serial for output +} + +void loop() +{ + Wire.requestFrom(2, 6); // request 6 bytes from slave device #2 + + while(Wire.available()) // slave may send less than requested + { + char c = Wire.read(); // receive a byte as character + Serial.print(c); // print the character + } + + delay(500); +} diff --git a/libs/arduino-1.0/libraries/Wire/examples/master_writer/master_writer.ino b/libs/arduino-1.0/libraries/Wire/examples/master_writer/master_writer.ino new file mode 100644 index 0000000..ccaa036 --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/master_writer/master_writer.ino @@ -0,0 +1,31 @@ +// Wire Master Writer +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Writes data to an I2C/TWI slave device +// Refer to the "Wire Slave Receiver" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(); // join i2c bus (address optional for master) +} + +byte x = 0; + +void loop() +{ + Wire.beginTransmission(4); // transmit to device #4 + Wire.write("x is "); // sends five bytes + Wire.write(x); // sends one byte + Wire.endTransmission(); // stop transmitting + + x++; + delay(500); +} diff --git a/libs/arduino-1.0/libraries/Wire/examples/slave_receiver/slave_receiver.ino b/libs/arduino-1.0/libraries/Wire/examples/slave_receiver/slave_receiver.ino new file mode 100644 index 0000000..60dd4bd --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/slave_receiver/slave_receiver.ino @@ -0,0 +1,38 @@ +// Wire Slave Receiver +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Receives data as an I2C/TWI slave device +// Refer to the "Wire Master Writer" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(4); // join i2c bus with address #4 + Wire.onReceive(receiveEvent); // register event + Serial.begin(9600); // start serial for output +} + +void loop() +{ + delay(100); +} + +// function that executes whenever data is received from master +// this function is registered as an event, see setup() +void receiveEvent(int howMany) +{ + while(1 < Wire.available()) // loop through all but the last + { + char c = Wire.read(); // receive byte as a character + Serial.print(c); // print the character + } + int x = Wire.read(); // receive byte as an integer + Serial.println(x); // print the integer +} diff --git a/libs/arduino-1.0/libraries/Wire/examples/slave_sender/slave_sender.ino b/libs/arduino-1.0/libraries/Wire/examples/slave_sender/slave_sender.ino new file mode 100644 index 0000000..d3b238a --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/examples/slave_sender/slave_sender.ino @@ -0,0 +1,32 @@ +// Wire Slave Sender +// by Nicholas Zambetti + +// Demonstrates use of the Wire library +// Sends data as an I2C/TWI slave device +// Refer to the "Wire Master Reader" example for use with this + +// Created 29 March 2006 + +// This example code is in the public domain. + + +#include + +void setup() +{ + Wire.begin(2); // join i2c bus with address #2 + Wire.onRequest(requestEvent); // register event +} + +void loop() +{ + delay(100); +} + +// function that executes whenever data is requested by master +// this function is registered as an event, see setup() +void requestEvent() +{ + Wire.write("hello "); // respond with message of 6 bytes + // as expected by master +} diff --git a/libs/arduino-1.0/libraries/Wire/keywords.txt b/libs/arduino-1.0/libraries/Wire/keywords.txt new file mode 100644 index 0000000..12f129b --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/keywords.txt @@ -0,0 +1,31 @@ +####################################### +# Syntax Coloring Map For Wire +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +beginTransmission KEYWORD2 +endTransmission KEYWORD2 +requestFrom KEYWORD2 +send KEYWORD2 +receive KEYWORD2 +onReceive KEYWORD2 +onRequest KEYWORD2 + +####################################### +# Instances (KEYWORD2) +####################################### + +Wire KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/libs/arduino-1.0/libraries/Wire/utility/twi.c b/libs/arduino-1.0/libraries/Wire/utility/twi.c new file mode 100644 index 0000000..6b2db3c --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/utility/twi.c @@ -0,0 +1,527 @@ +/* + twi.c - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts +*/ + +#include +#include +#include +#include +#include +#include +#include "Arduino.h" // for digitalWrite + +#ifndef cbi +#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif + +#ifndef sbi +#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +#include "pins_arduino.h" +#include "twi.h" + +static volatile uint8_t twi_state; +static volatile uint8_t twi_slarw; +static volatile uint8_t twi_sendStop; // should the transaction end with a stop +static volatile uint8_t twi_inRepStart; // in the middle of a repeated start + +static void (*twi_onSlaveTransmit)(void); +static void (*twi_onSlaveReceive)(uint8_t*, int); + +static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_masterBufferIndex; +static volatile uint8_t twi_masterBufferLength; + +static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_txBufferIndex; +static volatile uint8_t twi_txBufferLength; + +static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; +static volatile uint8_t twi_rxBufferIndex; + +static volatile uint8_t twi_error; + +/* + * Function twi_init + * Desc readys twi pins and sets twi bitrate + * Input none + * Output none + */ +void twi_init(void) +{ + // initialize state + twi_state = TWI_READY; + twi_sendStop = true; // default value + twi_inRepStart = false; + + // activate internal pullups for twi. + digitalWrite(SDA, 1); + digitalWrite(SCL, 1); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); + cbi(TWSR, TWPS1); + TWBR = ((F_CPU / TWI_FREQ) - 16) / 2; + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + // enable twi module, acks, and twi interrupt + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); +} + +/* + * Function twi_slaveInit + * Desc sets slave address and enables interrupt + * Input none + * Output none + */ +void twi_setAddress(uint8_t address) +{ + // set twi slave address (skip over TWGCE bit) + TWAR = address << 1; +} + +/* + * Function twi_readFrom + * Desc attempts to become twi bus master and read a + * series of bytes from a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes to read into array + * sendStop: Boolean indicating whether to send a stop at the end + * Output number of bytes read + */ +uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 0; + } + + // wait until twi is ready, become master receiver + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MRX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length-1; // This is not intuitive, read on... + // On receive, the previously configured ACK/NACK setting is transmitted in + // response to the received byte before the interrupt is signalled. + // Therefor we must actually set NACK when the _next_ to last byte is + // received, causing that NACK to be sent in response to receiving the last + // expected byte of data. + + // build sla+w, slave device address + w bit + twi_slarw = TW_READ; + twi_slarw |= address << 1; + + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent outselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + TWDR = twi_slarw; + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + + // wait for read operation to complete + while(TWI_MRX == twi_state){ + continue; + } + + if (twi_masterBufferIndex < length) + length = twi_masterBufferIndex; + + // copy twi buffer to data + for(i = 0; i < length; ++i){ + data[i] = twi_masterBuffer[i]; + } + + return length; +} + +/* + * Function twi_writeTo + * Desc attempts to become twi bus master and write a + * series of bytes to a device on the bus + * Input address: 7bit i2c device address + * data: pointer to byte array + * length: number of bytes in array + * wait: boolean indicating to wait for write or not + * sendStop: boolean indicating whether or not to send a stop at the end + * Output 0 .. success + * 1 .. length to long for buffer + * 2 .. address send, NACK received + * 3 .. data send, NACK received + * 4 .. other twi error (lost bus arbitration, bus error, ..) + */ +uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 1; + } + + // wait until twi is ready, become master transmitter + while(TWI_READY != twi_state){ + continue; + } + twi_state = TWI_MTX; + twi_sendStop = sendStop; + // reset error state (0xFF.. no error occured) + twi_error = 0xFF; + + // initialize buffer iteration vars + twi_masterBufferIndex = 0; + twi_masterBufferLength = length; + + // copy data to twi buffer + for(i = 0; i < length; ++i){ + twi_masterBuffer[i] = data[i]; + } + + // build sla+w, slave device address + w bit + twi_slarw = TW_WRITE; + twi_slarw |= address << 1; + + // if we're in a repeated start, then we've already sent the START + // in the ISR. Don't do it again. + // + if (true == twi_inRepStart) { + // if we're in the repeated start state, then we've already sent the start, + // (@@@ we hope), and the TWI statemachine is just waiting for the address byte. + // We need to remove ourselves from the repeated start state before we enable interrupts, + // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning + // up. Also, don't enable the START interrupt. There may be one pending from the + // repeated start that we sent outselves, and that would really confuse things. + twi_inRepStart = false; // remember, we're dealing with an ASYNC ISR + TWDR = twi_slarw; + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE); // enable INTs, but not START + } + else + // send start condition + TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA); // enable INTs + + // wait for write operation to complete + while(wait && (TWI_MTX == twi_state)){ + continue; + } + + if (twi_error == 0xFF) + return 0; // success + else if (twi_error == TW_MT_SLA_NACK) + return 2; // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + return 3; // error: data send, nack received + else + return 4; // other twi error +} + +/* + * Function twi_transmit + * Desc fills slave tx buffer with data + * must be called in slave tx event callback + * Input data: pointer to byte array + * length: number of bytes in array + * Output 1 length too long for buffer + * 2 not slave transmitter + * 0 ok + */ +uint8_t twi_transmit(const uint8_t* data, uint8_t length) +{ + uint8_t i; + + // ensure data will fit into buffer + if(TWI_BUFFER_LENGTH < length){ + return 1; + } + + // ensure we are currently a slave transmitter + if(TWI_STX != twi_state){ + return 2; + } + + // set length and copy data into tx buffer + twi_txBufferLength = length; + for(i = 0; i < length; ++i){ + twi_txBuffer[i] = data[i]; + } + + return 0; +} + +/* + * Function twi_attachSlaveRxEvent + * Desc sets function called before a slave read operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) ) +{ + twi_onSlaveReceive = function; +} + +/* + * Function twi_attachSlaveTxEvent + * Desc sets function called before a slave write operation + * Input function: callback function to use + * Output none + */ +void twi_attachSlaveTxEvent( void (*function)(void) ) +{ + twi_onSlaveTransmit = function; +} + +/* + * Function twi_reply + * Desc sends byte or readys receive line + * Input ack: byte indicating to ack or to nack + * Output none + */ +void twi_reply(uint8_t ack) +{ + // transmit master read ready signal, with or without ack + if(ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + }else{ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } +} + +/* + * Function twi_stop + * Desc relinquishes bus master status + * Input none + * Output none + */ +void twi_stop(void) +{ + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while(TWCR & _BV(TWSTO)){ + continue; + } + + // update twi state + twi_state = TWI_READY; +} + +/* + * Function twi_releaseBus + * Desc releases bus control + * Input none + * Output none + */ +void twi_releaseBus(void) +{ + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; +} + +SIGNAL(TWI_vect) +{ + switch(TW_STATUS){ + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if(twi_masterBufferIndex < twi_masterBufferLength){ + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + }else{ + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + } + break; + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if(twi_masterBufferIndex < twi_masterBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + if (twi_sendStop) + twi_stop(); + else { + twi_inRepStart = true; // we're gonna send the START + // don't enable the interrupt. We'll generate the start, but we + // avoid handling the interrupt until we're in the next transaction, + // at the point where we would normally issue the start. + TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ; + twi_state = TWI_READY; + } + break; + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + }else{ + // otherwise nack + twi_reply(0); + } + break; + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){ + twi_rxBuffer[twi_rxBufferIndex] = '\0'; + } + // sends ack and stops interface for clock stretching + twi_stop(); + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + // if they didn't change buffer & length, initialize it + if(0 == twi_txBufferLength){ + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + // transmit first byte from buffer, fall + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + // if there is more to send, ack, otherwise nack + if(twi_txBufferIndex < twi_txBufferLength){ + twi_reply(1); + }else{ + twi_reply(0); + } + break; + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + + // All + case TW_NO_INFO: // no state information + break; + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } +} + diff --git a/libs/arduino-1.0/libraries/Wire/utility/twi.h b/libs/arduino-1.0/libraries/Wire/utility/twi.h new file mode 100644 index 0000000..6526593 --- /dev/null +++ b/libs/arduino-1.0/libraries/Wire/utility/twi.h @@ -0,0 +1,53 @@ +/* + twi.h - TWI/I2C library for Wiring & Arduino + Copyright (c) 2006 Nicholas Zambetti. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef twi_h +#define twi_h + + #include + + //#define ATMEGA8 + + #ifndef TWI_FREQ + #define TWI_FREQ 100000L + #endif + + #ifndef TWI_BUFFER_LENGTH + #define TWI_BUFFER_LENGTH 32 + #endif + + #define TWI_READY 0 + #define TWI_MRX 1 + #define TWI_MTX 2 + #define TWI_SRX 3 + #define TWI_STX 4 + + void twi_init(void); + void twi_setAddress(uint8_t); + uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t); + uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t); + uint8_t twi_transmit(const uint8_t*, uint8_t); + void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) ); + void twi_attachSlaveTxEvent( void (*)(void) ); + void twi_reply(uint8_t); + void twi_stop(void); + void twi_releaseBus(void); + +#endif + diff --git a/libs/arduino-1.0/reference b/libs/arduino-1.0/reference new file mode 100644 index 0000000..f578879 --- /dev/null +++ b/libs/arduino-1.0/reference @@ -0,0 +1 @@ +../doc/arduino-core/reference \ No newline at end of file diff --git a/libs/arduino-1.0/tools/Mangler/src/Mangler.java b/libs/arduino-1.0/tools/Mangler/src/Mangler.java new file mode 100644 index 0000000..cfd5279 --- /dev/null +++ b/libs/arduino-1.0/tools/Mangler/src/Mangler.java @@ -0,0 +1,94 @@ +/* -*- mode: java; c-basic-offset: 2; indent-tabs-mode: nil -*- */ + +/* + Part of the Processing project - http://processing.org + + Copyright (c) 2008 Ben Fry and Casey Reas + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software Foundation, + Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +package com.transformers.supermangletron; + + +import java.text.SimpleDateFormat; +import java.util.Date; + +import javax.swing.JOptionPane; + +import processing.app.Editor; +import processing.app.tools.Tool; + + +/** + * Example Tools menu entry. + */ +public class Mangler implements Tool { + Editor editor; + + + public void init(Editor editor) { + this.editor = editor; + } + + + public String getMenuTitle() { + return "Mangle Selection"; + } + + + public void run() { + String sketchName = editor.getSketch().getName(); + + Object[] options = { "Yes, please", "No, thanks" }; + int result = JOptionPane.showOptionDialog(editor, + "Is " + sketchName + + " ready for destruction?", + "Super Mangle Tron", + JOptionPane.YES_NO_OPTION, + JOptionPane.QUESTION_MESSAGE, + null, + options, + options[1]); + if (result == JOptionPane.YES_OPTION) { + mangleSelection(); + } + } + + + protected void mangleSelection() { + if (editor.isSelectionActive()) { + String selection = editor.getSelectedText(); + char[] stuff = selection.toCharArray(); + // Randomly swap a bunch of characters in the text + for (int i = 0; i < stuff.length / 10; i++) { + int a = (int) (Math.random() * stuff.length); + int b = (int) (Math.random() * stuff.length); + if (stuff[a] == '\n' || stuff[b] == '\n') { + continue; // skip newline characters + } + stuff[a] = selection.charAt(b); + stuff[b] = selection.charAt(a); + } + editor.startCompoundEdit(); + editor.setSelectedText(new String(stuff)); + editor.stopCompoundEdit(); + editor.statusNotice("Now that feels better, doesn't it?"); + + } else { + editor.statusError("No selection, no dice."); + } + } +} diff --git a/libs/arduino-1.0/tools/howto.txt b/libs/arduino-1.0/tools/howto.txt new file mode 100644 index 0000000..77df31c --- /dev/null +++ b/libs/arduino-1.0/tools/howto.txt @@ -0,0 +1,143 @@ +TOOLS IN PROCESSING + +With initial help from code contributed by fjen, Processing release 0147 and +later have a dynamically loading tools menu, which can be used to expand the +environment in fun and fantastic ways. + +A Tool is a chunk of code that runs from the Tools menu. Tools are a means +of building onto the Processing Development Environment without needing to +rebuild the beast from source. + +The interface (at least for now) is extremely simple: + + +package processing.app.tools.Tool; + +public interface Tool extends Runnable { + + public void init(Editor editor); + + public void run(); + + public String getMenuTitle(); +} + + +The init() method is called when an Editor window first opens. This means +you won't have access to a sketch object, or a GUI, and should only do minimal +setup. (However it'd be a good idea to stash the "Editor" object for later.) + +The run() method will be called by the main application when the tool is +selected from the menu. This is called using invokeLater(), so that the tool +can safely use Swing and any other GUI yackety yack. If you're using a Frame, +you'll need to detect whether the Frame is already open (and bring it to the +front) or whether to create a new window. + +Faceless tools also use the run() method. You should avail yourselves of the +statusNotice() and statusError() methods in Editor, to let the user know what's +happened. (As per p. 107 of the Processing Development Environment Tools +Reference User Interface Guide.) + +The getMenuTitle() method just returns the title for what should appear in the +Tools menu. Not doing shortcuts for now, because resolving them between tools +(and the rest of the interface) is fugly. We would also need additional +modifiers for shift and alt. It just gets messy quick. Ordering in the Tools +menu is alphabetical. + + +////////////////////////////////////////////////////////////// + + +Where to put Tools + +Core tools live inside the "tools" subfolder of the Processing distribution, +however users should install "contributed" tools in their sketchbook folder, +inside a subfolder named "tools". + +If a tool works only with a particular release of Processing, then it may make +sense for the user to put things into the Processing tools folder, however we'd +like to keep users out of there as much as possible. In fact, it may not be +visible in future releases of Processing (for instance, on Mac OS X, the tools +folder is hidden inside the .app bundle). + +Tools should be laid out similar to libraries, though the structure is a little +more flexible. The tool folder should be the name of the main class (without +its package name but using the same capitalization), and have a subfolder named +"tool" that contains the .jar and .zip files it uses. I'll use the included +"Mangler" tool as an example. + +(This Tool is not built by default, due to a lack of non-dubious arguments +regarding the usefulness of including (by default) a Tool that mangles code.) + +The folder should be called Mangler (note the capitalization), and contain: + +sketchbook/Mangler -> tool folder +sketchbook/Mangler/tool -> location for code +sketchbook/Mangler/tool/mangle.jar -> jar with one or more classes + +The naming of jar and zip files in the tool/* directory doesn't matter. + +When Processing loads, the jar and zip files will be searched for +Mangler.class. Even though this tool is found in package poos.shoe, +it will be sussed out. Package names are required. + +Loose .class files are not supported, use only jar and zip files. + + +////////////////////////////////////////////////////////////// + + +What You Can and Cannot Do + +The only API methods that are officially scrubbed and sanctioned by the +Commissioner on Fair API and Proper Manners for use by the Tools classes +are found in: + +processing.app.Base +processing.app.Editor +processing.app.Preferences +processing.app.Sketch +processing.app.SketchCode + +In fact, most of the API you should be talking to is inside Editor. +Full API documentation can be found on dev.processing.org: +http://dev.processing.org/reference/everything/ +(Keep in mind that this is not always perfectly up to date, but we'll try.) + +Of course, you're welcome to go spelunking through the rest of the API +(that's where all the fun stuff is anyway), but don't be upset when something +changes and breaks your tool and makes your users sad. + +Currently, native code is not supported with tools. This might be possible, +but it's another potentially messy thing to dynamically add native library +paths to running code. (See "Future Releases" below.) + + +////////////////////////////////////////////////////////////// + + +Future Releases + +In future releases, we are considering the following features: + +1. How shortcut keys are handled. + http://dev.processing.org/bugs/show_bug.cgi?id=140 + +2. Whether to allow tools to dock into the Preferences panel. + http://dev.processing.org/bugs/show_bug.cgi?id=883 + +3. A means to run native code from the Tools menu. + http://dev.processing.org/bugs/show_bug.cgi?id=884 + +4. Methods for reorganizing the Tools menu, or placing contributed + Tools inside other menus (such as Edit or Sketch). + http://dev.processing.org/bugs/show_bug.cgi?id=885 + +This is the first round of documentation for the Tools menu, we reserve the +right to update, clarify, and change our mind in future releases. + + +////////////////////////////////////////////////////////////// + + +Ben Fry, last updated 19 August 2008 diff --git a/libs/dynamixel/.gitignore b/libs/dynamixel/.gitignore new file mode 100644 index 0000000..103a90e --- /dev/null +++ b/libs/dynamixel/.gitignore @@ -0,0 +1,5 @@ +lib +src_lib/dxl_hal.o +src_lib/dynamixel.o +src_lib/libdxl.a +src_lib/libusb2ax.so diff --git a/libs/dynamixel/LICENSE.txt b/libs/dynamixel/LICENSE.txt new file mode 100644 index 0000000..936884b --- /dev/null +++ b/libs/dynamixel/LICENSE.txt @@ -0,0 +1,26 @@ +Software License Agreement (BSD License) + +Copyright (c) 2014, ROBOTIS Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of ROBOTIS nor the names of its contributors may be + used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY ROBOTIS "AS IS" AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL ROBOTIS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/libs/dynamixel/README.txt b/libs/dynamixel/README.txt new file mode 100644 index 0000000..780d212 --- /dev/null +++ b/libs/dynamixel/README.txt @@ -0,0 +1,12 @@ +DynamixelSDK_sync (with SYNC_READ and SYNC_WRITE) +================================================= + +This patched version of the DynamixelSDK has functions to use the full potential of the USB2AX, with easy access to SYNC_READ and SYNC_WRITE. +This works with all versions of the SDK (but on Linux you need to use the linux_compatibility patch at the same time for the library to work with the USB2AX). + +Note: SYNC_WRITE would work with any other hardware interface (like the USB2Dynamixel) as it is part of the original protocol, but SYNC_READ only works with the USB2AX and some other specialized third party controllers. + +How to use in your application: +- replace all the files in the DynamixelSDK +- recompile the dynamixel library +- recompile the application diff --git a/libs/dynamixel/bin/dynamixel.dll b/libs/dynamixel/bin/dynamixel.dll new file mode 100644 index 0000000..4db27ed Binary files /dev/null and b/libs/dynamixel/bin/dynamixel.dll differ diff --git a/libs/dynamixel/example/ReadWrite/Makefile b/libs/dynamixel/example/ReadWrite/Makefile new file mode 100644 index 0000000..9f8a5d1 --- /dev/null +++ b/libs/dynamixel/example/ReadWrite/Makefile @@ -0,0 +1,23 @@ +TARGET = ReadWrite +OBJS = ReadWrite.o +SRCS = $(OBJS:.o=.c) +INCLUDEDIRS += -I../../include +LIBDIRS += -L../../lib +CFLAGS = $(INCLUDEDIRS) -W -Wall -O2 +LDFLAGS = $(LIBDIRS) -lm -ldxl + +CC = gcc + +$(TARGET): $(OBJS) + $(CC) -o $@ $^ $(LDFLAGS) + +.c.o: + $(CC) -c $< $(CFLAGS) + +clean: + rm -f $(OBJS) $(TARGET) + @echo "file deleted." + +dep: + gccmakedep $(SRCS) + diff --git a/libs/dynamixel/example/ReadWrite/ReadWrite b/libs/dynamixel/example/ReadWrite/ReadWrite new file mode 100644 index 0000000..abeb64b Binary files /dev/null and b/libs/dynamixel/example/ReadWrite/ReadWrite differ diff --git a/libs/dynamixel/example/ReadWrite/ReadWrite.c b/libs/dynamixel/example/ReadWrite/ReadWrite.c new file mode 100644 index 0000000..3f773f4 --- /dev/null +++ b/libs/dynamixel/example/ReadWrite/ReadWrite.c @@ -0,0 +1,124 @@ +//########################################################## +//## R O B O T I S ## +//## ReadWrite Example code for Dynamixel. ## +//## 2009.11.10 ## +//########################################################## +#include +#include +#include +#include + +// Control table address +#define P_GOAL_POSITION_L 30 +#define P_GOAL_POSITION_H 31 +#define P_PRESENT_POSITION_L 36 +#define P_PRESENT_POSITION_H 37 +#define P_MOVING 46 + +// Defulat setting +#define DEFAULT_BAUDNUM 1 // 1Mbps +#define DEFAULT_ID 2 + +void PrintCommStatus(int CommStatus); +void PrintErrorCode(void); + +int main() +{ + int baudnum = 1; + int GoalPos[2] = {0, 1023}; + //int GoalPos[2] = {0, 4095}; // for Ex series + int index = 0; + int deviceIndex = 0; + int Moving, PresentPos; + int CommStatus; + + printf( "\n\nRead/Write example for Linux\n\n" ); + ///////// Open USB2Dynamixel //////////// + if( dxl_initialize(deviceIndex, baudnum) == 0 ) + { + printf( "Failed to open USB2Dynamixel!\n" ); + printf( "Press Enter key to terminate...\n" ); + getchar(); + return 0; + } + else + printf( "Succeed to open USB2Dynamixel!\n" ); + + dxl_write_word(2, 34, 1000); + usleep(5000); + dxl_write_word(3, 34, 1000); + usleep(5000); + + while(1) + { + // Write goal position + dxl_write_word(2, P_GOAL_POSITION_L, 200); + usleep(5000); + dxl_write_word(3, P_GOAL_POSITION_L, 200); + sleep(1); + dxl_write_word(2, P_GOAL_POSITION_L, 1000); + usleep(5000); + dxl_write_word(3, P_GOAL_POSITION_L, 1000); + sleep(1); + } + return 0; +} +// Print communication result +void PrintCommStatus(int CommStatus) +{ + switch(CommStatus) + { + case COMM_TXFAIL: + printf("COMM_TXFAIL: Failed transmit instruction packet!\n"); + break; + + case COMM_TXERROR: + printf("COMM_TXERROR: Incorrect instruction packet!\n"); + break; + + case COMM_RXFAIL: + printf("COMM_RXFAIL: Failed get status packet from device!\n"); + break; + + case COMM_RXWAITING: + printf("COMM_RXWAITING: Now recieving status packet!\n"); + break; + + case COMM_RXTIMEOUT: + printf("COMM_RXTIMEOUT: There is no status packet!\n"); + break; + + case COMM_RXCORRUPT: + printf("COMM_RXCORRUPT: Incorrect status packet!\n"); + break; + + default: + printf("This is unknown error code!\n"); + break; + } +} + +// Print error bit of status packet +void PrintErrorCode() +{ + if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1) + printf("Input voltage error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1) + printf("Angle limit error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1) + printf("Overheat error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1) + printf("Out of range error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1) + printf("Checksum error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1) + printf("Overload error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1) + printf("Instruction code error!\n"); +} diff --git a/libs/dynamixel/example/ReadWrite/ReadWrite.o b/libs/dynamixel/example/ReadWrite/ReadWrite.o new file mode 100644 index 0000000..3d9ff31 Binary files /dev/null and b/libs/dynamixel/example/ReadWrite/ReadWrite.o differ diff --git a/libs/dynamixel/example/SyncWrite/Makefile b/libs/dynamixel/example/SyncWrite/Makefile new file mode 100644 index 0000000..6214d7f --- /dev/null +++ b/libs/dynamixel/example/SyncWrite/Makefile @@ -0,0 +1,23 @@ +TARGET = test +OBJS = test.o +SRCS = $(OBJS:.o=.c) +INCLUDEDIRS += -I../../include +LIBDIRS += -L../../lib +CFLAGS = $(INCLUDEDIRS) -W -Wall -O2 +LDFLAGS = $(LIBDIRS) -lm -ldxl + +CC = gcc + +$(TARGET): $(OBJS) + $(CC) -o $@ $^ $(LDFLAGS) + +.c.o: + $(CC) -c $< $(CFLAGS) + +clean: + rm -f $(OBJS) $(TARGET) + @echo "file deleted." + +dep: + gccmakedep $(SRCS) + diff --git a/libs/dynamixel/example/SyncWrite/SyncWrite.c b/libs/dynamixel/example/SyncWrite/SyncWrite.c new file mode 100644 index 0000000..15ea9ea --- /dev/null +++ b/libs/dynamixel/example/SyncWrite/SyncWrite.c @@ -0,0 +1,178 @@ +//########################################################## +//## R O B O T I S ## +//## SyncWrite Example code for Dynamixel. ## +//## 2009.11.10 ## +//########################################################## +#include +#include +#include +#include + +#include + +#define PI 3.141592f +#define NUM_ACTUATOR 3 + +// Control table address +#define P_GOAL_POSITION_L 30 +#define P_GOAL_POSITION_H 31 +#define P_GOAL_SPEED_L 32 +#define P_GOAL_SPEED_H 33 + +// Defulat setting +#define DEFAULT_BAUDNUM 1 // 1Mbps +#define NUM_ACTUATOR 3 // Number of actuator +#define STEP_THETA (PI / 100.0f) // Large value is more fast +#define CONTROL_PERIOD (10000) // usec (Large value is more slow) + +void PrintCommStatus(int CommStatus); +void PrintErrorCode(void); + +int main() +{ + int id[NUM_ACTUATOR]; + int baudnum = 1; + int deviceIndex = 0; + float phase[NUM_ACTUATOR]; + float theta = 0; + int AmpPos = 512; + //int AmpPos = 2048; // for EX series + int GoalPos; + int i; + int CommStatus; + printf( "\n\nSyncWrite example for Linux\n\n" ); + + // Initialize id and phase + for( i=0; i +#include +#include +#include + +#include + +#define P_GOAL_POSITION_L 30 +#define P_SPEED 0x26 +#define P_COUPLE 34 + +// Default setting +#define DEFAULT_PORTNUM 0 // COM3 +#define DEFAULT_BAUDNUM 1 // 1Mbps +#define DEFAULT_ID 3// ID AX12 + +#if defined(WIN32) || defined(_WIN32) +#define EXPORT __declspec(dllexport) +#else +#define EXPORT +#endif + +#define MARGE_POS 20 +#define MARGE_POS_MVT 5 +#define true 1 +#define false 0 + +int pos_AX12_3 = 150*1024/300; +int pos_AX12_2 = 150*1024/300; +int arrive = false; +int arrive2 = false; + +void loopAX12_3(){ + static int pos = 0; + int speed = dxl_read_word(3, P_SPEED); + + // Si il est pas à la bonne position + if(pos < pos_AX12_3 - MARGE_POS || pos > pos_AX12_3 + MARGE_POS) { + arrive = false; + // Si il bouge pas, on renvoie l'ordre + if(speed == 0) { + dxl_write_word(3, P_GOAL_POSITION_L, pos_AX12_3); + } + else { + pos = dxl_read_word(3, 36); + + } + } + else { + if(!arrive) { + arrive = true; + printf("[3] arrivé !\n"); + } + } +} +void loopAX12_2(){ + static int pos = 0; + int speed = dxl_read_word(2, P_SPEED); + + // Si il est pas à la bonne position + if(pos < pos_AX12_2 - MARGE_POS || pos > pos_AX12_2 + MARGE_POS) { + arrive2 = false; + // Si il bouge pas, on renvoie l'ordre + if(speed == 0) { + dxl_write_word(2, P_GOAL_POSITION_L, pos_AX12_2); + } + else { + pos = dxl_read_word(2, 36); + + } + } + else { + if(!arrive2) { + arrive2 = true; + printf("[2] arrivé !\n"); + } + } +} + +// EXPORT int init(int port_num, int baudrate) { +// return dxl_initialize(port_num, baudrate) <= 0; +// } + +// EXPORT void + +int main() +{ + dxl_write_word(3, P_COUPLE, 800); + dxl_write_word(2, P_COUPLE, 800); + + while (++i) + { + //movesync(id1, id2, pos_id1, pos_id2) ==> permet de renvoyer le nombre d'envoie d'ordre pour atteindre une position donnée (détection des bugs) + // movePos(3, 150*1024/300); + // dxl_write_word(3, P_GOAL_POSITION_L, (150*1024/300)); + // usleep(300000); + // movePos(3, 220*1024/300); + // dxl_write_word(3, P_GOAL_POSITION_L, (220*1024/300)); + // usleep(300000); + // printf("loop\n"); + loopAX12_2(); + usleep(1000); + loopAX12_3(); + usleep(50000); + if(i > 20) { + if(pos_AX12_3 == (int) 240*1024/300) { + pos_AX12_3 = 150*1024/300; + pos_AX12_2 = 150*1024/300; + } + else { + pos_AX12_3 = 240*1024/300; + pos_AX12_2 = 60*1024/300; + } + i = 0; + // printf("change pos_AX12_3 = %d\n", pos_AX12_3); + } + } + + return 0; +} + diff --git a/libs/dynamixel/example/SyncWrite/test.o b/libs/dynamixel/example/SyncWrite/test.o new file mode 100644 index 0000000..609d7ad Binary files /dev/null and b/libs/dynamixel/example/SyncWrite/test.o differ diff --git a/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.cpp b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.cpp new file mode 100644 index 0000000..7f4d362 --- /dev/null +++ b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.cpp @@ -0,0 +1,171 @@ +// Windows version +#include +#include +#include +#include +#include + +#pragma comment(lib, "dynamixel.lib") + + +// Control table address +#define P_GOAL_POSITION_L 30 +#define P_GOAL_POSITION_H 31 +#define P_GOAL_SPEED_L 32 +#define P_GOAL_SPEED_H 33 +#define P_PRESENT_POSITION_L 36 + +// Defulat setting +#define DEFAULT_PORTNUM 120 // COM3 +#define DEFAULT_BAUDNUM 1 // 1Mbps + + +#define NB_ACTUATOR 8 // Number of actuator +//int ids[NB_ACTUATOR] = { 1, 2, 3, 4, 5, 6, 7, 8 }; +int ids[NB_ACTUATOR] = { 50, 51, 52, 53, 60, 61, 62, 63 }; +int nb_to_move = NB_ACTUATOR; + +void PrintCommStatus(int CommStatus); +void PrintErrorCode(); + + int CommStatus; + +void sync_write_all(int pos){ + dxl_sync_write_start( P_GOAL_POSITION_L, 2 ); + for (int i = 0; i < nb_to_move; i++ ){ + dxl_sync_write_push_id( ids[i] ); + dxl_sync_write_push_word( pos ); + } + dxl_sync_write_send(); +} + +void sync_read (){ + dxl_sync_read_start( P_PRESENT_POSITION_L, 2 ); + for (int i = 0; i < nb_to_move; i++ ){ + dxl_sync_read_push_id( ids[i] ); + } + dxl_sync_read_send(); + CommStatus = dxl_get_result(); + if( CommStatus == COMM_RXSUCCESS ) + { + for (int i = 0; i < nb_to_move; i++ ){ + printf( "%i=%i, ", ids[i] , dxl_sync_read_pop_word()); + } + } + else + { + PrintCommStatus(CommStatus); + } + + printf("\n"); + +} + +void move_all_to(int pos){ + printf( "Set Goal_position = %i\n", pos ); + sync_write_all(pos); + for (int m = 0; m<100; m++ ){ + sync_read(); + } + Sleep( 200 ); +} + + + + +int main() +{ + // Open device + if( dxl_initialize(DEFAULT_PORTNUM, DEFAULT_BAUDNUM) == 0 ) + { + printf( "Failed to open USB2Dynamixel!\n" ); + printf( "Press any key to terminate...\n" ); + _getch(); + return 0; + } + else + printf( "Succeed to open USB2Dynamixel!\n" ); + + // Set goal speed + dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 ); + // Set goal position + dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, 512 ); + + while(1) + { + printf( "Press any key to continue!(press ESC to quit)\n" ); + if(_getch() == 0x1b) + break; + + move_all_to(470); + move_all_to(560); + printf( "\n" ); + } + + dxl_terminate(); + printf( "Press any key to terminate...\n" ); + _getch(); + + return 0; +} + + +// Print communication result +void PrintCommStatus(int CommStatus) +{ + switch(CommStatus) + { + case COMM_TXFAIL: + printf("COMM_TXFAIL: Failed transmit instruction packet!\n"); + break; + + case COMM_TXERROR: + printf("COMM_TXERROR: Incorrect instruction packet!\n"); + break; + + case COMM_RXFAIL: + printf("COMM_RXFAIL: Failed get status packet from device!\n"); + break; + + case COMM_RXWAITING: + printf("COMM_RXWAITING: Now recieving status packet!\n"); + break; + + case COMM_RXTIMEOUT: + printf("COMM_RXTIMEOUT: There is no status packet!\n"); + break; + + case COMM_RXCORRUPT: + printf("COMM_RXCORRUPT: Incorrect status packet!\n"); + break; + + default: + printf("This is unknown error code!\n"); + break; + } +} + +// Print error bit of status packet +void PrintErrorCode() +{ + if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1) + printf("Input voltage error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1) + printf("Angle limit error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1) + printf("Overheat error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1) + printf("Out of range error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1) + printf("Checksum error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1) + printf("Overload error!\n"); + + if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1) + printf("Instruction code error!\n"); +} \ No newline at end of file diff --git a/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.sln b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.sln new file mode 100644 index 0000000..7cbdea4 --- /dev/null +++ b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.sln @@ -0,0 +1,29 @@ + +Microsoft Visual Studio Solution File, Format Version 10.00 +# Visual Studio 2008 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "SyncRead", "SyncRead.vcproj", "{4F3663DE-A9AF-46FE-B236-513F9C47D4FA}" + ProjectSection(ProjectDependencies) = postProject + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E} = {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Dynamixel_DLL", "..\..\..\..\src\Dynamixel_DLL\Dynamixel_DLL.vcproj", "{FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Release|Win32 = Release|Win32 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4F3663DE-A9AF-46FE-B236-513F9C47D4FA}.Debug|Win32.ActiveCfg = Debug|Win32 + {4F3663DE-A9AF-46FE-B236-513F9C47D4FA}.Debug|Win32.Build.0 = Debug|Win32 + {4F3663DE-A9AF-46FE-B236-513F9C47D4FA}.Release|Win32.ActiveCfg = Release|Win32 + {4F3663DE-A9AF-46FE-B236-513F9C47D4FA}.Release|Win32.Build.0 = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.ActiveCfg = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.Build.0 = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.ActiveCfg = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.vcproj b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.vcproj new file mode 100644 index 0000000..c5cc196 --- /dev/null +++ b/libs/dynamixel/example/win32/visual c++/SyncRead/SyncRead.vcproj @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/libs/dynamixel/import/dynamixel.def b/libs/dynamixel/import/dynamixel.def new file mode 100644 index 0000000..2b2d012 --- /dev/null +++ b/libs/dynamixel/import/dynamixel.def @@ -0,0 +1,34 @@ +LIBRARY "dynamixel" + +EXPORTS + dxl_initialize + dxl_terminate + dxl_get_result + dxl_tx_packet + dxl_rx_packet + dxl_txrx_packet + dxl_set_txpacket_id + dxl_set_txpacket_instruction + dxl_set_txpacket_parameter + dxl_set_txpacket_length + dxl_get_rxpacket_error + dxl_get_rxpacket_length + dxl_get_rxpacket_parameter + dxl_makeword + dxl_get_lowbyte + dxl_get_highbyte + dxl_ping + dxl_read_byte + dxl_write_byte + dxl_read_word + dxl_write_word + dxl_sync_write_start + dxl_sync_write_push_id + dxl_sync_write_push_byte + dxl_sync_write_push_word + dxl_sync_write_send + dxl_sync_read_start + dxl_sync_read_push_id + dxl_sync_read_send + dxl_sync_read_pop_byte + dxl_sync_read_pop_word \ No newline at end of file diff --git a/libs/dynamixel/import/dynamixel.h b/libs/dynamixel/import/dynamixel.h new file mode 100644 index 0000000..041222b --- /dev/null +++ b/libs/dynamixel/import/dynamixel.h @@ -0,0 +1,97 @@ +#ifndef _DYNAMIXEL_HEADER +#define _DYNAMIXEL_HEADER + + +#ifdef __cplusplus +extern "C" { +#endif + + +///////////// device control methods //////////////////////// +int __stdcall dxl_initialize( int deviceIndex, int baudnum ); +void __stdcall dxl_terminate(); + + +///////////// set/get packet methods ////////////////////////// +#define MAXNUM_TXPARAM (150) +#define MAXNUM_RXPARAM (225) + +void __stdcall dxl_set_txpacket_id( int id ); +#define BROADCAST_ID (254) + +void __stdcall dxl_set_txpacket_instruction( int instruction ); +#define INST_PING (1) +#define INST_READ (2) +#define INST_WRITE (3) +#define INST_REG_WRITE (4) +#define INST_ACTION (5) +#define INST_RESET (6) +#define INST_SYNC_WRITE (131) +#define INST_SYNC_READ (132) + +void __stdcall dxl_set_txpacket_parameter( int index, int value ); +void __stdcall dxl_set_txpacket_length( int length ); + +int __stdcall dxl_get_rxpacket_error( int errbit ); +#define ERRBIT_VOLTAGE (1) +#define ERRBIT_ANGLE (2) +#define ERRBIT_OVERHEAT (4) +#define ERRBIT_RANGE (8) +#define ERRBIT_CHECKSUM (16) +#define ERRBIT_OVERLOAD (32) +#define ERRBIT_INSTRUCTION (64) + +int __stdcall dxl_get_rxpacket_length( void ); +int __stdcall dxl_get_rxpacket_parameter( int index ); + + +// utility for value +int __stdcall dxl_makeword( int lowbyte, int highbyte ); +int __stdcall dxl_get_lowbyte( int word ); +int __stdcall dxl_get_highbyte( int word ); + + +////////// packet communication methods /////////////////////// +void __stdcall dxl_tx_packet( void ); +void __stdcall dxl_rx_packet( void ); +void __stdcall dxl_txrx_packet( void ); + +int __stdcall dxl_get_result( void ); +#define COMM_TXSUCCESS (0) +#define COMM_RXSUCCESS (1) +#define COMM_TXFAIL (2) +#define COMM_RXFAIL (3) +#define COMM_TXERROR (4) +#define COMM_RXWAITING (5) +#define COMM_RXTIMEOUT (6) +#define COMM_RXCORRUPT (7) + + +//////////// high communication methods /////////////////////// +void __stdcall dxl_ping( int id ); +int __stdcall dxl_read_byte( int id, int address ); +void __stdcall dxl_write_byte( int id, int address, int value ); +int __stdcall dxl_read_word( int id, int address ); +void __stdcall dxl_write_word( int id, int address, int value ); + +//////////// Synchroneous communication methods /////////////////////// +void __stdcall dxl_sync_write_start( int address, int data_length ); +void __stdcall dxl_sync_write_push_id( int id ); +void __stdcall dxl_sync_write_push_byte( int value ); +void __stdcall dxl_sync_write_push_word( int value ); +void __stdcall dxl_sync_write_send(); + +void __stdcall dxl_sync_read_start( int address, int data_length ); +void __stdcall dxl_sync_read_push_id( int id ); +void __stdcall dxl_sync_read_send(); +//void __stdcall dxl_sync_read_noblock_send(); +//void __stdcall dxl_sync_read_noblock_receive(); +int __stdcall dxl_sync_read_pop_byte(); +int __stdcall dxl_sync_read_pop_word(); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libs/dynamixel/import/dynamixel.lib b/libs/dynamixel/import/dynamixel.lib new file mode 100644 index 0000000..26d9b00 Binary files /dev/null and b/libs/dynamixel/import/dynamixel.lib differ diff --git a/libs/dynamixel/include/dynamixel.h b/libs/dynamixel/include/dynamixel.h new file mode 100644 index 0000000..3bdacde --- /dev/null +++ b/libs/dynamixel/include/dynamixel.h @@ -0,0 +1,97 @@ +#ifndef _DYNAMIXEL_HEADER +#define _DYNAMIXEL_HEADER + + +#ifdef __cplusplus +extern "C" { +#endif + + +///////////// device control methods //////////////////////// +int dxl_initialize( int deviceIndex, int baudnum ); +void dxl_terminate(); + + +///////////// set/get packet methods ////////////////////////// +#define MAXNUM_TXPARAM (150) +#define MAXNUM_RXPARAM (225) + +void dxl_set_txpacket_id( int id ); +#define BROADCAST_ID (254) + +void dxl_set_txpacket_instruction( int instruction ); +#define INST_PING (1) +#define INST_READ (2) +#define INST_WRITE (3) +#define INST_REG_WRITE (4) +#define INST_ACTION (5) +#define INST_RESET (6) +#define INST_SYNC_WRITE (131) +#define INST_SYNC_READ (132) + +void dxl_set_txpacket_parameter( int index, int value ); +void dxl_set_txpacket_length( int length ); + +int dxl_get_rxpacket_error( int errbit ); +#define ERRBIT_VOLTAGE (1) +#define ERRBIT_ANGLE (2) +#define ERRBIT_OVERHEAT (4) +#define ERRBIT_RANGE (8) +#define ERRBIT_CHECKSUM (16) +#define ERRBIT_OVERLOAD (32) +#define ERRBIT_INSTRUCTION (64) + +int dxl_get_rxpacket_length( void ); +int dxl_get_rxpacket_parameter( int index ); + + +// utility for value +int dxl_makeword( int lowbyte, int highbyte ); +int dxl_get_lowbyte( int word ); +int dxl_get_highbyte( int word ); + + +////////// packet communication methods /////////////////////// +void dxl_tx_packet( void ); +void dxl_rx_packet( void ); +void dxl_txrx_packet( void ); + +int dxl_get_result( void ); +#define COMM_TXSUCCESS (0) +#define COMM_RXSUCCESS (1) +#define COMM_TXFAIL (2) +#define COMM_RXFAIL (3) +#define COMM_TXERROR (4) +#define COMM_RXWAITING (5) +#define COMM_RXTIMEOUT (6) +#define COMM_RXCORRUPT (7) + + +//////////// high communication methods /////////////////////// +void dxl_ping( int id ); +int dxl_read_byte( int id, int address ); +void dxl_write_byte( int id, int address, int value ); +int dxl_read_word( int id, int address ); +void dxl_write_word( int id, int address, int value ); + +//////////// Synchroneous communication methods /////////////////////// +void dxl_sync_write_start( int address, int data_length ); +void dxl_sync_write_push_id( int id ); +void dxl_sync_write_push_byte( int value ); +void dxl_sync_write_push_word( int value ); +void dxl_sync_write_send(); + +void dxl_sync_read_start( int address, int data_length ); +void dxl_sync_read_push_id( int id ); +void dxl_sync_read_send(); +//void dxl_sync_read_noblock_send(); +//void dxl_sync_read_noblock_receive(); +int dxl_sync_read_pop_byte(); +int dxl_sync_read_pop_word(); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.sln b/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.sln new file mode 100644 index 0000000..22af500 --- /dev/null +++ b/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.sln @@ -0,0 +1,26 @@ + +Microsoft Visual Studio Solution File, Format Version 10.00 +# Visual Studio 2008 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Dynamixel_DLL", "Dynamixel_DLL.vcproj", "{FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Debug|x64 = Debug|x64 + Release|Win32 = Release|Win32 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.ActiveCfg = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.Build.0 = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.ActiveCfg = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.Build.0 = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.ActiveCfg = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.Build.0 = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.ActiveCfg = Release|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.vcproj b/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.vcproj new file mode 100644 index 0000000..287fcee --- /dev/null +++ b/libs/dynamixel/src/Dynamixel_DLL/Dynamixel_DLL.vcproj @@ -0,0 +1,387 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/libs/dynamixel/src/Makefile b/libs/dynamixel/src/Makefile new file mode 100644 index 0000000..3685822 --- /dev/null +++ b/libs/dynamixel/src/Makefile @@ -0,0 +1,24 @@ +TARGET = libdxl.a +OBJS = dxl_hal.o dynamixel.o +SRCS = $(OBJS:.o=.c) +INCLUDEDIRS += -I../include +LIBDIRS += +CFLAGS = $(INCLUDEDIRS) -W -Wall -O2 + +CC = gcc +AR = ar + +$(TARGET): $(OBJS) + $(AR) rs $@ $^ + cp $(TARGET) ../lib + +.c.o: + $(CC) -c $< $(CFLAGS) + +clean: + rm -f $(OBJS) $(TARGET) + @echo "file deleted." + +dep: + gccmakedep $(SRCS) + diff --git a/libs/dynamixel/src/dxl_hal.c b/libs/dynamixel/src/dxl_hal.c new file mode 100644 index 0000000..ee18708 --- /dev/null +++ b/libs/dynamixel/src/dxl_hal.c @@ -0,0 +1,179 @@ +/** +Modification of the HAL of the Dynamixel SDK to be used with USB2AX. + +Nicolas Saugnier +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dxl_hal.h" + +int gSocket_fd = -1; +long glStartTime = 0; +float gfRcvWaitTime = 0.0f; +float gfByteTransTime = 0.0f; + +char gDeviceName[20]; + +int dxl_hal_open(int deviceIndex, float baudrate) +{ + struct termios newtio; + //struct serial_struct serinfo; + char dev_name[100] = {0, }; + + sprintf(dev_name, "/dev/ttyACM%d", deviceIndex); // USB2AX is ttyACM + + strcpy(gDeviceName, dev_name); + memset(&newtio, 0, sizeof(newtio)); + dxl_hal_close(); + + if((gSocket_fd = open(gDeviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) { + fprintf(stderr, "device open error: %s\n", dev_name); + goto DXL_HAL_OPEN_ERROR; + } + + newtio.c_cflag = B1000000|CS8|CLOCAL|CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; // time-out 값 (TIME * 0.1초) 0 : disable + newtio.c_cc[VMIN] = 0; // MIN 은 read 가 return 되기 위한 최소 문자 개수 + + tcflush(gSocket_fd, TCIFLUSH); + tcsetattr(gSocket_fd, TCSANOW, &newtio); + + if(gSocket_fd == -1) + return 0; + + //USB2AX uses the CDC ACM driver for which these settings do not exist. + /* + if(ioctl(gSocket_fd, TIOCGSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot get serial info\n"); + return 0; + } + + serinfo.flags &= ~ASYNC_SPD_MASK; + serinfo.flags |= ASYNC_SPD_CUST; + serinfo.custom_divisor = serinfo.baud_base / baudrate; + + if(ioctl(gSocket_fd, TIOCSSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot set serial info\n"); + return 0; + }*/ + + dxl_hal_close(); + + gfByteTransTime = (float)((1000.0f / baudrate) * 12.0f); + + strcpy(gDeviceName, dev_name); + memset(&newtio, 0, sizeof(newtio)); + dxl_hal_close(); + + if((gSocket_fd = open(gDeviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) { + fprintf(stderr, "device open error: %s\n", dev_name); + goto DXL_HAL_OPEN_ERROR; + } + + newtio.c_cflag = B1000000|CS8|CLOCAL|CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; // time-out 값 (TIME * 0.1초) 0 : disable + newtio.c_cc[VMIN] = 0; // MIN 은 read 가 return 되기 위한 최소 문자 개수 + + tcflush(gSocket_fd, TCIFLUSH); + tcsetattr(gSocket_fd, TCSANOW, &newtio); + + return 1; + +DXL_HAL_OPEN_ERROR: + dxl_hal_close(); + return 0; +} + +void dxl_hal_close() +{ + if(gSocket_fd != -1) + close(gSocket_fd); + gSocket_fd = -1; +} + +int dxl_hal_set_baud( float baudrate ) +{ + struct serial_struct serinfo; + + if(gSocket_fd == -1) + return 0; + + //USB2AX uses the CDC ACM driver for which these settings do not exist. + /* + if(ioctl(gSocket_fd, TIOCGSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot get serial info\n"); + return 0; + } + + serinfo.flags &= ~ASYNC_SPD_MASK; + serinfo.flags |= ASYNC_SPD_CUST; + serinfo.custom_divisor = serinfo.baud_base / baudrate; + + if(ioctl(gSocket_fd, TIOCSSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot set serial info\n"); + return 0; + } + */ + //dxl_hal_close(); + //dxl_hal_open(gDeviceName, baudrate); + + gfByteTransTime = (float)((1000.0f / baudrate) * 12.0f); + return 1; +} + +void dxl_hal_clear(void) +{ + tcflush(gSocket_fd, TCIFLUSH); +} + +int dxl_hal_tx( unsigned char *pPacket, int numPacket ) +{ + return write(gSocket_fd, pPacket, numPacket); +} + +int dxl_hal_rx( unsigned char *pPacket, int numPacket ) +{ + memset(pPacket, 0, numPacket); + return read(gSocket_fd, pPacket, numPacket); +} + +static inline long myclock() +{ + struct timeval tv; + gettimeofday (&tv, NULL); + return (tv.tv_sec * 1000 + tv.tv_usec / 1000); +} + +void dxl_hal_set_timeout( int NumRcvByte ) +{ + glStartTime = myclock(); + gfRcvWaitTime = (float)(gfByteTransTime*(float)NumRcvByte + 5.0f); +} + +int dxl_hal_timeout(void) +{ + long time; + + time = myclock() - glStartTime; + + if(time > gfRcvWaitTime) + return 1; + else if(time < 0) + glStartTime = myclock(); + + return 0; +} diff --git a/libs/dynamixel/src/dxl_hal.h b/libs/dynamixel/src/dxl_hal.h new file mode 100644 index 0000000..983f73d --- /dev/null +++ b/libs/dynamixel/src/dxl_hal.h @@ -0,0 +1,24 @@ +#ifndef _DYNAMIXEL_HAL_HEADER +#define _DYNAMIXEL_HAL_HEADER + + +#ifdef __cplusplus +extern "C" { +#endif + + +int dxl_hal_open( int devIndex, float baudrate ); +void dxl_hal_close(); +void dxl_hal_clear(); +int dxl_hal_tx( unsigned char *pPacket, int numPacket ); +int dxl_hal_rx( unsigned char *pPacket, int numPacket ); +void dxl_hal_set_timeout( int NumRcvByte ); +int dxl_hal_timeout(); + + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libs/dynamixel/src/dxl_hal.o b/libs/dynamixel/src/dxl_hal.o new file mode 100644 index 0000000..a74b63c Binary files /dev/null and b/libs/dynamixel/src/dxl_hal.o differ diff --git a/libs/dynamixel/src/dynamixel.c b/libs/dynamixel/src/dynamixel.c new file mode 100644 index 0000000..e1bf1ed --- /dev/null +++ b/libs/dynamixel/src/dynamixel.c @@ -0,0 +1,495 @@ +#include "dxl_hal.h" +#include "dynamixel.h" + +#define ID (2) +#define LENGTH (3) +#define INSTRUCTION (4) +#define ERRBIT (4) +#define PARAMETER (5) +#define DEFAULT_BAUDNUMBER (1) + +unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; +unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; +unsigned char gbRxPacketLength = 0; +unsigned char gbRxGetLength = 0; +int gbCommStatus = COMM_RXSUCCESS; +int giBusUsing = 0; + + +int dxl_initialize( int devIndex, int baudnum ) +{ + float baudrate; + baudrate = 2000000.0f / (float)(baudnum + 1); + + if( dxl_hal_open(devIndex, baudrate) == 0 ) + return 0; + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return 1; +} + +void dxl_terminate() +{ + dxl_hal_close(); +} + +void dxl_tx_packet() +{ + unsigned char i; + unsigned char TxNumByte, RealTxNumByte; + unsigned char checksum = 0; + + if( giBusUsing == 1 ) + return; + + giBusUsing = 1; + + if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] != INST_PING + && gbInstructionPacket[INSTRUCTION] != INST_READ + && gbInstructionPacket[INSTRUCTION] != INST_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_ACTION + && gbInstructionPacket[INSTRUCTION] != INST_RESET + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + gbInstructionPacket[0] = 0xff; + gbInstructionPacket[1] = 0xff; + for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) + checksum += gbInstructionPacket[i+2]; + gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; + + if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) + dxl_hal_clear(); + + TxNumByte = gbInstructionPacket[LENGTH] + 4; + RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); + + if( TxNumByte != RealTxNumByte ) + { + gbCommStatus = COMM_TXFAIL; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] == INST_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else if ( gbInstructionPacket[INSTRUCTION] == INST_SYNC_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else + dxl_hal_set_timeout( 6 ); + + gbCommStatus = COMM_TXSUCCESS; +} + +void dxl_rx_packet() +{ + unsigned char i, j, nRead; + unsigned char checksum = 0; + + if( giBusUsing == 0 ) + return; + + if( gbInstructionPacket[ID] == BROADCAST_ID ) + { + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return; + } + + if( gbCommStatus == COMM_TXSUCCESS ) + { + gbRxGetLength = 0; + gbRxPacketLength = 6; + } + + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + if( dxl_hal_timeout() == 1 ) + { + if(gbRxGetLength == 0) + gbCommStatus = COMM_RXTIMEOUT; + else + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + } + + // Find packet header + for( i=0; i<(gbRxGetLength-1); i++ ) + { + if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) + { + break; + } + else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) + { + break; + } + } + if( i > 0 ) + { + for( j=0; j<(gbRxGetLength-i); j++ ) + gbStatusPacket[j] = gbStatusPacket[j + i]; + + gbRxGetLength -= i; + } + + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + + // Check id pairing + if( gbInstructionPacket[ID] != gbStatusPacket[ID]) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbRxPacketLength = gbStatusPacket[LENGTH] + 4; + if( gbRxGetLength < gbRxPacketLength ) + { + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + } + + // Check checksum + for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) + checksum += gbStatusPacket[i+2]; + checksum = ~checksum; + + if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; +} + +void dxl_txrx_packet() +{ + dxl_tx_packet(); + + if( gbCommStatus != COMM_TXSUCCESS ) + return; + + do{ + dxl_rx_packet(); + }while( gbCommStatus == COMM_RXWAITING ); +} + +int dxl_get_result() +{ + return gbCommStatus; +} + +void dxl_set_txpacket_id( int id ) +{ + gbInstructionPacket[ID] = (unsigned char)id; +} + +void dxl_set_txpacket_instruction( int instruction ) +{ + gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; +} + +void dxl_set_txpacket_parameter( int index, int value ) +{ + gbInstructionPacket[PARAMETER+index] = (unsigned char)value; +} + +void dxl_set_txpacket_length( int length ) +{ + gbInstructionPacket[LENGTH] = (unsigned char)length; +} + +int dxl_get_rxpacket_error( int errbit ) +{ + if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) + return 1; + + return 0; +} + +int dxl_get_rxpacket_length() +{ + return (int)gbStatusPacket[LENGTH]; +} + +int dxl_get_rxpacket_parameter( int index ) +{ + return (int)gbStatusPacket[PARAMETER+index]; +} + +int dxl_makeword( int lowbyte, int highbyte ) +{ + unsigned short word; + + word = highbyte; + word = word << 8; + word = word + lowbyte; + return (int)word; +} + +int dxl_get_lowbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff; + return (int)temp; +} + +int dxl_get_highbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff00; + temp = temp >> 8; + return (int)temp; +} + +void dxl_ping( int id ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_PING; + gbInstructionPacket[LENGTH] = 2; + + dxl_txrx_packet(); +} + +int dxl_read_byte( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 1; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + + return (int)gbStatusPacket[PARAMETER]; +} + +void dxl_write_byte( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)value; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); +} + +int dxl_read_word( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 2; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + + return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); +} + +void dxl_write_word( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); + gbInstructionPacket[LENGTH] = 5; + + dxl_txrx_packet(); +} + + +unsigned char gbSyncNbParam; + +void dxl_sync_write_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = BROADCAST_ID; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the RX code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_write_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_write_push_byte( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)value; +} + +void dxl_sync_write_push_word( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_highbyte(value); +} + +void dxl_sync_write_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + + dxl_txrx_packet(); +} + + +void dxl_sync_read_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = 0XFD; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the rx code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_read_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_read_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + gbSyncNbParam = 0; + + dxl_txrx_packet(); +} + +//// you will need to make a noblock_receive before anything else, because it will not allow any other command before it is done. +//void dxl_sync_read_noblock_send(){ +// while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. +// +// gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; +// gbSyncNbParam = 0; +// +// dxl_tx_packet(); +// +// if( gbCommStatus != COMM_TXSUCCESS ) +// return; +// +// do{ +// dxl_rx_packet(); +// }while( gbCommStatus == COMM_RXWAITING ); +// +// +//} +// +//void dxl_sync_read_noblock_receive(){ +// TODO +//} + + +int dxl_sync_read_pop_byte() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 2 ) + { + return -1; + } + + return (int)gbStatusPacket[PARAMETER+gbSyncNbParam++]; +} + +int dxl_sync_read_pop_word() +{ + int b0, b1; + + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 3 ) + { + return -1; + } + + b0 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + b1 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + + return dxl_makeword( b0, b1 ); +} + + + + diff --git a/libs/dynamixel/src/dynamixel.o b/libs/dynamixel/src/dynamixel.o new file mode 100644 index 0000000..8a46b56 Binary files /dev/null and b/libs/dynamixel/src/dynamixel.o differ diff --git a/libs/dynamixel/src/libdxl.a b/libs/dynamixel/src/libdxl.a new file mode 100644 index 0000000..2d368ae Binary files /dev/null and b/libs/dynamixel/src/libdxl.a differ diff --git a/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.sln b/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.sln new file mode 100644 index 0000000..22af500 --- /dev/null +++ b/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.sln @@ -0,0 +1,26 @@ + +Microsoft Visual Studio Solution File, Format Version 10.00 +# Visual Studio 2008 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Dynamixel_DLL", "Dynamixel_DLL.vcproj", "{FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Debug|x64 = Debug|x64 + Release|Win32 = Release|Win32 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.ActiveCfg = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.Build.0 = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.ActiveCfg = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.Build.0 = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.ActiveCfg = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.Build.0 = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.ActiveCfg = Release|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.vcproj b/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.vcproj new file mode 100644 index 0000000..287fcee --- /dev/null +++ b/libs/dynamixel/src_lib/Dynamixel_DLL/Dynamixel_DLL.vcproj @@ -0,0 +1,387 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/libs/dynamixel/src_lib/Makefile b/libs/dynamixel/src_lib/Makefile new file mode 100644 index 0000000..7edf049 --- /dev/null +++ b/libs/dynamixel/src_lib/Makefile @@ -0,0 +1,28 @@ +TARGET = libdxl.a +LIBNAME = libusb2ax.so +OBJS = dxl_hal.o dynamixel.o +SRCS = $(OBJS:.o=.c) +INCLUDEDIRS += -I../include +LIBDIRS += +CFLAGS = $(INCLUDEDIRS) -W -Wall -O2 -fPIC + +CC = gcc +AR = ar + +$(TARGET): $(OBJS) + $(AR) rs $@ $^ + cp $(TARGET) ../lib + +.c.o: + $(CC) -c $< $(CFLAGS) + +clean: + rm -f $(OBJS) $(TARGET) $(OBJS) + @echo "file deleted." + +lib: + gcc -fPIC -shared $(OBJS) -o $(LIBNAME) + cp $(LIBNAME) ../lib + +dep: + gccmakedep $(SRCS) diff --git a/libs/dynamixel/src_lib/dxl_hal.c b/libs/dynamixel/src_lib/dxl_hal.c new file mode 100644 index 0000000..ee18708 --- /dev/null +++ b/libs/dynamixel/src_lib/dxl_hal.c @@ -0,0 +1,179 @@ +/** +Modification of the HAL of the Dynamixel SDK to be used with USB2AX. + +Nicolas Saugnier +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dxl_hal.h" + +int gSocket_fd = -1; +long glStartTime = 0; +float gfRcvWaitTime = 0.0f; +float gfByteTransTime = 0.0f; + +char gDeviceName[20]; + +int dxl_hal_open(int deviceIndex, float baudrate) +{ + struct termios newtio; + //struct serial_struct serinfo; + char dev_name[100] = {0, }; + + sprintf(dev_name, "/dev/ttyACM%d", deviceIndex); // USB2AX is ttyACM + + strcpy(gDeviceName, dev_name); + memset(&newtio, 0, sizeof(newtio)); + dxl_hal_close(); + + if((gSocket_fd = open(gDeviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) { + fprintf(stderr, "device open error: %s\n", dev_name); + goto DXL_HAL_OPEN_ERROR; + } + + newtio.c_cflag = B1000000|CS8|CLOCAL|CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; // time-out 값 (TIME * 0.1초) 0 : disable + newtio.c_cc[VMIN] = 0; // MIN 은 read 가 return 되기 위한 최소 문자 개수 + + tcflush(gSocket_fd, TCIFLUSH); + tcsetattr(gSocket_fd, TCSANOW, &newtio); + + if(gSocket_fd == -1) + return 0; + + //USB2AX uses the CDC ACM driver for which these settings do not exist. + /* + if(ioctl(gSocket_fd, TIOCGSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot get serial info\n"); + return 0; + } + + serinfo.flags &= ~ASYNC_SPD_MASK; + serinfo.flags |= ASYNC_SPD_CUST; + serinfo.custom_divisor = serinfo.baud_base / baudrate; + + if(ioctl(gSocket_fd, TIOCSSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot set serial info\n"); + return 0; + }*/ + + dxl_hal_close(); + + gfByteTransTime = (float)((1000.0f / baudrate) * 12.0f); + + strcpy(gDeviceName, dev_name); + memset(&newtio, 0, sizeof(newtio)); + dxl_hal_close(); + + if((gSocket_fd = open(gDeviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) { + fprintf(stderr, "device open error: %s\n", dev_name); + goto DXL_HAL_OPEN_ERROR; + } + + newtio.c_cflag = B1000000|CS8|CLOCAL|CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; // time-out 값 (TIME * 0.1초) 0 : disable + newtio.c_cc[VMIN] = 0; // MIN 은 read 가 return 되기 위한 최소 문자 개수 + + tcflush(gSocket_fd, TCIFLUSH); + tcsetattr(gSocket_fd, TCSANOW, &newtio); + + return 1; + +DXL_HAL_OPEN_ERROR: + dxl_hal_close(); + return 0; +} + +void dxl_hal_close() +{ + if(gSocket_fd != -1) + close(gSocket_fd); + gSocket_fd = -1; +} + +int dxl_hal_set_baud( float baudrate ) +{ + struct serial_struct serinfo; + + if(gSocket_fd == -1) + return 0; + + //USB2AX uses the CDC ACM driver for which these settings do not exist. + /* + if(ioctl(gSocket_fd, TIOCGSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot get serial info\n"); + return 0; + } + + serinfo.flags &= ~ASYNC_SPD_MASK; + serinfo.flags |= ASYNC_SPD_CUST; + serinfo.custom_divisor = serinfo.baud_base / baudrate; + + if(ioctl(gSocket_fd, TIOCSSERIAL, &serinfo) < 0) { + fprintf(stderr, "Cannot set serial info\n"); + return 0; + } + */ + //dxl_hal_close(); + //dxl_hal_open(gDeviceName, baudrate); + + gfByteTransTime = (float)((1000.0f / baudrate) * 12.0f); + return 1; +} + +void dxl_hal_clear(void) +{ + tcflush(gSocket_fd, TCIFLUSH); +} + +int dxl_hal_tx( unsigned char *pPacket, int numPacket ) +{ + return write(gSocket_fd, pPacket, numPacket); +} + +int dxl_hal_rx( unsigned char *pPacket, int numPacket ) +{ + memset(pPacket, 0, numPacket); + return read(gSocket_fd, pPacket, numPacket); +} + +static inline long myclock() +{ + struct timeval tv; + gettimeofday (&tv, NULL); + return (tv.tv_sec * 1000 + tv.tv_usec / 1000); +} + +void dxl_hal_set_timeout( int NumRcvByte ) +{ + glStartTime = myclock(); + gfRcvWaitTime = (float)(gfByteTransTime*(float)NumRcvByte + 5.0f); +} + +int dxl_hal_timeout(void) +{ + long time; + + time = myclock() - glStartTime; + + if(time > gfRcvWaitTime) + return 1; + else if(time < 0) + glStartTime = myclock(); + + return 0; +} diff --git a/libs/dynamixel/src_lib/dxl_hal.h b/libs/dynamixel/src_lib/dxl_hal.h new file mode 100644 index 0000000..983f73d --- /dev/null +++ b/libs/dynamixel/src_lib/dxl_hal.h @@ -0,0 +1,24 @@ +#ifndef _DYNAMIXEL_HAL_HEADER +#define _DYNAMIXEL_HAL_HEADER + + +#ifdef __cplusplus +extern "C" { +#endif + + +int dxl_hal_open( int devIndex, float baudrate ); +void dxl_hal_close(); +void dxl_hal_clear(); +int dxl_hal_tx( unsigned char *pPacket, int numPacket ); +int dxl_hal_rx( unsigned char *pPacket, int numPacket ); +void dxl_hal_set_timeout( int NumRcvByte ); +int dxl_hal_timeout(); + + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libs/dynamixel/src_lib/dynamixel.c b/libs/dynamixel/src_lib/dynamixel.c new file mode 100644 index 0000000..8115323 --- /dev/null +++ b/libs/dynamixel/src_lib/dynamixel.c @@ -0,0 +1,506 @@ +#if defined(WIN32) || defined(_WIN32) +#define EXPORT __declspec(dllexport) +#else +#define EXPORT +#endif + +#include +#include "dxl_hal.h" +#include "dynamixel.h" + +#define ID (2) +#define LENGTH (3) +#define INSTRUCTION (4) +#define ERRBIT (4) +#define PARAMETER (5) +#define DEFAULT_BAUDNUMBER (1) + +unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; +unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; +unsigned char gbRxPacketLength = 0; +unsigned char gbRxGetLength = 0; +int gbCommStatus = COMM_RXSUCCESS; +int giBusUsing = 0; + + +EXPORT int dxl_initialize( int devIndex, int baudnum ) +{ + float baudrate; + baudrate = 2000000.0f / (float)(baudnum + 1); + + if( dxl_hal_open(devIndex, baudrate) == 0 ) + return 0; + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return 1; +} + +EXPORT void dxl_terminate() +{ + dxl_hal_close(); +} + +void dxl_tx_packet() +{ + unsigned char i; + unsigned char TxNumByte, RealTxNumByte; + unsigned char checksum = 0; + + if( giBusUsing == 1 ) + return; + + giBusUsing = 1; + + if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] != INST_PING + && gbInstructionPacket[INSTRUCTION] != INST_READ + && gbInstructionPacket[INSTRUCTION] != INST_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_ACTION + && gbInstructionPacket[INSTRUCTION] != INST_RESET + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + gbInstructionPacket[0] = 0xff; + gbInstructionPacket[1] = 0xff; + for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) + checksum += gbInstructionPacket[i+2]; + gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; + + if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) + dxl_hal_clear(); + + TxNumByte = gbInstructionPacket[LENGTH] + 4; + RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); + + if( TxNumByte != RealTxNumByte ) + { + gbCommStatus = COMM_TXFAIL; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] == INST_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else if ( gbInstructionPacket[INSTRUCTION] == INST_SYNC_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else + dxl_hal_set_timeout( 6 ); + + gbCommStatus = COMM_TXSUCCESS; +} + +void dxl_rx_packet() +{ + unsigned char i, j, nRead; + unsigned char checksum = 0; + + if( giBusUsing == 0 ) + return; + + if( gbInstructionPacket[ID] == BROADCAST_ID ) + { + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return; + } + + if( gbCommStatus == COMM_TXSUCCESS ) + { + gbRxGetLength = 0; + gbRxPacketLength = 6; + } + + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + if( dxl_hal_timeout() == 1 ) + { + if(gbRxGetLength == 0) + gbCommStatus = COMM_RXTIMEOUT; + else + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + } + + // Find packet header + for( i=0; i<(gbRxGetLength-1); i++ ) + { + if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) + { + break; + } + else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) + { + break; + } + } + if( i > 0 ) + { + for( j=0; j<(gbRxGetLength-i); j++ ) + gbStatusPacket[j] = gbStatusPacket[j + i]; + + gbRxGetLength -= i; + } + + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + + // Check id pairing + if( gbInstructionPacket[ID] != gbStatusPacket[ID]) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbRxPacketLength = gbStatusPacket[LENGTH] + 4; + if( gbRxGetLength < gbRxPacketLength ) + { + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + } + + // Check checksum + for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) + checksum += gbStatusPacket[i+2]; + checksum = ~checksum; + + if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; +} + +void dxl_txrx_packet() +{ + dxl_tx_packet(); + + if( gbCommStatus != COMM_TXSUCCESS ) + return; + + do{ + dxl_rx_packet(); + }while( gbCommStatus == COMM_RXWAITING ); +} + +EXPORT int dxl_get_result() +{ + return gbCommStatus; +} + +void dxl_set_txpacket_id( int id ) +{ + gbInstructionPacket[ID] = (unsigned char)id; +} + +void dxl_set_txpacket_instruction( int instruction ) +{ + gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; +} + +void dxl_set_txpacket_parameter( int index, int value ) +{ + gbInstructionPacket[PARAMETER+index] = (unsigned char)value; +} + +void dxl_set_txpacket_length( int length ) +{ + gbInstructionPacket[LENGTH] = (unsigned char)length; +} + +int dxl_get_rxpacket_error( int errbit ) +{ + if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) + return 1; + + return 0; +} + +int dxl_get_rxpacket_length() +{ + return (int)gbStatusPacket[LENGTH]; +} + +int dxl_get_rxpacket_parameter( int index ) +{ + return (int)gbStatusPacket[PARAMETER+index]; +} + +int dxl_makeword( int lowbyte, int highbyte ) +{ + unsigned short word; + + word = highbyte; + word = word << 8; + word = word + lowbyte; + return (int)word; +} + +int dxl_get_lowbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff; + return (int)temp; +} + +int dxl_get_highbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff00; + temp = temp >> 8; + return (int)temp; +} + +EXPORT void dxl_ping( int id ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_PING; + gbInstructionPacket[LENGTH] = 2; + + dxl_txrx_packet(); +} + +EXPORT int dxl_read_byte( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 1; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + usleep(5000); + + return (int)gbStatusPacket[PARAMETER]; +} + +EXPORT void dxl_write_byte( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)value; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + usleep(5000); +} + +EXPORT int dxl_read_word( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 2; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + usleep(5000); + + return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); +} + +EXPORT void dxl_write_word( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); + gbInstructionPacket[LENGTH] = 5; + + dxl_txrx_packet(); + usleep(5000); +} + + +unsigned char gbSyncNbParam; + +void dxl_sync_write_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = BROADCAST_ID; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the RX code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_write_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_write_push_byte( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)value; +} + +void dxl_sync_write_push_word( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_highbyte(value); +} + +void dxl_sync_write_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + + dxl_txrx_packet(); +} + + +void dxl_sync_read_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = 0XFD; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the rx code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_read_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_read_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + gbSyncNbParam = 0; + + dxl_txrx_packet(); +} + +//// you will need to make a noblock_receive before anything else, because it will not allow any other command before it is done. +//void dxl_sync_read_noblock_send(){ +// while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. +// +// gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; +// gbSyncNbParam = 0; +// +// dxl_tx_packet(); +// +// if( gbCommStatus != COMM_TXSUCCESS ) +// return; +// +// do{ +// dxl_rx_packet(); +// }while( gbCommStatus == COMM_RXWAITING ); +// +// +//} +// +//void dxl_sync_read_noblock_receive(){ +// TODO +//} + + +int dxl_sync_read_pop_byte() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 2 ) + { + return -1; + } + + return (int)gbStatusPacket[PARAMETER+gbSyncNbParam++]; +} + +int dxl_sync_read_pop_word() +{ + int b0, b1; + + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 3 ) + { + return -1; + } + + b0 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + b1 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + + return dxl_makeword( b0, b1 ); +} + + + + diff --git a/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.sln b/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.sln new file mode 100644 index 0000000..22af500 --- /dev/null +++ b/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.sln @@ -0,0 +1,26 @@ + +Microsoft Visual Studio Solution File, Format Version 10.00 +# Visual Studio 2008 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Dynamixel_DLL", "Dynamixel_DLL.vcproj", "{FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Debug|x64 = Debug|x64 + Release|Win32 = Release|Win32 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.ActiveCfg = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|Win32.Build.0 = Debug|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.ActiveCfg = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Debug|x64.Build.0 = Debug|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.ActiveCfg = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|Win32.Build.0 = Release|Win32 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.ActiveCfg = Release|x64 + {FB0F8E0F-5792-4F42-8955-2BE5E7A63E4E}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.vcproj b/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.vcproj new file mode 100644 index 0000000..287fcee --- /dev/null +++ b/libs/dynamixel/src_win/Dynamixel_DLL/Dynamixel_DLL.vcproj @@ -0,0 +1,387 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/libs/dynamixel/src_win/Makefile b/libs/dynamixel/src_win/Makefile new file mode 100644 index 0000000..3685822 --- /dev/null +++ b/libs/dynamixel/src_win/Makefile @@ -0,0 +1,24 @@ +TARGET = libdxl.a +OBJS = dxl_hal.o dynamixel.o +SRCS = $(OBJS:.o=.c) +INCLUDEDIRS += -I../include +LIBDIRS += +CFLAGS = $(INCLUDEDIRS) -W -Wall -O2 + +CC = gcc +AR = ar + +$(TARGET): $(OBJS) + $(AR) rs $@ $^ + cp $(TARGET) ../lib + +.c.o: + $(CC) -c $< $(CFLAGS) + +clean: + rm -f $(OBJS) $(TARGET) + @echo "file deleted." + +dep: + gccmakedep $(SRCS) + diff --git a/libs/dynamixel/src_win/dxl_hal.c b/libs/dynamixel/src_win/dxl_hal.c new file mode 100644 index 0000000..4b0f3f1 --- /dev/null +++ b/libs/dynamixel/src_win/dxl_hal.c @@ -0,0 +1,170 @@ +// windows version +// by windows serial programming +#include +#include +#include "dxl_hal.h" + +#define LATENCY_TIME (16) //ms (USB2Serial Latency timer) +#define IN_TRASFER_SIZE (512) //unsigned char + +HANDLE ghSerial_Handle = INVALID_HANDLE_VALUE; // Serial port handle +float gfByteTransTime = 0.0f; +float gfRcvWaitTime = 0.0f; +LARGE_INTEGER gStartTime; + + +int dxl_hal_open( int devIndex, float baudrate ) +{ + // Opening device + // devIndex: Device index + // baudrate: Real baudrate (ex> 115200, 57600, 38400...) + // Return: 0(Failed), 1(Succeed) + + DCB Dcb; + COMMTIMEOUTS Timeouts; + DWORD dwError; + char PortName[15]; + + dxl_hal_close(); + + // Make real port name + sprintf_s(PortName, 15, "\\\\.\\COM%d", devIndex); + // Open serial device + ghSerial_Handle = CreateFile( PortName, GENERIC_READ|GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL ); + if( ghSerial_Handle == INVALID_HANDLE_VALUE ) + return 0; + + // Setting communication property + Dcb.DCBlength = sizeof(DCB); + if( GetCommState( ghSerial_Handle, &Dcb ) == FALSE ) + goto DXL_HAL_OPEN_ERROR; + + // Set baudrate + gfByteTransTime = 1000.0f / baudrate * 10.0f; // 1000/baudrate(bit per msec) * 10(start bit + data bit + stop bit) + Dcb.BaudRate = (DWORD)baudrate; + Dcb.ByteSize = 8; // Data bit = 8bit + Dcb.Parity = NOPARITY; // No parity + Dcb.StopBits = ONESTOPBIT; // Stop bit = 1 + Dcb.fParity = NOPARITY; // No Parity check + Dcb.fBinary = 1; // Binary mode + Dcb.fNull = 0; // Get Null byte + Dcb.fAbortOnError = 1; + Dcb.fErrorChar = 0; + // Not using XOn/XOff + Dcb.fOutX = 0; + Dcb.fInX = 0; + // Not using H/W flow control + Dcb.fDtrControl = DTR_CONTROL_DISABLE; + Dcb.fRtsControl = RTS_CONTROL_DISABLE; + Dcb.fDsrSensitivity = 0; + Dcb.fOutxDsrFlow = 0; + Dcb.fOutxCtsFlow = 0; + if( SetCommState( ghSerial_Handle, &Dcb ) == FALSE ) + goto DXL_HAL_OPEN_ERROR; + + if( SetCommMask( ghSerial_Handle, 0 ) == FALSE ) // Not using Comm event + goto DXL_HAL_OPEN_ERROR; + if( SetupComm( ghSerial_Handle, 4096, 4096 ) == FALSE ) // Buffer size (Rx,Tx) + goto DXL_HAL_OPEN_ERROR; + if( PurgeComm( ghSerial_Handle, PURGE_TXABORT|PURGE_TXCLEAR|PURGE_RXABORT|PURGE_RXCLEAR ) == FALSE ) // Clear buffer + goto DXL_HAL_OPEN_ERROR; + if( ClearCommError( ghSerial_Handle, &dwError, NULL ) == FALSE ) + goto DXL_HAL_OPEN_ERROR; + + if( GetCommTimeouts( ghSerial_Handle, &Timeouts ) == FALSE ) + goto DXL_HAL_OPEN_ERROR; + // Timeout (Not using timeout) + // Immediatly return + Timeouts.ReadIntervalTimeout = 0; + Timeouts.ReadTotalTimeoutMultiplier = 0; + Timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. + Timeouts.WriteTotalTimeoutMultiplier = 0; + Timeouts.WriteTotalTimeoutConstant = 0; + if( SetCommTimeouts( ghSerial_Handle, &Timeouts ) == FALSE ) + goto DXL_HAL_OPEN_ERROR; + + return 1; + +DXL_HAL_OPEN_ERROR: + dxl_hal_close(); + return 0; +} + +void dxl_hal_close() +{ + // Closing device + if(ghSerial_Handle != INVALID_HANDLE_VALUE) + { + CloseHandle( ghSerial_Handle ); + ghSerial_Handle = INVALID_HANDLE_VALUE; + } +} + +void dxl_hal_clear(void) +{ + // Clear communication buffer + PurgeComm( ghSerial_Handle, PURGE_RXABORT|PURGE_RXCLEAR ); +} + +int dxl_hal_tx( unsigned char *pPacket, int numPacket ) +{ + // Transmiting date + // *pPacket: data array pointer + // numPacket: number of data array + // Return: number of data transmitted. -1 is error. + DWORD dwToWrite, dwWritten; + + dwToWrite = (DWORD)numPacket; + dwWritten = 0; + + if( WriteFile( ghSerial_Handle, pPacket, dwToWrite, &dwWritten, NULL ) == FALSE ) + return -1; + + return (int)dwWritten; +} + +int dxl_hal_rx( unsigned char *pPacket, int numPacket ) +{ + // Recieving date + // *pPacket: data array pointer + // numPacket: number of data array + // Return: number of data recieved. -1 is error. + DWORD dwToRead, dwRead; + + dwToRead = (DWORD)numPacket; + dwRead = 0; + + if( ReadFile( ghSerial_Handle, pPacket, dwToRead, &dwRead, NULL ) == FALSE ) + return -1; + + return (int)dwRead; +} + +void dxl_hal_set_timeout( int NumRcvByte ) +{ + // Start stop watch + // NumRcvByte: number of recieving data(to calculate maximum waiting time) + QueryPerformanceCounter( &gStartTime ); + gfRcvWaitTime = (float)(gfByteTransTime*(float)NumRcvByte + 2*LATENCY_TIME + 2.0f); +} + +int dxl_hal_timeout(void) +{ + // Check timeout + // Return: 0 is false, 1 is true(timeout occurred) + LARGE_INTEGER end, freq; + double time; + + QueryPerformanceCounter( &end ); + QueryPerformanceFrequency( &freq ); + + time = (double)(end.QuadPart - gStartTime.QuadPart) / (double)freq.QuadPart; + time *= 1000.0; + + if( time > gfRcvWaitTime ) + return 1; + else if( time < 0 ) + QueryPerformanceCounter( &gStartTime ); + + return 0; +} \ No newline at end of file diff --git a/libs/dynamixel/src_win/dxl_hal.h b/libs/dynamixel/src_win/dxl_hal.h new file mode 100644 index 0000000..983f73d --- /dev/null +++ b/libs/dynamixel/src_win/dxl_hal.h @@ -0,0 +1,24 @@ +#ifndef _DYNAMIXEL_HAL_HEADER +#define _DYNAMIXEL_HAL_HEADER + + +#ifdef __cplusplus +extern "C" { +#endif + + +int dxl_hal_open( int devIndex, float baudrate ); +void dxl_hal_close(); +void dxl_hal_clear(); +int dxl_hal_tx( unsigned char *pPacket, int numPacket ); +int dxl_hal_rx( unsigned char *pPacket, int numPacket ); +void dxl_hal_set_timeout( int NumRcvByte ); +int dxl_hal_timeout(); + + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/libs/dynamixel/src_win/dynamixel.c b/libs/dynamixel/src_win/dynamixel.c new file mode 100644 index 0000000..c2df3cb --- /dev/null +++ b/libs/dynamixel/src_win/dynamixel.c @@ -0,0 +1,506 @@ +#if defined(WIN32) || defined(_WIN32) +#define EXPORT __declspec(dllexport) +#else +#define EXPORT +#endif + +#include +#include "dxl_hal.h" +#include "dynamixel.h" + +#define ID (2) +#define LENGTH (3) +#define INSTRUCTION (4) +#define ERRBIT (4) +#define PARAMETER (5) +#define DEFAULT_BAUDNUMBER (1) + +unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; +unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; +unsigned char gbRxPacketLength = 0; +unsigned char gbRxGetLength = 0; +int gbCommStatus = COMM_RXSUCCESS; +int giBusUsing = 0; + + +EXPORT int dxl_initialize( int devIndex, int baudnum ) +{ + float baudrate; + baudrate = 2000000.0f / (float)(baudnum + 1); + + if( dxl_hal_open(devIndex, baudrate) == 0 ) + return 0; + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return 1; +} + +EXPORT void dxl_terminate() +{ + dxl_hal_close(); +} + +void dxl_tx_packet() +{ + unsigned char i; + unsigned char TxNumByte, RealTxNumByte; + unsigned char checksum = 0; + + if( giBusUsing == 1 ) + return; + + giBusUsing = 1; + + if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] != INST_PING + && gbInstructionPacket[INSTRUCTION] != INST_READ + && gbInstructionPacket[INSTRUCTION] != INST_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_ACTION + && gbInstructionPacket[INSTRUCTION] != INST_RESET + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE + && gbInstructionPacket[INSTRUCTION] != INST_SYNC_READ) + { + gbCommStatus = COMM_TXERROR; + giBusUsing = 0; + return; + } + + gbInstructionPacket[0] = 0xff; + gbInstructionPacket[1] = 0xff; + for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) + checksum += gbInstructionPacket[i+2]; + gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; + + if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) + dxl_hal_clear(); + + TxNumByte = gbInstructionPacket[LENGTH] + 4; + RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); + + if( TxNumByte != RealTxNumByte ) + { + gbCommStatus = COMM_TXFAIL; + giBusUsing = 0; + return; + } + + if( gbInstructionPacket[INSTRUCTION] == INST_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else if ( gbInstructionPacket[INSTRUCTION] == INST_SYNC_READ ) + dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); + else + dxl_hal_set_timeout( 6 ); + + gbCommStatus = COMM_TXSUCCESS; +} + +void dxl_rx_packet() +{ + unsigned char i, j, nRead; + unsigned char checksum = 0; + + if( giBusUsing == 0 ) + return; + + if( gbInstructionPacket[ID] == BROADCAST_ID ) + { + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; + return; + } + + if( gbCommStatus == COMM_TXSUCCESS ) + { + gbRxGetLength = 0; + gbRxPacketLength = 6; + } + + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + if( dxl_hal_timeout() == 1 ) + { + if(gbRxGetLength == 0) + gbCommStatus = COMM_RXTIMEOUT; + else + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + } + + // Find packet header + for( i=0; i<(gbRxGetLength-1); i++ ) + { + if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) + { + break; + } + else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) + { + break; + } + } + if( i > 0 ) + { + for( j=0; j<(gbRxGetLength-i); j++ ) + gbStatusPacket[j] = gbStatusPacket[j + i]; + + gbRxGetLength -= i; + } + + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + + // Check id pairing + if( gbInstructionPacket[ID] != gbStatusPacket[ID]) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbRxPacketLength = gbStatusPacket[LENGTH] + 4; + if( gbRxGetLength < gbRxPacketLength ) + { + nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); + gbRxGetLength += nRead; + if( gbRxGetLength < gbRxPacketLength ) + { + gbCommStatus = COMM_RXWAITING; + return; + } + } + + // Check checksum + for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) + checksum += gbStatusPacket[i+2]; + checksum = ~checksum; + + if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) + { + gbCommStatus = COMM_RXCORRUPT; + giBusUsing = 0; + return; + } + + gbCommStatus = COMM_RXSUCCESS; + giBusUsing = 0; +} + +void dxl_txrx_packet() +{ + dxl_tx_packet(); + + if( gbCommStatus != COMM_TXSUCCESS ) + return; + + do{ + dxl_rx_packet(); + }while( gbCommStatus == COMM_RXWAITING ); +} + +EXPORT int dxl_get_result() +{ + return gbCommStatus; +} + +void dxl_set_txpacket_id( int id ) +{ + gbInstructionPacket[ID] = (unsigned char)id; +} + +void dxl_set_txpacket_instruction( int instruction ) +{ + gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; +} + +void dxl_set_txpacket_parameter( int index, int value ) +{ + gbInstructionPacket[PARAMETER+index] = (unsigned char)value; +} + +void dxl_set_txpacket_length( int length ) +{ + gbInstructionPacket[LENGTH] = (unsigned char)length; +} + +int dxl_get_rxpacket_error( int errbit ) +{ + if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) + return 1; + + return 0; +} + +int dxl_get_rxpacket_length() +{ + return (int)gbStatusPacket[LENGTH]; +} + +int dxl_get_rxpacket_parameter( int index ) +{ + return (int)gbStatusPacket[PARAMETER+index]; +} + +int dxl_makeword( int lowbyte, int highbyte ) +{ + unsigned short word; + + word = highbyte; + word = word << 8; + word = word + lowbyte; + return (int)word; +} + +int dxl_get_lowbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff; + return (int)temp; +} + +int dxl_get_highbyte( int word ) +{ + unsigned short temp; + + temp = word & 0xff00; + temp = temp >> 8; + return (int)temp; +} + +EXPORT void dxl_ping( int id ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_PING; + gbInstructionPacket[LENGTH] = 2; + + dxl_txrx_packet(); +} + +EXPORT int dxl_read_byte( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 1; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + Sleep(5); + + return (int)gbStatusPacket[PARAMETER]; +} + +EXPORT void dxl_write_byte( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)value; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + Sleep(5); +} + +EXPORT int dxl_read_word( int id, int address ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = 2; + gbInstructionPacket[LENGTH] = 4; + + dxl_txrx_packet(); + Sleep(5); + + return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); +} + +EXPORT void dxl_write_word( int id, int address, int value ) +{ + while(giBusUsing); + + gbInstructionPacket[ID] = (unsigned char)id; + gbInstructionPacket[INSTRUCTION] = INST_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); + gbInstructionPacket[LENGTH] = 5; + + dxl_txrx_packet(); + Sleep(5); +} + + +unsigned char gbSyncNbParam; + +void dxl_sync_write_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = BROADCAST_ID; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the RX code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_write_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_write_push_byte( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)value; +} + +void dxl_sync_write_push_word( int value ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_lowbyte(value); + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)dxl_get_highbyte(value); +} + +void dxl_sync_write_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + + dxl_txrx_packet(); +} + + +void dxl_sync_read_start( int address, int data_length ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[ID] = 0XFD; // use the device ID of the USB2AX instead of the broadcast ID to avoid some modifications to the rx code. + gbInstructionPacket[INSTRUCTION] = INST_SYNC_READ; + gbInstructionPacket[PARAMETER] = (unsigned char)address; + gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length; + gbSyncNbParam = 2; +} + +void dxl_sync_read_push_id( int id ) +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam > MAXNUM_TXPARAM ) + { + return; + } + + gbInstructionPacket[PARAMETER+gbSyncNbParam++] = (unsigned char)id; +} + +void dxl_sync_read_send() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; + gbSyncNbParam = 0; + + dxl_txrx_packet(); +} + +//// you will need to make a noblock_receive before anything else, because it will not allow any other command before it is done. +//void dxl_sync_read_noblock_send(){ +// while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. +// +// gbInstructionPacket[LENGTH] = gbSyncNbParam + 2; +// gbSyncNbParam = 0; +// +// dxl_tx_packet(); +// +// if( gbCommStatus != COMM_TXSUCCESS ) +// return; +// +// do{ +// dxl_rx_packet(); +// }while( gbCommStatus == COMM_RXWAITING ); +// +// +//} +// +//void dxl_sync_read_noblock_receive(){ +// TODO +//} + + +int dxl_sync_read_pop_byte() +{ + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 2 ) + { + return -1; + } + + return (int)gbStatusPacket[PARAMETER+gbSyncNbParam++]; +} + +int dxl_sync_read_pop_word() +{ + int b0, b1; + + while(giBusUsing); // needs to be done before touching the TX buffer as it is used until the end of RX. + + if ( gbSyncNbParam >= gbStatusPacket[LENGTH] - 3 ) + { + return -1; + } + + b0 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + b1 = gbStatusPacket[PARAMETER + gbSyncNbParam++]; + + return dxl_makeword( b0, b1 ); +} + + + + diff --git a/libs/jquery-2.1.3.min.js b/libs/jquery-2.1.3.min.js new file mode 100644 index 0000000..25714ed --- /dev/null +++ b/libs/jquery-2.1.3.min.js @@ -0,0 +1,4 @@ +/*! jQuery v2.1.3 | (c) 2005, 2014 jQuery Foundation, Inc. | jquery.org/license */ +!function(a,b){"object"==typeof module&&"object"==typeof module.exports?module.exports=a.document?b(a,!0):function(a){if(!a.document)throw new Error("jQuery requires a window with a document");return b(a)}:b(a)}("undefined"!=typeof window?window:this,function(a,b){var c=[],d=c.slice,e=c.concat,f=c.push,g=c.indexOf,h={},i=h.toString,j=h.hasOwnProperty,k={},l=a.document,m="2.1.3",n=function(a,b){return new n.fn.init(a,b)},o=/^[\s\uFEFF\xA0]+|[\s\uFEFF\xA0]+$/g,p=/^-ms-/,q=/-([\da-z])/gi,r=function(a,b){return b.toUpperCase()};n.fn=n.prototype={jquery:m,constructor:n,selector:"",length:0,toArray:function(){return d.call(this)},get:function(a){return null!=a?0>a?this[a+this.length]:this[a]:d.call(this)},pushStack:function(a){var b=n.merge(this.constructor(),a);return b.prevObject=this,b.context=this.context,b},each:function(a,b){return n.each(this,a,b)},map:function(a){return this.pushStack(n.map(this,function(b,c){return a.call(b,c,b)}))},slice:function(){return this.pushStack(d.apply(this,arguments))},first:function(){return this.eq(0)},last:function(){return this.eq(-1)},eq:function(a){var b=this.length,c=+a+(0>a?b:0);return this.pushStack(c>=0&&b>c?[this[c]]:[])},end:function(){return this.prevObject||this.constructor(null)},push:f,sort:c.sort,splice:c.splice},n.extend=n.fn.extend=function(){var a,b,c,d,e,f,g=arguments[0]||{},h=1,i=arguments.length,j=!1;for("boolean"==typeof g&&(j=g,g=arguments[h]||{},h++),"object"==typeof g||n.isFunction(g)||(g={}),h===i&&(g=this,h--);i>h;h++)if(null!=(a=arguments[h]))for(b in a)c=g[b],d=a[b],g!==d&&(j&&d&&(n.isPlainObject(d)||(e=n.isArray(d)))?(e?(e=!1,f=c&&n.isArray(c)?c:[]):f=c&&n.isPlainObject(c)?c:{},g[b]=n.extend(j,f,d)):void 0!==d&&(g[b]=d));return g},n.extend({expando:"jQuery"+(m+Math.random()).replace(/\D/g,""),isReady:!0,error:function(a){throw new Error(a)},noop:function(){},isFunction:function(a){return"function"===n.type(a)},isArray:Array.isArray,isWindow:function(a){return null!=a&&a===a.window},isNumeric:function(a){return!n.isArray(a)&&a-parseFloat(a)+1>=0},isPlainObject:function(a){return"object"!==n.type(a)||a.nodeType||n.isWindow(a)?!1:a.constructor&&!j.call(a.constructor.prototype,"isPrototypeOf")?!1:!0},isEmptyObject:function(a){var b;for(b in a)return!1;return!0},type:function(a){return null==a?a+"":"object"==typeof a||"function"==typeof a?h[i.call(a)]||"object":typeof a},globalEval:function(a){var b,c=eval;a=n.trim(a),a&&(1===a.indexOf("use strict")?(b=l.createElement("script"),b.text=a,l.head.appendChild(b).parentNode.removeChild(b)):c(a))},camelCase:function(a){return a.replace(p,"ms-").replace(q,r)},nodeName:function(a,b){return a.nodeName&&a.nodeName.toLowerCase()===b.toLowerCase()},each:function(a,b,c){var d,e=0,f=a.length,g=s(a);if(c){if(g){for(;f>e;e++)if(d=b.apply(a[e],c),d===!1)break}else for(e in a)if(d=b.apply(a[e],c),d===!1)break}else if(g){for(;f>e;e++)if(d=b.call(a[e],e,a[e]),d===!1)break}else for(e in a)if(d=b.call(a[e],e,a[e]),d===!1)break;return a},trim:function(a){return null==a?"":(a+"").replace(o,"")},makeArray:function(a,b){var c=b||[];return null!=a&&(s(Object(a))?n.merge(c,"string"==typeof a?[a]:a):f.call(c,a)),c},inArray:function(a,b,c){return null==b?-1:g.call(b,a,c)},merge:function(a,b){for(var c=+b.length,d=0,e=a.length;c>d;d++)a[e++]=b[d];return a.length=e,a},grep:function(a,b,c){for(var d,e=[],f=0,g=a.length,h=!c;g>f;f++)d=!b(a[f],f),d!==h&&e.push(a[f]);return e},map:function(a,b,c){var d,f=0,g=a.length,h=s(a),i=[];if(h)for(;g>f;f++)d=b(a[f],f,c),null!=d&&i.push(d);else for(f in a)d=b(a[f],f,c),null!=d&&i.push(d);return e.apply([],i)},guid:1,proxy:function(a,b){var c,e,f;return"string"==typeof b&&(c=a[b],b=a,a=c),n.isFunction(a)?(e=d.call(arguments,2),f=function(){return a.apply(b||this,e.concat(d.call(arguments)))},f.guid=a.guid=a.guid||n.guid++,f):void 0},now:Date.now,support:k}),n.each("Boolean Number String Function Array Date RegExp Object Error".split(" "),function(a,b){h["[object "+b+"]"]=b.toLowerCase()});function s(a){var b=a.length,c=n.type(a);return"function"===c||n.isWindow(a)?!1:1===a.nodeType&&b?!0:"array"===c||0===b||"number"==typeof b&&b>0&&b-1 in a}var t=function(a){var b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u="sizzle"+1*new Date,v=a.document,w=0,x=0,y=hb(),z=hb(),A=hb(),B=function(a,b){return a===b&&(l=!0),0},C=1<<31,D={}.hasOwnProperty,E=[],F=E.pop,G=E.push,H=E.push,I=E.slice,J=function(a,b){for(var c=0,d=a.length;d>c;c++)if(a[c]===b)return c;return-1},K="checked|selected|async|autofocus|autoplay|controls|defer|disabled|hidden|ismap|loop|multiple|open|readonly|required|scoped",L="[\\x20\\t\\r\\n\\f]",M="(?:\\\\.|[\\w-]|[^\\x00-\\xa0])+",N=M.replace("w","w#"),O="\\["+L+"*("+M+")(?:"+L+"*([*^$|!~]?=)"+L+"*(?:'((?:\\\\.|[^\\\\'])*)'|\"((?:\\\\.|[^\\\\\"])*)\"|("+N+"))|)"+L+"*\\]",P=":("+M+")(?:\\((('((?:\\\\.|[^\\\\'])*)'|\"((?:\\\\.|[^\\\\\"])*)\")|((?:\\\\.|[^\\\\()[\\]]|"+O+")*)|.*)\\)|)",Q=new RegExp(L+"+","g"),R=new RegExp("^"+L+"+|((?:^|[^\\\\])(?:\\\\.)*)"+L+"+$","g"),S=new RegExp("^"+L+"*,"+L+"*"),T=new RegExp("^"+L+"*([>+~]|"+L+")"+L+"*"),U=new RegExp("="+L+"*([^\\]'\"]*?)"+L+"*\\]","g"),V=new RegExp(P),W=new RegExp("^"+N+"$"),X={ID:new RegExp("^#("+M+")"),CLASS:new RegExp("^\\.("+M+")"),TAG:new RegExp("^("+M.replace("w","w*")+")"),ATTR:new RegExp("^"+O),PSEUDO:new RegExp("^"+P),CHILD:new RegExp("^:(only|first|last|nth|nth-last)-(child|of-type)(?:\\("+L+"*(even|odd|(([+-]|)(\\d*)n|)"+L+"*(?:([+-]|)"+L+"*(\\d+)|))"+L+"*\\)|)","i"),bool:new RegExp("^(?:"+K+")$","i"),needsContext:new RegExp("^"+L+"*[>+~]|:(even|odd|eq|gt|lt|nth|first|last)(?:\\("+L+"*((?:-\\d)?\\d*)"+L+"*\\)|)(?=[^-]|$)","i")},Y=/^(?:input|select|textarea|button)$/i,Z=/^h\d$/i,$=/^[^{]+\{\s*\[native \w/,_=/^(?:#([\w-]+)|(\w+)|\.([\w-]+))$/,ab=/[+~]/,bb=/'|\\/g,cb=new RegExp("\\\\([\\da-f]{1,6}"+L+"?|("+L+")|.)","ig"),db=function(a,b,c){var d="0x"+b-65536;return d!==d||c?b:0>d?String.fromCharCode(d+65536):String.fromCharCode(d>>10|55296,1023&d|56320)},eb=function(){m()};try{H.apply(E=I.call(v.childNodes),v.childNodes),E[v.childNodes.length].nodeType}catch(fb){H={apply:E.length?function(a,b){G.apply(a,I.call(b))}:function(a,b){var c=a.length,d=0;while(a[c++]=b[d++]);a.length=c-1}}}function gb(a,b,d,e){var f,h,j,k,l,o,r,s,w,x;if((b?b.ownerDocument||b:v)!==n&&m(b),b=b||n,d=d||[],k=b.nodeType,"string"!=typeof a||!a||1!==k&&9!==k&&11!==k)return d;if(!e&&p){if(11!==k&&(f=_.exec(a)))if(j=f[1]){if(9===k){if(h=b.getElementById(j),!h||!h.parentNode)return d;if(h.id===j)return d.push(h),d}else if(b.ownerDocument&&(h=b.ownerDocument.getElementById(j))&&t(b,h)&&h.id===j)return d.push(h),d}else{if(f[2])return H.apply(d,b.getElementsByTagName(a)),d;if((j=f[3])&&c.getElementsByClassName)return H.apply(d,b.getElementsByClassName(j)),d}if(c.qsa&&(!q||!q.test(a))){if(s=r=u,w=b,x=1!==k&&a,1===k&&"object"!==b.nodeName.toLowerCase()){o=g(a),(r=b.getAttribute("id"))?s=r.replace(bb,"\\$&"):b.setAttribute("id",s),s="[id='"+s+"'] ",l=o.length;while(l--)o[l]=s+rb(o[l]);w=ab.test(a)&&pb(b.parentNode)||b,x=o.join(",")}if(x)try{return H.apply(d,w.querySelectorAll(x)),d}catch(y){}finally{r||b.removeAttribute("id")}}}return i(a.replace(R,"$1"),b,d,e)}function hb(){var a=[];function b(c,e){return a.push(c+" ")>d.cacheLength&&delete b[a.shift()],b[c+" "]=e}return b}function ib(a){return a[u]=!0,a}function jb(a){var b=n.createElement("div");try{return!!a(b)}catch(c){return!1}finally{b.parentNode&&b.parentNode.removeChild(b),b=null}}function kb(a,b){var c=a.split("|"),e=a.length;while(e--)d.attrHandle[c[e]]=b}function lb(a,b){var c=b&&a,d=c&&1===a.nodeType&&1===b.nodeType&&(~b.sourceIndex||C)-(~a.sourceIndex||C);if(d)return d;if(c)while(c=c.nextSibling)if(c===b)return-1;return a?1:-1}function mb(a){return function(b){var c=b.nodeName.toLowerCase();return"input"===c&&b.type===a}}function nb(a){return function(b){var c=b.nodeName.toLowerCase();return("input"===c||"button"===c)&&b.type===a}}function ob(a){return ib(function(b){return b=+b,ib(function(c,d){var e,f=a([],c.length,b),g=f.length;while(g--)c[e=f[g]]&&(c[e]=!(d[e]=c[e]))})})}function pb(a){return a&&"undefined"!=typeof a.getElementsByTagName&&a}c=gb.support={},f=gb.isXML=function(a){var b=a&&(a.ownerDocument||a).documentElement;return b?"HTML"!==b.nodeName:!1},m=gb.setDocument=function(a){var b,e,g=a?a.ownerDocument||a:v;return g!==n&&9===g.nodeType&&g.documentElement?(n=g,o=g.documentElement,e=g.defaultView,e&&e!==e.top&&(e.addEventListener?e.addEventListener("unload",eb,!1):e.attachEvent&&e.attachEvent("onunload",eb)),p=!f(g),c.attributes=jb(function(a){return a.className="i",!a.getAttribute("className")}),c.getElementsByTagName=jb(function(a){return a.appendChild(g.createComment("")),!a.getElementsByTagName("*").length}),c.getElementsByClassName=$.test(g.getElementsByClassName),c.getById=jb(function(a){return o.appendChild(a).id=u,!g.getElementsByName||!g.getElementsByName(u).length}),c.getById?(d.find.ID=function(a,b){if("undefined"!=typeof b.getElementById&&p){var c=b.getElementById(a);return c&&c.parentNode?[c]:[]}},d.filter.ID=function(a){var b=a.replace(cb,db);return function(a){return a.getAttribute("id")===b}}):(delete d.find.ID,d.filter.ID=function(a){var b=a.replace(cb,db);return function(a){var c="undefined"!=typeof a.getAttributeNode&&a.getAttributeNode("id");return c&&c.value===b}}),d.find.TAG=c.getElementsByTagName?function(a,b){return"undefined"!=typeof b.getElementsByTagName?b.getElementsByTagName(a):c.qsa?b.querySelectorAll(a):void 0}:function(a,b){var c,d=[],e=0,f=b.getElementsByTagName(a);if("*"===a){while(c=f[e++])1===c.nodeType&&d.push(c);return d}return f},d.find.CLASS=c.getElementsByClassName&&function(a,b){return p?b.getElementsByClassName(a):void 0},r=[],q=[],(c.qsa=$.test(g.querySelectorAll))&&(jb(function(a){o.appendChild(a).innerHTML="",a.querySelectorAll("[msallowcapture^='']").length&&q.push("[*^$]="+L+"*(?:''|\"\")"),a.querySelectorAll("[selected]").length||q.push("\\["+L+"*(?:value|"+K+")"),a.querySelectorAll("[id~="+u+"-]").length||q.push("~="),a.querySelectorAll(":checked").length||q.push(":checked"),a.querySelectorAll("a#"+u+"+*").length||q.push(".#.+[+~]")}),jb(function(a){var b=g.createElement("input");b.setAttribute("type","hidden"),a.appendChild(b).setAttribute("name","D"),a.querySelectorAll("[name=d]").length&&q.push("name"+L+"*[*^$|!~]?="),a.querySelectorAll(":enabled").length||q.push(":enabled",":disabled"),a.querySelectorAll("*,:x"),q.push(",.*:")})),(c.matchesSelector=$.test(s=o.matches||o.webkitMatchesSelector||o.mozMatchesSelector||o.oMatchesSelector||o.msMatchesSelector))&&jb(function(a){c.disconnectedMatch=s.call(a,"div"),s.call(a,"[s!='']:x"),r.push("!=",P)}),q=q.length&&new RegExp(q.join("|")),r=r.length&&new RegExp(r.join("|")),b=$.test(o.compareDocumentPosition),t=b||$.test(o.contains)?function(a,b){var c=9===a.nodeType?a.documentElement:a,d=b&&b.parentNode;return a===d||!(!d||1!==d.nodeType||!(c.contains?c.contains(d):a.compareDocumentPosition&&16&a.compareDocumentPosition(d)))}:function(a,b){if(b)while(b=b.parentNode)if(b===a)return!0;return!1},B=b?function(a,b){if(a===b)return l=!0,0;var d=!a.compareDocumentPosition-!b.compareDocumentPosition;return d?d:(d=(a.ownerDocument||a)===(b.ownerDocument||b)?a.compareDocumentPosition(b):1,1&d||!c.sortDetached&&b.compareDocumentPosition(a)===d?a===g||a.ownerDocument===v&&t(v,a)?-1:b===g||b.ownerDocument===v&&t(v,b)?1:k?J(k,a)-J(k,b):0:4&d?-1:1)}:function(a,b){if(a===b)return l=!0,0;var c,d=0,e=a.parentNode,f=b.parentNode,h=[a],i=[b];if(!e||!f)return a===g?-1:b===g?1:e?-1:f?1:k?J(k,a)-J(k,b):0;if(e===f)return lb(a,b);c=a;while(c=c.parentNode)h.unshift(c);c=b;while(c=c.parentNode)i.unshift(c);while(h[d]===i[d])d++;return d?lb(h[d],i[d]):h[d]===v?-1:i[d]===v?1:0},g):n},gb.matches=function(a,b){return gb(a,null,null,b)},gb.matchesSelector=function(a,b){if((a.ownerDocument||a)!==n&&m(a),b=b.replace(U,"='$1']"),!(!c.matchesSelector||!p||r&&r.test(b)||q&&q.test(b)))try{var d=s.call(a,b);if(d||c.disconnectedMatch||a.document&&11!==a.document.nodeType)return d}catch(e){}return gb(b,n,null,[a]).length>0},gb.contains=function(a,b){return(a.ownerDocument||a)!==n&&m(a),t(a,b)},gb.attr=function(a,b){(a.ownerDocument||a)!==n&&m(a);var e=d.attrHandle[b.toLowerCase()],f=e&&D.call(d.attrHandle,b.toLowerCase())?e(a,b,!p):void 0;return void 0!==f?f:c.attributes||!p?a.getAttribute(b):(f=a.getAttributeNode(b))&&f.specified?f.value:null},gb.error=function(a){throw new Error("Syntax error, unrecognized expression: "+a)},gb.uniqueSort=function(a){var b,d=[],e=0,f=0;if(l=!c.detectDuplicates,k=!c.sortStable&&a.slice(0),a.sort(B),l){while(b=a[f++])b===a[f]&&(e=d.push(f));while(e--)a.splice(d[e],1)}return k=null,a},e=gb.getText=function(a){var b,c="",d=0,f=a.nodeType;if(f){if(1===f||9===f||11===f){if("string"==typeof a.textContent)return a.textContent;for(a=a.firstChild;a;a=a.nextSibling)c+=e(a)}else if(3===f||4===f)return a.nodeValue}else while(b=a[d++])c+=e(b);return c},d=gb.selectors={cacheLength:50,createPseudo:ib,match:X,attrHandle:{},find:{},relative:{">":{dir:"parentNode",first:!0}," ":{dir:"parentNode"},"+":{dir:"previousSibling",first:!0},"~":{dir:"previousSibling"}},preFilter:{ATTR:function(a){return a[1]=a[1].replace(cb,db),a[3]=(a[3]||a[4]||a[5]||"").replace(cb,db),"~="===a[2]&&(a[3]=" "+a[3]+" "),a.slice(0,4)},CHILD:function(a){return a[1]=a[1].toLowerCase(),"nth"===a[1].slice(0,3)?(a[3]||gb.error(a[0]),a[4]=+(a[4]?a[5]+(a[6]||1):2*("even"===a[3]||"odd"===a[3])),a[5]=+(a[7]+a[8]||"odd"===a[3])):a[3]&&gb.error(a[0]),a},PSEUDO:function(a){var b,c=!a[6]&&a[2];return X.CHILD.test(a[0])?null:(a[3]?a[2]=a[4]||a[5]||"":c&&V.test(c)&&(b=g(c,!0))&&(b=c.indexOf(")",c.length-b)-c.length)&&(a[0]=a[0].slice(0,b),a[2]=c.slice(0,b)),a.slice(0,3))}},filter:{TAG:function(a){var b=a.replace(cb,db).toLowerCase();return"*"===a?function(){return!0}:function(a){return a.nodeName&&a.nodeName.toLowerCase()===b}},CLASS:function(a){var b=y[a+" "];return b||(b=new RegExp("(^|"+L+")"+a+"("+L+"|$)"))&&y(a,function(a){return b.test("string"==typeof a.className&&a.className||"undefined"!=typeof a.getAttribute&&a.getAttribute("class")||"")})},ATTR:function(a,b,c){return function(d){var e=gb.attr(d,a);return null==e?"!="===b:b?(e+="","="===b?e===c:"!="===b?e!==c:"^="===b?c&&0===e.indexOf(c):"*="===b?c&&e.indexOf(c)>-1:"$="===b?c&&e.slice(-c.length)===c:"~="===b?(" "+e.replace(Q," ")+" ").indexOf(c)>-1:"|="===b?e===c||e.slice(0,c.length+1)===c+"-":!1):!0}},CHILD:function(a,b,c,d,e){var f="nth"!==a.slice(0,3),g="last"!==a.slice(-4),h="of-type"===b;return 1===d&&0===e?function(a){return!!a.parentNode}:function(b,c,i){var j,k,l,m,n,o,p=f!==g?"nextSibling":"previousSibling",q=b.parentNode,r=h&&b.nodeName.toLowerCase(),s=!i&&!h;if(q){if(f){while(p){l=b;while(l=l[p])if(h?l.nodeName.toLowerCase()===r:1===l.nodeType)return!1;o=p="only"===a&&!o&&"nextSibling"}return!0}if(o=[g?q.firstChild:q.lastChild],g&&s){k=q[u]||(q[u]={}),j=k[a]||[],n=j[0]===w&&j[1],m=j[0]===w&&j[2],l=n&&q.childNodes[n];while(l=++n&&l&&l[p]||(m=n=0)||o.pop())if(1===l.nodeType&&++m&&l===b){k[a]=[w,n,m];break}}else if(s&&(j=(b[u]||(b[u]={}))[a])&&j[0]===w)m=j[1];else while(l=++n&&l&&l[p]||(m=n=0)||o.pop())if((h?l.nodeName.toLowerCase()===r:1===l.nodeType)&&++m&&(s&&((l[u]||(l[u]={}))[a]=[w,m]),l===b))break;return m-=e,m===d||m%d===0&&m/d>=0}}},PSEUDO:function(a,b){var c,e=d.pseudos[a]||d.setFilters[a.toLowerCase()]||gb.error("unsupported pseudo: "+a);return e[u]?e(b):e.length>1?(c=[a,a,"",b],d.setFilters.hasOwnProperty(a.toLowerCase())?ib(function(a,c){var d,f=e(a,b),g=f.length;while(g--)d=J(a,f[g]),a[d]=!(c[d]=f[g])}):function(a){return e(a,0,c)}):e}},pseudos:{not:ib(function(a){var b=[],c=[],d=h(a.replace(R,"$1"));return d[u]?ib(function(a,b,c,e){var f,g=d(a,null,e,[]),h=a.length;while(h--)(f=g[h])&&(a[h]=!(b[h]=f))}):function(a,e,f){return b[0]=a,d(b,null,f,c),b[0]=null,!c.pop()}}),has:ib(function(a){return function(b){return gb(a,b).length>0}}),contains:ib(function(a){return a=a.replace(cb,db),function(b){return(b.textContent||b.innerText||e(b)).indexOf(a)>-1}}),lang:ib(function(a){return W.test(a||"")||gb.error("unsupported lang: "+a),a=a.replace(cb,db).toLowerCase(),function(b){var c;do if(c=p?b.lang:b.getAttribute("xml:lang")||b.getAttribute("lang"))return c=c.toLowerCase(),c===a||0===c.indexOf(a+"-");while((b=b.parentNode)&&1===b.nodeType);return!1}}),target:function(b){var c=a.location&&a.location.hash;return c&&c.slice(1)===b.id},root:function(a){return a===o},focus:function(a){return a===n.activeElement&&(!n.hasFocus||n.hasFocus())&&!!(a.type||a.href||~a.tabIndex)},enabled:function(a){return a.disabled===!1},disabled:function(a){return a.disabled===!0},checked:function(a){var b=a.nodeName.toLowerCase();return"input"===b&&!!a.checked||"option"===b&&!!a.selected},selected:function(a){return a.parentNode&&a.parentNode.selectedIndex,a.selected===!0},empty:function(a){for(a=a.firstChild;a;a=a.nextSibling)if(a.nodeType<6)return!1;return!0},parent:function(a){return!d.pseudos.empty(a)},header:function(a){return Z.test(a.nodeName)},input:function(a){return Y.test(a.nodeName)},button:function(a){var b=a.nodeName.toLowerCase();return"input"===b&&"button"===a.type||"button"===b},text:function(a){var b;return"input"===a.nodeName.toLowerCase()&&"text"===a.type&&(null==(b=a.getAttribute("type"))||"text"===b.toLowerCase())},first:ob(function(){return[0]}),last:ob(function(a,b){return[b-1]}),eq:ob(function(a,b,c){return[0>c?c+b:c]}),even:ob(function(a,b){for(var c=0;b>c;c+=2)a.push(c);return a}),odd:ob(function(a,b){for(var c=1;b>c;c+=2)a.push(c);return a}),lt:ob(function(a,b,c){for(var d=0>c?c+b:c;--d>=0;)a.push(d);return a}),gt:ob(function(a,b,c){for(var d=0>c?c+b:c;++db;b++)d+=a[b].value;return d}function sb(a,b,c){var d=b.dir,e=c&&"parentNode"===d,f=x++;return b.first?function(b,c,f){while(b=b[d])if(1===b.nodeType||e)return a(b,c,f)}:function(b,c,g){var h,i,j=[w,f];if(g){while(b=b[d])if((1===b.nodeType||e)&&a(b,c,g))return!0}else while(b=b[d])if(1===b.nodeType||e){if(i=b[u]||(b[u]={}),(h=i[d])&&h[0]===w&&h[1]===f)return j[2]=h[2];if(i[d]=j,j[2]=a(b,c,g))return!0}}}function tb(a){return a.length>1?function(b,c,d){var e=a.length;while(e--)if(!a[e](b,c,d))return!1;return!0}:a[0]}function ub(a,b,c){for(var d=0,e=b.length;e>d;d++)gb(a,b[d],c);return c}function vb(a,b,c,d,e){for(var f,g=[],h=0,i=a.length,j=null!=b;i>h;h++)(f=a[h])&&(!c||c(f,d,e))&&(g.push(f),j&&b.push(h));return g}function wb(a,b,c,d,e,f){return d&&!d[u]&&(d=wb(d)),e&&!e[u]&&(e=wb(e,f)),ib(function(f,g,h,i){var j,k,l,m=[],n=[],o=g.length,p=f||ub(b||"*",h.nodeType?[h]:h,[]),q=!a||!f&&b?p:vb(p,m,a,h,i),r=c?e||(f?a:o||d)?[]:g:q;if(c&&c(q,r,h,i),d){j=vb(r,n),d(j,[],h,i),k=j.length;while(k--)(l=j[k])&&(r[n[k]]=!(q[n[k]]=l))}if(f){if(e||a){if(e){j=[],k=r.length;while(k--)(l=r[k])&&j.push(q[k]=l);e(null,r=[],j,i)}k=r.length;while(k--)(l=r[k])&&(j=e?J(f,l):m[k])>-1&&(f[j]=!(g[j]=l))}}else r=vb(r===g?r.splice(o,r.length):r),e?e(null,g,r,i):H.apply(g,r)})}function xb(a){for(var b,c,e,f=a.length,g=d.relative[a[0].type],h=g||d.relative[" "],i=g?1:0,k=sb(function(a){return a===b},h,!0),l=sb(function(a){return J(b,a)>-1},h,!0),m=[function(a,c,d){var e=!g&&(d||c!==j)||((b=c).nodeType?k(a,c,d):l(a,c,d));return b=null,e}];f>i;i++)if(c=d.relative[a[i].type])m=[sb(tb(m),c)];else{if(c=d.filter[a[i].type].apply(null,a[i].matches),c[u]){for(e=++i;f>e;e++)if(d.relative[a[e].type])break;return wb(i>1&&tb(m),i>1&&rb(a.slice(0,i-1).concat({value:" "===a[i-2].type?"*":""})).replace(R,"$1"),c,e>i&&xb(a.slice(i,e)),f>e&&xb(a=a.slice(e)),f>e&&rb(a))}m.push(c)}return tb(m)}function yb(a,b){var c=b.length>0,e=a.length>0,f=function(f,g,h,i,k){var l,m,o,p=0,q="0",r=f&&[],s=[],t=j,u=f||e&&d.find.TAG("*",k),v=w+=null==t?1:Math.random()||.1,x=u.length;for(k&&(j=g!==n&&g);q!==x&&null!=(l=u[q]);q++){if(e&&l){m=0;while(o=a[m++])if(o(l,g,h)){i.push(l);break}k&&(w=v)}c&&((l=!o&&l)&&p--,f&&r.push(l))}if(p+=q,c&&q!==p){m=0;while(o=b[m++])o(r,s,g,h);if(f){if(p>0)while(q--)r[q]||s[q]||(s[q]=F.call(i));s=vb(s)}H.apply(i,s),k&&!f&&s.length>0&&p+b.length>1&&gb.uniqueSort(i)}return k&&(w=v,j=t),r};return c?ib(f):f}return h=gb.compile=function(a,b){var c,d=[],e=[],f=A[a+" "];if(!f){b||(b=g(a)),c=b.length;while(c--)f=xb(b[c]),f[u]?d.push(f):e.push(f);f=A(a,yb(e,d)),f.selector=a}return f},i=gb.select=function(a,b,e,f){var i,j,k,l,m,n="function"==typeof a&&a,o=!f&&g(a=n.selector||a);if(e=e||[],1===o.length){if(j=o[0]=o[0].slice(0),j.length>2&&"ID"===(k=j[0]).type&&c.getById&&9===b.nodeType&&p&&d.relative[j[1].type]){if(b=(d.find.ID(k.matches[0].replace(cb,db),b)||[])[0],!b)return e;n&&(b=b.parentNode),a=a.slice(j.shift().value.length)}i=X.needsContext.test(a)?0:j.length;while(i--){if(k=j[i],d.relative[l=k.type])break;if((m=d.find[l])&&(f=m(k.matches[0].replace(cb,db),ab.test(j[0].type)&&pb(b.parentNode)||b))){if(j.splice(i,1),a=f.length&&rb(j),!a)return H.apply(e,f),e;break}}}return(n||h(a,o))(f,b,!p,e,ab.test(a)&&pb(b.parentNode)||b),e},c.sortStable=u.split("").sort(B).join("")===u,c.detectDuplicates=!!l,m(),c.sortDetached=jb(function(a){return 1&a.compareDocumentPosition(n.createElement("div"))}),jb(function(a){return a.innerHTML="","#"===a.firstChild.getAttribute("href")})||kb("type|href|height|width",function(a,b,c){return c?void 0:a.getAttribute(b,"type"===b.toLowerCase()?1:2)}),c.attributes&&jb(function(a){return a.innerHTML="",a.firstChild.setAttribute("value",""),""===a.firstChild.getAttribute("value")})||kb("value",function(a,b,c){return c||"input"!==a.nodeName.toLowerCase()?void 0:a.defaultValue}),jb(function(a){return null==a.getAttribute("disabled")})||kb(K,function(a,b,c){var d;return c?void 0:a[b]===!0?b.toLowerCase():(d=a.getAttributeNode(b))&&d.specified?d.value:null}),gb}(a);n.find=t,n.expr=t.selectors,n.expr[":"]=n.expr.pseudos,n.unique=t.uniqueSort,n.text=t.getText,n.isXMLDoc=t.isXML,n.contains=t.contains;var u=n.expr.match.needsContext,v=/^<(\w+)\s*\/?>(?:<\/\1>|)$/,w=/^.[^:#\[\.,]*$/;function x(a,b,c){if(n.isFunction(b))return n.grep(a,function(a,d){return!!b.call(a,d,a)!==c});if(b.nodeType)return n.grep(a,function(a){return a===b!==c});if("string"==typeof b){if(w.test(b))return n.filter(b,a,c);b=n.filter(b,a)}return n.grep(a,function(a){return g.call(b,a)>=0!==c})}n.filter=function(a,b,c){var d=b[0];return c&&(a=":not("+a+")"),1===b.length&&1===d.nodeType?n.find.matchesSelector(d,a)?[d]:[]:n.find.matches(a,n.grep(b,function(a){return 1===a.nodeType}))},n.fn.extend({find:function(a){var b,c=this.length,d=[],e=this;if("string"!=typeof a)return this.pushStack(n(a).filter(function(){for(b=0;c>b;b++)if(n.contains(e[b],this))return!0}));for(b=0;c>b;b++)n.find(a,e[b],d);return d=this.pushStack(c>1?n.unique(d):d),d.selector=this.selector?this.selector+" "+a:a,d},filter:function(a){return this.pushStack(x(this,a||[],!1))},not:function(a){return this.pushStack(x(this,a||[],!0))},is:function(a){return!!x(this,"string"==typeof a&&u.test(a)?n(a):a||[],!1).length}});var y,z=/^(?:\s*(<[\w\W]+>)[^>]*|#([\w-]*))$/,A=n.fn.init=function(a,b){var c,d;if(!a)return this;if("string"==typeof a){if(c="<"===a[0]&&">"===a[a.length-1]&&a.length>=3?[null,a,null]:z.exec(a),!c||!c[1]&&b)return!b||b.jquery?(b||y).find(a):this.constructor(b).find(a);if(c[1]){if(b=b instanceof n?b[0]:b,n.merge(this,n.parseHTML(c[1],b&&b.nodeType?b.ownerDocument||b:l,!0)),v.test(c[1])&&n.isPlainObject(b))for(c in b)n.isFunction(this[c])?this[c](b[c]):this.attr(c,b[c]);return this}return d=l.getElementById(c[2]),d&&d.parentNode&&(this.length=1,this[0]=d),this.context=l,this.selector=a,this}return a.nodeType?(this.context=this[0]=a,this.length=1,this):n.isFunction(a)?"undefined"!=typeof y.ready?y.ready(a):a(n):(void 0!==a.selector&&(this.selector=a.selector,this.context=a.context),n.makeArray(a,this))};A.prototype=n.fn,y=n(l);var B=/^(?:parents|prev(?:Until|All))/,C={children:!0,contents:!0,next:!0,prev:!0};n.extend({dir:function(a,b,c){var d=[],e=void 0!==c;while((a=a[b])&&9!==a.nodeType)if(1===a.nodeType){if(e&&n(a).is(c))break;d.push(a)}return d},sibling:function(a,b){for(var c=[];a;a=a.nextSibling)1===a.nodeType&&a!==b&&c.push(a);return c}}),n.fn.extend({has:function(a){var b=n(a,this),c=b.length;return this.filter(function(){for(var a=0;c>a;a++)if(n.contains(this,b[a]))return!0})},closest:function(a,b){for(var c,d=0,e=this.length,f=[],g=u.test(a)||"string"!=typeof a?n(a,b||this.context):0;e>d;d++)for(c=this[d];c&&c!==b;c=c.parentNode)if(c.nodeType<11&&(g?g.index(c)>-1:1===c.nodeType&&n.find.matchesSelector(c,a))){f.push(c);break}return this.pushStack(f.length>1?n.unique(f):f)},index:function(a){return a?"string"==typeof a?g.call(n(a),this[0]):g.call(this,a.jquery?a[0]:a):this[0]&&this[0].parentNode?this.first().prevAll().length:-1},add:function(a,b){return this.pushStack(n.unique(n.merge(this.get(),n(a,b))))},addBack:function(a){return this.add(null==a?this.prevObject:this.prevObject.filter(a))}});function D(a,b){while((a=a[b])&&1!==a.nodeType);return a}n.each({parent:function(a){var b=a.parentNode;return b&&11!==b.nodeType?b:null},parents:function(a){return n.dir(a,"parentNode")},parentsUntil:function(a,b,c){return n.dir(a,"parentNode",c)},next:function(a){return D(a,"nextSibling")},prev:function(a){return D(a,"previousSibling")},nextAll:function(a){return n.dir(a,"nextSibling")},prevAll:function(a){return n.dir(a,"previousSibling")},nextUntil:function(a,b,c){return n.dir(a,"nextSibling",c)},prevUntil:function(a,b,c){return n.dir(a,"previousSibling",c)},siblings:function(a){return n.sibling((a.parentNode||{}).firstChild,a)},children:function(a){return n.sibling(a.firstChild)},contents:function(a){return a.contentDocument||n.merge([],a.childNodes)}},function(a,b){n.fn[a]=function(c,d){var e=n.map(this,b,c);return"Until"!==a.slice(-5)&&(d=c),d&&"string"==typeof d&&(e=n.filter(d,e)),this.length>1&&(C[a]||n.unique(e),B.test(a)&&e.reverse()),this.pushStack(e)}});var E=/\S+/g,F={};function G(a){var b=F[a]={};return n.each(a.match(E)||[],function(a,c){b[c]=!0}),b}n.Callbacks=function(a){a="string"==typeof a?F[a]||G(a):n.extend({},a);var b,c,d,e,f,g,h=[],i=!a.once&&[],j=function(l){for(b=a.memory&&l,c=!0,g=e||0,e=0,f=h.length,d=!0;h&&f>g;g++)if(h[g].apply(l[0],l[1])===!1&&a.stopOnFalse){b=!1;break}d=!1,h&&(i?i.length&&j(i.shift()):b?h=[]:k.disable())},k={add:function(){if(h){var c=h.length;!function g(b){n.each(b,function(b,c){var d=n.type(c);"function"===d?a.unique&&k.has(c)||h.push(c):c&&c.length&&"string"!==d&&g(c)})}(arguments),d?f=h.length:b&&(e=c,j(b))}return this},remove:function(){return h&&n.each(arguments,function(a,b){var c;while((c=n.inArray(b,h,c))>-1)h.splice(c,1),d&&(f>=c&&f--,g>=c&&g--)}),this},has:function(a){return a?n.inArray(a,h)>-1:!(!h||!h.length)},empty:function(){return h=[],f=0,this},disable:function(){return h=i=b=void 0,this},disabled:function(){return!h},lock:function(){return i=void 0,b||k.disable(),this},locked:function(){return!i},fireWith:function(a,b){return!h||c&&!i||(b=b||[],b=[a,b.slice?b.slice():b],d?i.push(b):j(b)),this},fire:function(){return k.fireWith(this,arguments),this},fired:function(){return!!c}};return k},n.extend({Deferred:function(a){var b=[["resolve","done",n.Callbacks("once memory"),"resolved"],["reject","fail",n.Callbacks("once memory"),"rejected"],["notify","progress",n.Callbacks("memory")]],c="pending",d={state:function(){return c},always:function(){return e.done(arguments).fail(arguments),this},then:function(){var a=arguments;return n.Deferred(function(c){n.each(b,function(b,f){var g=n.isFunction(a[b])&&a[b];e[f[1]](function(){var a=g&&g.apply(this,arguments);a&&n.isFunction(a.promise)?a.promise().done(c.resolve).fail(c.reject).progress(c.notify):c[f[0]+"With"](this===d?c.promise():this,g?[a]:arguments)})}),a=null}).promise()},promise:function(a){return null!=a?n.extend(a,d):d}},e={};return d.pipe=d.then,n.each(b,function(a,f){var g=f[2],h=f[3];d[f[1]]=g.add,h&&g.add(function(){c=h},b[1^a][2].disable,b[2][2].lock),e[f[0]]=function(){return e[f[0]+"With"](this===e?d:this,arguments),this},e[f[0]+"With"]=g.fireWith}),d.promise(e),a&&a.call(e,e),e},when:function(a){var b=0,c=d.call(arguments),e=c.length,f=1!==e||a&&n.isFunction(a.promise)?e:0,g=1===f?a:n.Deferred(),h=function(a,b,c){return function(e){b[a]=this,c[a]=arguments.length>1?d.call(arguments):e,c===i?g.notifyWith(b,c):--f||g.resolveWith(b,c)}},i,j,k;if(e>1)for(i=new Array(e),j=new Array(e),k=new Array(e);e>b;b++)c[b]&&n.isFunction(c[b].promise)?c[b].promise().done(h(b,k,c)).fail(g.reject).progress(h(b,j,i)):--f;return f||g.resolveWith(k,c),g.promise()}});var H;n.fn.ready=function(a){return n.ready.promise().done(a),this},n.extend({isReady:!1,readyWait:1,holdReady:function(a){a?n.readyWait++:n.ready(!0)},ready:function(a){(a===!0?--n.readyWait:n.isReady)||(n.isReady=!0,a!==!0&&--n.readyWait>0||(H.resolveWith(l,[n]),n.fn.triggerHandler&&(n(l).triggerHandler("ready"),n(l).off("ready"))))}});function I(){l.removeEventListener("DOMContentLoaded",I,!1),a.removeEventListener("load",I,!1),n.ready()}n.ready.promise=function(b){return H||(H=n.Deferred(),"complete"===l.readyState?setTimeout(n.ready):(l.addEventListener("DOMContentLoaded",I,!1),a.addEventListener("load",I,!1))),H.promise(b)},n.ready.promise();var J=n.access=function(a,b,c,d,e,f,g){var h=0,i=a.length,j=null==c;if("object"===n.type(c)){e=!0;for(h in c)n.access(a,b,h,c[h],!0,f,g)}else if(void 0!==d&&(e=!0,n.isFunction(d)||(g=!0),j&&(g?(b.call(a,d),b=null):(j=b,b=function(a,b,c){return j.call(n(a),c)})),b))for(;i>h;h++)b(a[h],c,g?d:d.call(a[h],h,b(a[h],c)));return e?a:j?b.call(a):i?b(a[0],c):f};n.acceptData=function(a){return 1===a.nodeType||9===a.nodeType||!+a.nodeType};function K(){Object.defineProperty(this.cache={},0,{get:function(){return{}}}),this.expando=n.expando+K.uid++}K.uid=1,K.accepts=n.acceptData,K.prototype={key:function(a){if(!K.accepts(a))return 0;var b={},c=a[this.expando];if(!c){c=K.uid++;try{b[this.expando]={value:c},Object.defineProperties(a,b)}catch(d){b[this.expando]=c,n.extend(a,b)}}return this.cache[c]||(this.cache[c]={}),c},set:function(a,b,c){var d,e=this.key(a),f=this.cache[e];if("string"==typeof b)f[b]=c;else if(n.isEmptyObject(f))n.extend(this.cache[e],b);else for(d in b)f[d]=b[d];return f},get:function(a,b){var c=this.cache[this.key(a)];return void 0===b?c:c[b]},access:function(a,b,c){var d;return void 0===b||b&&"string"==typeof b&&void 0===c?(d=this.get(a,b),void 0!==d?d:this.get(a,n.camelCase(b))):(this.set(a,b,c),void 0!==c?c:b)},remove:function(a,b){var c,d,e,f=this.key(a),g=this.cache[f];if(void 0===b)this.cache[f]={};else{n.isArray(b)?d=b.concat(b.map(n.camelCase)):(e=n.camelCase(b),b in g?d=[b,e]:(d=e,d=d in g?[d]:d.match(E)||[])),c=d.length;while(c--)delete g[d[c]]}},hasData:function(a){return!n.isEmptyObject(this.cache[a[this.expando]]||{})},discard:function(a){a[this.expando]&&delete this.cache[a[this.expando]]}};var L=new K,M=new K,N=/^(?:\{[\w\W]*\}|\[[\w\W]*\])$/,O=/([A-Z])/g;function P(a,b,c){var d;if(void 0===c&&1===a.nodeType)if(d="data-"+b.replace(O,"-$1").toLowerCase(),c=a.getAttribute(d),"string"==typeof c){try{c="true"===c?!0:"false"===c?!1:"null"===c?null:+c+""===c?+c:N.test(c)?n.parseJSON(c):c}catch(e){}M.set(a,b,c)}else c=void 0;return c}n.extend({hasData:function(a){return M.hasData(a)||L.hasData(a)},data:function(a,b,c){return M.access(a,b,c) +},removeData:function(a,b){M.remove(a,b)},_data:function(a,b,c){return L.access(a,b,c)},_removeData:function(a,b){L.remove(a,b)}}),n.fn.extend({data:function(a,b){var c,d,e,f=this[0],g=f&&f.attributes;if(void 0===a){if(this.length&&(e=M.get(f),1===f.nodeType&&!L.get(f,"hasDataAttrs"))){c=g.length;while(c--)g[c]&&(d=g[c].name,0===d.indexOf("data-")&&(d=n.camelCase(d.slice(5)),P(f,d,e[d])));L.set(f,"hasDataAttrs",!0)}return e}return"object"==typeof a?this.each(function(){M.set(this,a)}):J(this,function(b){var c,d=n.camelCase(a);if(f&&void 0===b){if(c=M.get(f,a),void 0!==c)return c;if(c=M.get(f,d),void 0!==c)return c;if(c=P(f,d,void 0),void 0!==c)return c}else this.each(function(){var c=M.get(this,d);M.set(this,d,b),-1!==a.indexOf("-")&&void 0!==c&&M.set(this,a,b)})},null,b,arguments.length>1,null,!0)},removeData:function(a){return this.each(function(){M.remove(this,a)})}}),n.extend({queue:function(a,b,c){var d;return a?(b=(b||"fx")+"queue",d=L.get(a,b),c&&(!d||n.isArray(c)?d=L.access(a,b,n.makeArray(c)):d.push(c)),d||[]):void 0},dequeue:function(a,b){b=b||"fx";var c=n.queue(a,b),d=c.length,e=c.shift(),f=n._queueHooks(a,b),g=function(){n.dequeue(a,b)};"inprogress"===e&&(e=c.shift(),d--),e&&("fx"===b&&c.unshift("inprogress"),delete f.stop,e.call(a,g,f)),!d&&f&&f.empty.fire()},_queueHooks:function(a,b){var c=b+"queueHooks";return L.get(a,c)||L.access(a,c,{empty:n.Callbacks("once memory").add(function(){L.remove(a,[b+"queue",c])})})}}),n.fn.extend({queue:function(a,b){var c=2;return"string"!=typeof a&&(b=a,a="fx",c--),arguments.lengthx",k.noCloneChecked=!!b.cloneNode(!0).lastChild.defaultValue}();var U="undefined";k.focusinBubbles="onfocusin"in a;var V=/^key/,W=/^(?:mouse|pointer|contextmenu)|click/,X=/^(?:focusinfocus|focusoutblur)$/,Y=/^([^.]*)(?:\.(.+)|)$/;function Z(){return!0}function $(){return!1}function _(){try{return l.activeElement}catch(a){}}n.event={global:{},add:function(a,b,c,d,e){var f,g,h,i,j,k,l,m,o,p,q,r=L.get(a);if(r){c.handler&&(f=c,c=f.handler,e=f.selector),c.guid||(c.guid=n.guid++),(i=r.events)||(i=r.events={}),(g=r.handle)||(g=r.handle=function(b){return typeof n!==U&&n.event.triggered!==b.type?n.event.dispatch.apply(a,arguments):void 0}),b=(b||"").match(E)||[""],j=b.length;while(j--)h=Y.exec(b[j])||[],o=q=h[1],p=(h[2]||"").split(".").sort(),o&&(l=n.event.special[o]||{},o=(e?l.delegateType:l.bindType)||o,l=n.event.special[o]||{},k=n.extend({type:o,origType:q,data:d,handler:c,guid:c.guid,selector:e,needsContext:e&&n.expr.match.needsContext.test(e),namespace:p.join(".")},f),(m=i[o])||(m=i[o]=[],m.delegateCount=0,l.setup&&l.setup.call(a,d,p,g)!==!1||a.addEventListener&&a.addEventListener(o,g,!1)),l.add&&(l.add.call(a,k),k.handler.guid||(k.handler.guid=c.guid)),e?m.splice(m.delegateCount++,0,k):m.push(k),n.event.global[o]=!0)}},remove:function(a,b,c,d,e){var f,g,h,i,j,k,l,m,o,p,q,r=L.hasData(a)&&L.get(a);if(r&&(i=r.events)){b=(b||"").match(E)||[""],j=b.length;while(j--)if(h=Y.exec(b[j])||[],o=q=h[1],p=(h[2]||"").split(".").sort(),o){l=n.event.special[o]||{},o=(d?l.delegateType:l.bindType)||o,m=i[o]||[],h=h[2]&&new RegExp("(^|\\.)"+p.join("\\.(?:.*\\.|)")+"(\\.|$)"),g=f=m.length;while(f--)k=m[f],!e&&q!==k.origType||c&&c.guid!==k.guid||h&&!h.test(k.namespace)||d&&d!==k.selector&&("**"!==d||!k.selector)||(m.splice(f,1),k.selector&&m.delegateCount--,l.remove&&l.remove.call(a,k));g&&!m.length&&(l.teardown&&l.teardown.call(a,p,r.handle)!==!1||n.removeEvent(a,o,r.handle),delete i[o])}else for(o in i)n.event.remove(a,o+b[j],c,d,!0);n.isEmptyObject(i)&&(delete r.handle,L.remove(a,"events"))}},trigger:function(b,c,d,e){var f,g,h,i,k,m,o,p=[d||l],q=j.call(b,"type")?b.type:b,r=j.call(b,"namespace")?b.namespace.split("."):[];if(g=h=d=d||l,3!==d.nodeType&&8!==d.nodeType&&!X.test(q+n.event.triggered)&&(q.indexOf(".")>=0&&(r=q.split("."),q=r.shift(),r.sort()),k=q.indexOf(":")<0&&"on"+q,b=b[n.expando]?b:new n.Event(q,"object"==typeof b&&b),b.isTrigger=e?2:3,b.namespace=r.join("."),b.namespace_re=b.namespace?new RegExp("(^|\\.)"+r.join("\\.(?:.*\\.|)")+"(\\.|$)"):null,b.result=void 0,b.target||(b.target=d),c=null==c?[b]:n.makeArray(c,[b]),o=n.event.special[q]||{},e||!o.trigger||o.trigger.apply(d,c)!==!1)){if(!e&&!o.noBubble&&!n.isWindow(d)){for(i=o.delegateType||q,X.test(i+q)||(g=g.parentNode);g;g=g.parentNode)p.push(g),h=g;h===(d.ownerDocument||l)&&p.push(h.defaultView||h.parentWindow||a)}f=0;while((g=p[f++])&&!b.isPropagationStopped())b.type=f>1?i:o.bindType||q,m=(L.get(g,"events")||{})[b.type]&&L.get(g,"handle"),m&&m.apply(g,c),m=k&&g[k],m&&m.apply&&n.acceptData(g)&&(b.result=m.apply(g,c),b.result===!1&&b.preventDefault());return b.type=q,e||b.isDefaultPrevented()||o._default&&o._default.apply(p.pop(),c)!==!1||!n.acceptData(d)||k&&n.isFunction(d[q])&&!n.isWindow(d)&&(h=d[k],h&&(d[k]=null),n.event.triggered=q,d[q](),n.event.triggered=void 0,h&&(d[k]=h)),b.result}},dispatch:function(a){a=n.event.fix(a);var b,c,e,f,g,h=[],i=d.call(arguments),j=(L.get(this,"events")||{})[a.type]||[],k=n.event.special[a.type]||{};if(i[0]=a,a.delegateTarget=this,!k.preDispatch||k.preDispatch.call(this,a)!==!1){h=n.event.handlers.call(this,a,j),b=0;while((f=h[b++])&&!a.isPropagationStopped()){a.currentTarget=f.elem,c=0;while((g=f.handlers[c++])&&!a.isImmediatePropagationStopped())(!a.namespace_re||a.namespace_re.test(g.namespace))&&(a.handleObj=g,a.data=g.data,e=((n.event.special[g.origType]||{}).handle||g.handler).apply(f.elem,i),void 0!==e&&(a.result=e)===!1&&(a.preventDefault(),a.stopPropagation()))}return k.postDispatch&&k.postDispatch.call(this,a),a.result}},handlers:function(a,b){var c,d,e,f,g=[],h=b.delegateCount,i=a.target;if(h&&i.nodeType&&(!a.button||"click"!==a.type))for(;i!==this;i=i.parentNode||this)if(i.disabled!==!0||"click"!==a.type){for(d=[],c=0;h>c;c++)f=b[c],e=f.selector+" ",void 0===d[e]&&(d[e]=f.needsContext?n(e,this).index(i)>=0:n.find(e,this,null,[i]).length),d[e]&&d.push(f);d.length&&g.push({elem:i,handlers:d})}return h]*)\/>/gi,bb=/<([\w:]+)/,cb=/<|&#?\w+;/,db=/<(?:script|style|link)/i,eb=/checked\s*(?:[^=]|=\s*.checked.)/i,fb=/^$|\/(?:java|ecma)script/i,gb=/^true\/(.*)/,hb=/^\s*\s*$/g,ib={option:[1,""],thead:[1,"","
"],col:[2,"","
"],tr:[2,"","
"],td:[3,"","
"],_default:[0,"",""]};ib.optgroup=ib.option,ib.tbody=ib.tfoot=ib.colgroup=ib.caption=ib.thead,ib.th=ib.td;function jb(a,b){return n.nodeName(a,"table")&&n.nodeName(11!==b.nodeType?b:b.firstChild,"tr")?a.getElementsByTagName("tbody")[0]||a.appendChild(a.ownerDocument.createElement("tbody")):a}function kb(a){return a.type=(null!==a.getAttribute("type"))+"/"+a.type,a}function lb(a){var b=gb.exec(a.type);return b?a.type=b[1]:a.removeAttribute("type"),a}function mb(a,b){for(var c=0,d=a.length;d>c;c++)L.set(a[c],"globalEval",!b||L.get(b[c],"globalEval"))}function nb(a,b){var c,d,e,f,g,h,i,j;if(1===b.nodeType){if(L.hasData(a)&&(f=L.access(a),g=L.set(b,f),j=f.events)){delete g.handle,g.events={};for(e in j)for(c=0,d=j[e].length;d>c;c++)n.event.add(b,e,j[e][c])}M.hasData(a)&&(h=M.access(a),i=n.extend({},h),M.set(b,i))}}function ob(a,b){var c=a.getElementsByTagName?a.getElementsByTagName(b||"*"):a.querySelectorAll?a.querySelectorAll(b||"*"):[];return void 0===b||b&&n.nodeName(a,b)?n.merge([a],c):c}function pb(a,b){var c=b.nodeName.toLowerCase();"input"===c&&T.test(a.type)?b.checked=a.checked:("input"===c||"textarea"===c)&&(b.defaultValue=a.defaultValue)}n.extend({clone:function(a,b,c){var d,e,f,g,h=a.cloneNode(!0),i=n.contains(a.ownerDocument,a);if(!(k.noCloneChecked||1!==a.nodeType&&11!==a.nodeType||n.isXMLDoc(a)))for(g=ob(h),f=ob(a),d=0,e=f.length;e>d;d++)pb(f[d],g[d]);if(b)if(c)for(f=f||ob(a),g=g||ob(h),d=0,e=f.length;e>d;d++)nb(f[d],g[d]);else nb(a,h);return g=ob(h,"script"),g.length>0&&mb(g,!i&&ob(a,"script")),h},buildFragment:function(a,b,c,d){for(var e,f,g,h,i,j,k=b.createDocumentFragment(),l=[],m=0,o=a.length;o>m;m++)if(e=a[m],e||0===e)if("object"===n.type(e))n.merge(l,e.nodeType?[e]:e);else if(cb.test(e)){f=f||k.appendChild(b.createElement("div")),g=(bb.exec(e)||["",""])[1].toLowerCase(),h=ib[g]||ib._default,f.innerHTML=h[1]+e.replace(ab,"<$1>")+h[2],j=h[0];while(j--)f=f.lastChild;n.merge(l,f.childNodes),f=k.firstChild,f.textContent=""}else l.push(b.createTextNode(e));k.textContent="",m=0;while(e=l[m++])if((!d||-1===n.inArray(e,d))&&(i=n.contains(e.ownerDocument,e),f=ob(k.appendChild(e),"script"),i&&mb(f),c)){j=0;while(e=f[j++])fb.test(e.type||"")&&c.push(e)}return k},cleanData:function(a){for(var b,c,d,e,f=n.event.special,g=0;void 0!==(c=a[g]);g++){if(n.acceptData(c)&&(e=c[L.expando],e&&(b=L.cache[e]))){if(b.events)for(d in b.events)f[d]?n.event.remove(c,d):n.removeEvent(c,d,b.handle);L.cache[e]&&delete L.cache[e]}delete M.cache[c[M.expando]]}}}),n.fn.extend({text:function(a){return J(this,function(a){return void 0===a?n.text(this):this.empty().each(function(){(1===this.nodeType||11===this.nodeType||9===this.nodeType)&&(this.textContent=a)})},null,a,arguments.length)},append:function(){return this.domManip(arguments,function(a){if(1===this.nodeType||11===this.nodeType||9===this.nodeType){var b=jb(this,a);b.appendChild(a)}})},prepend:function(){return this.domManip(arguments,function(a){if(1===this.nodeType||11===this.nodeType||9===this.nodeType){var b=jb(this,a);b.insertBefore(a,b.firstChild)}})},before:function(){return this.domManip(arguments,function(a){this.parentNode&&this.parentNode.insertBefore(a,this)})},after:function(){return this.domManip(arguments,function(a){this.parentNode&&this.parentNode.insertBefore(a,this.nextSibling)})},remove:function(a,b){for(var c,d=a?n.filter(a,this):this,e=0;null!=(c=d[e]);e++)b||1!==c.nodeType||n.cleanData(ob(c)),c.parentNode&&(b&&n.contains(c.ownerDocument,c)&&mb(ob(c,"script")),c.parentNode.removeChild(c));return this},empty:function(){for(var a,b=0;null!=(a=this[b]);b++)1===a.nodeType&&(n.cleanData(ob(a,!1)),a.textContent="");return this},clone:function(a,b){return a=null==a?!1:a,b=null==b?a:b,this.map(function(){return n.clone(this,a,b)})},html:function(a){return J(this,function(a){var b=this[0]||{},c=0,d=this.length;if(void 0===a&&1===b.nodeType)return b.innerHTML;if("string"==typeof a&&!db.test(a)&&!ib[(bb.exec(a)||["",""])[1].toLowerCase()]){a=a.replace(ab,"<$1>");try{for(;d>c;c++)b=this[c]||{},1===b.nodeType&&(n.cleanData(ob(b,!1)),b.innerHTML=a);b=0}catch(e){}}b&&this.empty().append(a)},null,a,arguments.length)},replaceWith:function(){var a=arguments[0];return this.domManip(arguments,function(b){a=this.parentNode,n.cleanData(ob(this)),a&&a.replaceChild(b,this)}),a&&(a.length||a.nodeType)?this:this.remove()},detach:function(a){return this.remove(a,!0)},domManip:function(a,b){a=e.apply([],a);var c,d,f,g,h,i,j=0,l=this.length,m=this,o=l-1,p=a[0],q=n.isFunction(p);if(q||l>1&&"string"==typeof p&&!k.checkClone&&eb.test(p))return this.each(function(c){var d=m.eq(c);q&&(a[0]=p.call(this,c,d.html())),d.domManip(a,b)});if(l&&(c=n.buildFragment(a,this[0].ownerDocument,!1,this),d=c.firstChild,1===c.childNodes.length&&(c=d),d)){for(f=n.map(ob(c,"script"),kb),g=f.length;l>j;j++)h=c,j!==o&&(h=n.clone(h,!0,!0),g&&n.merge(f,ob(h,"script"))),b.call(this[j],h,j);if(g)for(i=f[f.length-1].ownerDocument,n.map(f,lb),j=0;g>j;j++)h=f[j],fb.test(h.type||"")&&!L.access(h,"globalEval")&&n.contains(i,h)&&(h.src?n._evalUrl&&n._evalUrl(h.src):n.globalEval(h.textContent.replace(hb,"")))}return this}}),n.each({appendTo:"append",prependTo:"prepend",insertBefore:"before",insertAfter:"after",replaceAll:"replaceWith"},function(a,b){n.fn[a]=function(a){for(var c,d=[],e=n(a),g=e.length-1,h=0;g>=h;h++)c=h===g?this:this.clone(!0),n(e[h])[b](c),f.apply(d,c.get());return this.pushStack(d)}});var qb,rb={};function sb(b,c){var d,e=n(c.createElement(b)).appendTo(c.body),f=a.getDefaultComputedStyle&&(d=a.getDefaultComputedStyle(e[0]))?d.display:n.css(e[0],"display");return e.detach(),f}function tb(a){var b=l,c=rb[a];return c||(c=sb(a,b),"none"!==c&&c||(qb=(qb||n("