Skip to content

Instantly share code, notes, and snippets.

@cosard

cosard/app_init.c

Last active March 16, 2017 21:14
Show Gist options
  • Save cosard/0f535581af23e2f008da to your computer and use it in GitHub Desktop.
Save cosard/0f535581af23e2f008da to your computer and use it in GitHub Desktop.
Download ZIP
atmel pi robot
Raw
app_init.c
#include "app_init.h"
#include <stdio.h>
void pwm_init(void)
{
// OC2 sol motor
TCCR2 |= _BV(WGM21) | _BV(WGM20); // OC2 fast pwm mode
TCCR2 &= ~_BV(COM21) & ~_BV(COM20); // disable
//TCCR2 |= _BV(COM21); // Clear OC2 on compare match, set OC2 at BOTTOM
//TCCR2 &= ~_BV(COM20); // Clear OC2 on compare match, set OC2 at BOTTOM
//open
TCCR2 |= _BV(CS22); // clkT2S/64 ~ 1khz
TCCR2 &= ~_BV(CS21) & ~_BV(CS20); // clkT2S/64 ~ 1khz
// oc1a sag motor ve oc1b buzzer
TCCR1A &= ~_BV(COM1A1) & ~_BV(COM1A0); // oc1a disable
TCCR1A &= ~_BV(COM1B1) & ~_BV(COM1B0); // oc1b disable
//TCCR1A |= _BV(COM1A1); // Clear OC1A on compare match
//TCCR1A &= ~_BV(COM1A0); // Clear OC1A on compare match
//TCCR1A |= _BV(COM1B1); // Clear OC1B on compare match
//TCCR1A &= ~_BV(COM1B0); // Clear OC1B on compare match
// open
TCCR1B &= ~_BV(WGM13); // Fast PWM, 8-bit 0x00FF TOP
TCCR1B |= _BV(WGM12); // Fast PWM, 8-bit 0x00FF TOP
TCCR1A &= ~_BV(WGM11); // Fast PWM, 8-bit 0x00FF TOP
TCCR1A |= _BV(WGM10); // Fast PWM, 8-bit 0x00FF TOP
TCCR1B &= ~_BV(CS12); // clkI/O/64 (From prescaler) ~ 1 khz
TCCR1B |= _BV(CS11); // clkI/O/64 (From prescaler) ~ 1 khz
TCCR1B |= _BV(CS10); // clkI/O/64 (From prescaler) ~ 1 khz
}
void adc_init(void)
{
ADCSRA |= _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0); // Set ADC prescalar to 128 - 125KHz sample rate @ 16MHz
ADMUX |= _BV(REFS0); // Set ADC reference to AVCC
ADMUX &= ~_BV(ADLAR); // right adjust
SFIOR = 0; // ADC free running mode
ADCSRA |= _BV(ADEN); // Enable ADC
//ADCSRA |= _BV(ADSC); // Start A2D Conversions
}
void uart_init() {
UBRRH = UBRRH_VALUE;
UBRRL = UBRRL_VALUE;
#if USE_2X
UCSRA |= _BV(U2X);
#else
UCSRA &= ~(_BV(U2X));
#endif
UCSRB |= _BV(RXEN) | _BV(TXEN);
UCSRC = (1 << URSEL) | (1 << UCSZ0) | (1 << UCSZ1); // Use 8-bit character sizes - URSEL bit set to select the UCRSC register
}
void uart_putchar(char c, FILE *stream) {
if (c == '\n') {
uart_putchar('\r', stream);
}
loop_until_bit_is_set(UCSRA, UDRE);
UDR = c;
}
char uart_getchar(FILE *stream) {
loop_until_bit_is_set(UCSRA, RXC); /* Wait until data exists. */
return UDR;
}
void app_init()
{
LED_DDR |= _BV(RED_LED) | _BV(GREEN_LED) | _BV(BLUE_LED);
BUZZER_DDR |= _BV(BUZZER_PIN);
BUTTON_DDR &= ~_BV(BUTTON_PIN);
BUTTON_PORT |= _BV(BUTTON_PIN);
MOTOR_EN1_DDR |= _BV(MOTOR_EN1_PIN);
MOTOR_EN2_DDR |= _BV(MOTOR_EN2_PIN);
MOTOR_IN1_DDR |= _BV(MOTOR_IN1_PIN);
MOTOR_IN2_DDR |= _BV(MOTOR_IN2_PIN);
MOTOR_IN3_DDR |= _BV(MOTOR_IN3_PIN);
MOTOR_IN4_DDR |= _BV(MOTOR_IN4_PIN);
QRD_DDR &= ~_BV(QRD_EN_SOL_PIN) & ~_BV(QRD_SOL_PIN) & ~_BV(QRD_ORTA_PIN) & ~_BV(QRD_SAG_PIN) & ~_BV(QRD_EN_SAG_PIN);
QRD_PORT = 0;
#ifdef HALL_CONNECTED
{
HALL_DDR &= ~_BV(HALL_INPUT);
HALL_DDR |= _BV(HALL_GND) | _BV(HALL_VDD);
HALL_PORT |= _BV(HALL_INPUT);
HALL_PORT &= ~_BV(HALL_GND);
HALL_PORT |= _BV(HALL_VDD);
/*
MCUCSR |= _BV(ISC2); // int2 falling edge
_delay_ms(10);
GIFR &= ~_BV(INTF2); // clear int 2 int flag
_delay_ms(1);
GICR |= _BV(INT2); // int 2 enable
GIFR &= ~_BV(INTF2); // clear int 2 int flag
*/
}
#endif
uart_init();
pwm_init();
adc_init();
UCSRB |= _BV(RXCIE); // Enable the USART Receieve Complete interrupt (USART_RXCn)
UCSRB |= _BV(TXCIE); // Enable the USART Transmit Complete interrupt (USART_TXCn)
// Ayarlar için bekle
_delay_ms(10);
}
Raw
app_init.h
#include <stdlib.h>
#include "pi_robot.h"
void app_init();
void pwm_init(void);
void adc_init(void);
void uart_putchar(char c, FILE *stream);
char uart_getchar(FILE *stream);
Raw
app_shell.c
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "app_shell.h"
#include "chprintf.h"
enum arrowKey {
arrowNONE,
arrowUP,
arrowDOWM,
arrowLEFT,
arrowRIGHT,
arrowCOUNT
};
/**
* @brief Shell termination event source.
*/
static bool rxFlag = false;
static bool txFlag = false;
static char rxByte = '\0';
static char txByte = '\0';
static char printStack[MAX_PRINT_DATA_LENGTH];
static short printStackIndex = 0;
static short printStackPtr = 0;
static int state = 0;
static int cmdStackIndex = 0;
static int txCount = 0;
static int mCmdStackIndex = 0;
ShellCommand *scp = NULL;
char line[SHELL_MAX_LINE_LENGTH];
short lineIndex = 0;
static struct cmdLineStack *head = NULL;
static char *_strtok(char *str, const char *delim, char **saveptr) {
char *token;
if (str)
*saveptr = str;
token = *saveptr;
if (!token)
return NULL;
token += strspn(token, delim);
*saveptr = strpbrk(token, delim);
if (*saveptr)
*(*saveptr)++ = '\0';
return *token ? token : NULL;
}
static void usage(char *p) {
chprintf("Usage: %s\r\n", p);
}
static void list_commands(const ShellCommand *scp) {
while (scp->sc_name != NULL) {
chprintf("%s \t", scp->sc_name);
scp++;
}
chprintf("\r\n");
}
static void cmd_info(int argc, char *argv[]) {
(void)argv;
if (argc > 0) {
usage("info");
return;
}
#ifdef CH_COMPILER_NAME
chprintf("Compiler: %s\r\n", CH_COMPILER_NAME);
#endif
chprintf("Architecture: AVR- Atmega16\r\n");
#ifdef CH_CORE_VARIANT_NAME
chprintf("Core Variant: %s\r\n", CH_CORE_VARIANT_NAME);
#endif
#ifdef CH_PORT_INFO
chprintf("Port Info: %s\r\n", CH_PORT_INFO);
#endif
#ifdef PLATFORM_NAME
chprintf("Platform: %s\r\n", PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
chprintf("Board: %s\r\n", BOARD_NAME);
#endif
#ifdef __DATE__
#ifdef __TIME__
chprintf("Build time: %s%s%s\r\n", __DATE__, " - ", __TIME__);
#endif
#endif
}
static void appPwmSetDuty(int argc, char *argv[])
{
(void)argc;
if(argv != NULL && argv[0] != NULL && argv[1] != NULL && argv[2] == NULL)
{
if( (strcmp(argv[0],"-s") == 0) && strlen(argv[1]) > 0 )
{
chprintf("%s\r\n",(argv[1]));
}
}
else{
chprintf("pwm: illegal operation\r\n");
}
}
/**
* @brief Array of the default commands.
*/
static ShellCommand local_commands[] = {
{"info", cmd_info},
{"pwm", appPwmSetDuty},
{NULL, NULL}
};
static bool cmdexec(const ShellCommand *scp, char *name, int argc, char *argv[]) {
while (scp->sc_name != NULL) {
if (strcasecmp(scp->sc_name, name) == 0) {
scp->sc_function(argc, argv);
return false;
}
scp++;
}
return true;
}
static struct cmdLineStack* create_list(char *ch)
{
cmdStackIndex = 0;
struct cmdLineStack *ptr = (struct cmdLineStack*)malloc(sizeof(struct cmdLineStack));
ptr->index = cmdStackIndex;
ptr->cmdLinePtr = NULL;
ptr->cmdLinePtr = malloc(strlen(ch));
if(ptr->cmdLinePtr == NULL)
return NULL;
strcpy(ptr->cmdLinePtr, ch);
ptr->next = NULL;
cmdStackIndex++;
head = ptr;
return ptr;
}
static struct cmdLineStack* search_in_list(int ind, struct cmdLineStack **prev)
{
struct cmdLineStack *ptr = head;
struct cmdLineStack *tmp = NULL;
bool found = false;
while(ptr != NULL)
{
if(ptr->index == ind)
{
found = true;
break;
}
else
{
tmp = ptr;
ptr = ptr->next;
}
}
if(true == found)
{
if(prev)
*prev = tmp;
return ptr;
}
else
{
return NULL;
}
}
static bool delete_from_list(int ind)
{
struct cmdLineStack *prev = NULL;
struct cmdLineStack *del = NULL;
del = search_in_list(ind,&prev);
if(del == NULL)
{
return false;
}
else
{
if(prev != NULL)
prev->next = del->next;
if(del == head)
{
head = del->next;
}
}
free(del->cmdLinePtr);
free(del);
del = NULL;
return true;
}
struct cmdLineStack* add_to_list(char *ch)
{
if(NULL == head)
{
return (create_list(ch));
}
if(strcmp(head->cmdLinePtr, ch) != 0)
{
struct cmdLineStack *ptr = (struct cmdLineStack*)malloc(sizeof(struct cmdLineStack));
if(ptr == NULL)
return NULL;
struct cmdLineStack *searchPtr = NULL;
for(short i = 0; i < SHELL_MAX_STACK_DEPTH; i++)
{
searchPtr = search_in_list(i, NULL);
if(searchPtr != NULL)
{
if(strcasecmp(ch, searchPtr->cmdLinePtr) == 0)
{
free(ptr);
return NULL;
}
}
}
if(cmdStackIndex >= SHELL_MAX_STACK_DEPTH)
{
cmdStackIndex = SHELL_MAX_STACK_DEPTH - 1;
struct cmdLineStack *_ptr;
_ptr = head;
while(NULL != _ptr->next)
{
_ptr->index = _ptr->next->index;
if(_ptr->index == 0)
_ptr->next->index = -1;
_ptr = _ptr->next;
}
if(delete_from_list(cmdStackIndex) == false)
chprintf("delnot\r\n");
}
ptr->index = cmdStackIndex;
ptr->cmdLinePtr = NULL;
ptr->cmdLinePtr = malloc(strlen(ch));
if(ptr->cmdLinePtr == NULL)
return NULL;
strcpy(ptr->cmdLinePtr, ch);
ptr->next = NULL;
ptr->next = head;
head = ptr;
cmdStackIndex++;
return ptr;
}
return NULL;
}
/**
* @brief Shell thread function.
*
* @param[in] p pointer to a @p BaseSequentialStream object
* @return Termination reason.
* @retval RDY_OK terminated by command.
* @retval RDY_RESET terminated by reset condition on the I/O channel.
*/
void shell_init(void *p) {
if(p != NULL)
scp = ((ShellConfig *)p)->sc_commands;
chprintf("\r\nChibiOS/RT Shell\r\n");
chprintf("ch> ");
state = 1;
//scp = ((ShellConfig *)p)->sc_commands;
}
bool processData()
{
int n;
char ch = rxByte;
char *lp = NULL, *cmd = NULL, *tokp = NULL;
char *args[SHELL_MAX_ARGUMENTS + 1];
char *mCmdLinePtr = NULL;
if(state == STATE_RESET)
{
chprintf("ch> ");
state = 1;
}
if (!shellGetLine(ch)) {
state = STATE_RESET;
strcpy(line, "");
lineIndex = 0;
return false;
}
if(state == 4)
{
short allocSize = strlen(line);
lp = _strtok(line, " \t", &tokp);
short i;
lineIndex = 0;
cmd = lp;
n = 0;
while ((lp = _strtok(NULL, " ", &tokp)) != NULL) {
if (n >= SHELL_MAX_ARGUMENTS) {
chprintf("too many arguments\r\n");
cmd = NULL;
break;
}
args[n++] = lp;
}
args[n] = NULL;
if (cmd != NULL) {
mCmdLinePtr = malloc(allocSize);
if(mCmdLinePtr != NULL)
{
strcpy(mCmdLinePtr, line);
for(i = 0; i < n; i++)
{
strcat(mCmdLinePtr," ");
strcat(mCmdLinePtr, args[i]);
}
strcat(mCmdLinePtr,"\0");
add_to_list(mCmdLinePtr);
free(mCmdLinePtr);
}
if (strcasecmp(cmd, "help") == 0) {
if (n > 0) {
usage("help");
return true;
}
chprintf("Commands:");
list_commands(local_commands);
if (scp != NULL)
list_commands(scp);
chprintf("\r\n");
}
else if (cmdexec(local_commands, cmd, n, args) &&
((scp == NULL) || cmdexec(scp, cmd, n, args))) {
chprintf( "%s ?\r\n", cmd);
}
}
chprintf("ch> ");
state = 1;
for(i = 0; i < SHELL_MAX_LINE_LENGTH; i++)
line[i] = '\0';
}
return true;
}
/**
* @brief Reads a whole line from the input channel.
*
* @param[in] line
* @param[in] size buffer maximum length
* @return The operation status.
* @retval true the channel was reset or CTRL-D pressed.
* @retval false operation successful.
*/
bool shellGetLine(char c) {
struct cmdLineStack *ptr = NULL;
if(state == STATE_STARTED)
{
mCmdStackIndex = cmdStackIndex;
state = STATE_READING;
}
static char doOperation = false;
if(state == STATE_CURSOR)
{
if( c == 'A' || c == 'B')
{
doOperation = false;
if(c == 'A')
{
if(mCmdStackIndex > 0)
{
mCmdStackIndex--;
doOperation = true;
}
}
else if(c == 'B')
{
if(mCmdStackIndex < cmdStackIndex - 1)
{
mCmdStackIndex++;
doOperation = true;
}
}
if(doOperation)
{
ptr = search_in_list(mCmdStackIndex,NULL);
if(ptr != NULL)
{
strcpy(line, (const char *)ptr->cmdLinePtr);
chprintf("\r\nch> %s",line);
lineIndex = strlen(line);
}
return true;
}
else
return true;
}
else
{
state = STATE_READING;
}
}
if (c == 0x04) {
chprintf("^D");
return false;
}
if (c == 0x08) { // backspace
if (lineIndex > 0) {
chputc(0x08);
chputc(0x20);
chputc(0x08);
lineIndex--;
line[lineIndex] = '\0';
}
return true;
}
if (c == '\r') {
chprintf("\r\n");
line[lineIndex] = '\0';
state = STATE_PROCESS;
return true;
}
if(c == '[')
{
state = STATE_CURSOR;
return true;
}
if (c < 0x20)
return true;
if (lineIndex < SHELL_MAX_LINE_LENGTH - 1) {
line[lineIndex] = c;
lineIndex++;
chputc(c);
}
return true;
}
ISR(USART_RXC_vect) //trigger interrupt when uart1 receives data
{
cli();
rxByte = UDR;
sei();
rxFlag = false;
processData();
}
ISR(USART_TXC_vect)
{
cli();
sei();
txFlag = false;
txCount++;
if(printStackPtr != printStackIndex)
{
UDR = printStack[printStackPtr];
txFlag = true;
printStackPtr++;
if(printStackPtr == MAX_PRINT_DATA_LENGTH)
printStackPtr = 0;
}
}
void chputc(char c)
{
printStack[printStackIndex] = c;
printStackIndex++;
if(printStackIndex == MAX_PRINT_DATA_LENGTH)
{
printStackIndex = 0;
}
if(!txFlag)
{
UDR = printStack[printStackPtr];
printStackPtr++;
if(printStackPtr == MAX_PRINT_DATA_LENGTH)
printStackPtr = 0;
txFlag = true;
}
txByte = c;
}
/** @} */
Raw
app_shell.h
#ifndef _SHELL_H_
#define _SHELL_H_
#include <stdbool.h>
/**
* @brief Shell maximum input line length.
*/
#if !defined(SHELL_MAX_LINE_LENGTH)
#define SHELL_MAX_LINE_LENGTH 25
#endif
#define SERIAL_MAX_LENGTH 50
/**
* @brief Shell maximum arguments per command.
*/
#if !defined(SHELL_MAX_ARGUMENTS)
#define SHELL_MAX_ARGUMENTS 4
#endif
/**
* @brief Shell maximum cmd line stack depth.
*/
#if !defined(SHELL_MAX_STACK_DEPTH)
#define SHELL_MAX_STACK_DEPTH 5
#endif
#define MAX_PRINT_DATA_LENGTH 50
#define STATE_RESET 0
#define STATE_STARTED 1
#define STATE_READING 2
#define STATE_CURSOR 3
#define STATE_PROCESS 4
struct cmdLineStack
{
int index;
struct cmdLineStack *next;
char *cmdLinePtr;
};
/**
* @brief Command handler function type.
*/
typedef void (*shellcmd_t)(int argc, char *argv[]);
/**
* @brief Custom command entry type.
*/
typedef struct {
const char *sc_name; /**< @brief Command name. */
shellcmd_t sc_function; /**< @brief Command function. */
} ShellCommand;
/**
* @brief Shell descriptor type.
*/
typedef struct {
const ShellCommand *sc_commands; /**< @brief Shell extra commands table. */
} ShellConfig;
#ifdef __cplusplus
extern "C" {
#endif
bool shellGetLine(char c);
void shell_init(void *p);
bool processData();
void chputc(char);
#ifdef __cplusplus
}
#endif
#endif /* _SHELL_H_ */
/** @} */
Raw
chprintf.c
/*
ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
/*
Concepts and parts of this file have been contributed by Fabio Utzig,
chvprintf() added by Brent Roman.
*/
/**
* @file chprintf.c
* @brief Mini printf-like functionality.
*
* @addtogroup chprintf
* @{
*/
#include <avr/interrupt.h>
#include "chprintf.h"
#include "app_shell.h"
#define MAX_FILLER 11
#define FLOAT_PRECISION 100000
static char *long_to_string_with_divisor(char *p,
long num,
unsigned radix,
long divisor) {
int i;
char *q;
long l, ll;
l = num;
if (divisor == 0) {
ll = num;
} else {
ll = divisor;
}
q = p + MAX_FILLER;
do {
i = (int)(l % radix);
i += '0';
if (i > '9')
i += 'A' - '0' - 10;
*--q = i;
l /= radix;
} while ((ll /= radix) != 0);
i = (int)(p + MAX_FILLER - q);
do
*p++ = *q++;
while (--i);
return p;
}
static char *ltoa(char *p, long num, unsigned radix) {
return long_to_string_with_divisor(p, num, radix, 0);
}
#if CHPRINTF_USE_FLOAT
static char *ftoa(char *p, double num) {
long l;
unsigned long precision = FLOAT_PRECISION;
l = num;
p = long_to_string_with_divisor(p, l, 10, 0);
*p++ = '.';
l = (num - l) * precision;
return long_to_string_with_divisor(p, l, 10, precision / 10);
}
#endif
/**
* @brief System formatted output function.
* @details This function implements a minimal @p vprintf()-like functionality
* with output on a @p BaseSequentialStream.
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
* The following parameter types (p) are supported:
* - <b>x</b> hexadecimal integer.
* - <b>X</b> hexadecimal long.
* - <b>o</b> octal integer.
* - <b>O</b> octal long.
* - <b>d</b> decimal signed integer.
* - <b>D</b> decimal signed long.
* - <b>u</b> decimal unsigned integer.
* - <b>U</b> decimal unsigned long.
* - <b>c</b> character.
* - <b>s</b> string.
* .
*
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
* @param[in] fmt formatting string
* @param[in] ap list of parameters
*
* @api
*/
void chvprintf(const char *fmt, va_list ap) {
char *p, *s, c, filler;
int i, precision, width;
bool is_long, left_align;
long l;
#if CHPRINTF_USE_FLOAT
float f;
char tmpbuf[2*MAX_FILLER + 1];
#else
char tmpbuf[MAX_FILLER + 1];
#endif
while (true) {
c = *fmt++;
if (c == 0)
return;
if (c != '%') {
chputc(c);
continue;
}
p = tmpbuf;
s = tmpbuf;
left_align = false;
if (*fmt == '-') {
fmt++;
left_align = true;
}
filler = ' ';
if ((*fmt == '.') || (*fmt == '0')) {
fmt++;
filler = '0';
}
width = 0;
while (true) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
width = width * 10 + c;
}
precision = 0;
if (c == '.') {
while (true) {
c = *fmt++;
if (c >= '0' && c <= '9')
c -= '0';
else if (c == '*')
c = va_arg(ap, int);
else
break;
precision *= 10;
precision += c;
}
}
/* Long modifier.*/
if (c == 'l' || c == 'L') {
is_long = true;
if (*fmt)
c = *fmt++;
}
else
is_long = (c >= 'A') && (c <= 'Z');
/* Command decoding.*/
switch (c) {
case 'c':
filler = ' ';
*p++ = va_arg(ap, int);
break;
case 's':
filler = ' ';
if ((s = va_arg(ap, char *)) == 0)
s = "(null)";
if (precision == 0)
precision = 32767;
for (p = s; *p && (--precision >= 0); p++)
;
break;
case 'D':
case 'd':
case 'I':
case 'i':
if (is_long)
l = va_arg(ap, long);
else
l = va_arg(ap, int);
if (l < 0) {
*p++ = '-';
l = -l;
}
p = ltoa(p, l, 10);
break;
#if CHPRINTF_USE_FLOAT
case 'f':
f = (float) va_arg(ap, double);
if (f < 0) {
*p++ = '-';
f = -f;
}
p = ftoa(p, f);
break;
#endif
case 'X':
case 'x':
c = 16;
goto unsigned_common;
case 'U':
case 'u':
c = 10;
goto unsigned_common;
case 'O':
case 'o':
c = 8;
unsigned_common:
if (is_long)
l = va_arg(ap, unsigned long);
else
l = va_arg(ap, unsigned int);
p = ltoa(p, l, c);
break;
default:
*p++ = c;
break;
}
i = (int)(p - s);
if ((width -= i) < 0)
width = 0;
if (left_align == false)
width = -width;
if (width < 0) {
if (*s == '-' && filler == '0') {
chputc(*s++);
i--;
}
do
chputc(filler);
while (++width != 0);
}
while (--i >= 0)
chputc(*s++);
while (width) {
chputc(filler);
width--;
}
}
}
/** @} */
Raw
chprintf.h
/*
ChibiOS/RT - Copyright (C) 2006-2013 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
/**
* @file chprintf.h
* @brief Mini printf-like functionality.
*
* @addtogroup chprintf
* @{
*/
#ifndef _CHPRINTF_H_
#define _CHPRINTF_H_
#include <stdarg.h>
#include <stddef.h>
#include <stdbool.h>
/**
* @brief Float type support.
*/
#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
#define CHPRINTF_USE_FLOAT false
#endif
#ifdef __cplusplus
extern "C" {
#endif
void chvprintf(const char *fmt, va_list ap);
int chsnprintf(char *str, size_t size, const char *fmt, ...);
#ifdef __cplusplus
}
#endif
/**
* @brief System formatted output function.
* @details This function implements a minimal @p printf() like functionality
* with output on a @p BaseSequentialStream.
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
* The following parameter types (p) are supported:
* - <b>x</b> hexadecimal integer.
* - <b>X</b> hexadecimal long.
* - <b>o</b> octal integer.
* - <b>O</b> octal long.
* - <b>d</b> decimal signed integer.
* - <b>D</b> decimal signed long.
* - <b>u</b> decimal unsigned integer.
* - <b>U</b> decimal unsigned long.
* - <b>c</b> character.
* - <b>s</b> string.
* .
*
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
* @param[in] fmt formatting string
*
* @api
*/
static inline void chprintf(const char *fmt, ...) {
va_list ap;
va_start(ap, fmt);
chvprintf(fmt, ap);
va_end(ap);
}
#endif /* _CHPRINTF_H_ */
/** @} */
Raw
pi_robot.c
/*
* pi_robot_deneme1.c
*
* Created: 06.03.2013 22:52:29
* Author: cosar
*/
#include <avr/interrupt.h>
#include "chprintf.h"
#include "pi_robot.h"
#include "app_init.h"
#include "app_shell.h"
bool hall_triggered = false;
int16_t sensors[5],
hall_value = 0,
path_length = 0,
left_power = 0,
right_power = 0,
x_axis = 2,
y_axis = 0,
direction = 0,
last_position = 0;
unsigned char path[200],
turn_direction,
state = LEARN;
char buffer[10], blue_state = 0, blue_var;
unsigned char buffer_ptr = 0;
int16_t idiv = 40, ddiv = 40, max_power = 90, pdiv = 1;
void optimize_path(void)
{
int16_t i, rotation = 0;
if (path_length < 3 || path[path_length - 2] != 'B')
return;
for (i = 1; i < 4; i++) {
switch (path[path_length - i]) {
case 'L':
rotation += 90;
break;
case 'B':
rotation += 180;
break;
case 'R':
rotation += 270;
break;
case 'F':
break;
}
}
rotation %= 360;
switch (rotation) {
case 0:
path[path_length - 3] = 'F';
break;
case 90:
path[path_length - 3] = 'L';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'R';
break;
}
path_length -= 2;
}
void go_ahead(unsigned char line_color)
{
int16_t counter = 0, proportional = 0, last_proportional = 0, power_difference = 0,derivative = 0;
int32_t integral = 0;
// ileri gitmede pid hatası için biraz ilerle
set_motors(50, 50);
_delay_ms(100);
while (1) {
// sensorleri oku
hall_value = read_hall_sensor();
proportional = read_line(sensors, line_color) - 2000;
if(POWER_DIFF_DEBUG)
printf("pro: %d\n",proportional);
derivative = proportional - last_proportional;
integral += proportional;
last_proportional = proportional;
// pid kontrol katsayıları ile motorlar arası hız farkını hesapla
power_difference = proportional/pdiv + (int16_t)(integral/idiv/100) + derivative*ddiv;
if(POWER_DIFF_DEBUG)
printf("pow dif: %d\n",power_difference);
if (power_difference > max_power)
power_difference = max_power;
else if (power_difference < -max_power)
power_difference = -max_power;
if (power_difference < 0) {
set_motors(max_power + power_difference, max_power);
} else {
set_motors(max_power, max_power - power_difference);
}
if (counter <= 250)
counter++;
if (PID_DEBUG)
continue;
if (line_color == 'W') {
// dönüş var mı?
if (sensors[0] < 300 || sensors[4] < 300 || hall_value == 1) {
break;
}
} else {
// beyaz çizgi başladı mı?
if (sensors[0] > 800 && sensors[4] > 800)
break;
}
}
}
void stopper(void)
{
set_motors(-left_power, -right_power);
_delay_ms(5);
set_motors(0, 0);
}
void turn(unsigned char turn_dir)
{
switch (turn_dir) {
case 'L':
set_motors(-100, 100);
_delay_ms(180);
break;
case 'R':
set_motors(100, -100);
_delay_ms(160);
break;
case 'B':
set_motors(-100, 100);
_delay_ms(320);
break;
case 'F':
break;
}
}
void define_coordinate(int16_t dir)
{
if (dir == 0)
y_axis++;
else if (dir == 2)
y_axis--;
if (dir == 1)
x_axis++;
else if (dir == 3)
x_axis--;
}
int16_t read_line(int16_t *sensors, unsigned char line_color)
{
int16_t i, inner_sensors[5], total_value = 0, total_position = 0, position;
int16_t adc_channels[5] = {QRD_EN_SOL_PIN, QRD_SOL_PIN, QRD_ORTA_PIN, QRD_SAG_PIN, QRD_EN_SAG_PIN};
unsigned char on_line = 0;
for (i = 0; i < 5; i++) {
sensors[i] = read_one_sensor(adc_channels[i]);
if (sensors[i] > QRD_MAX_VALUE)
sensors[i] = QRD_MAX_VALUE;
else if (sensors[i] < QRD_MIN_VALUE)
sensors[i] = QRD_MIN_VALUE;
sensors[i] = inner_sensors[i] = (int16_t)((float)(sensors[i] - QRD_MIN_VALUE) / (QRD_MAX_VALUE - QRD_MIN_VALUE) * 1000);
// printf("Sensör %d: %u\n", i, sensors[i]);
if (line_color == 'W') {
inner_sensors[i] = 1000 - inner_sensors[i];
}
total_position += i * inner_sensors[i];
total_value += inner_sensors[i];
if (inner_sensors[i] > 600)
on_line = 1;
}
position = (int16_t)1000*((float)total_position / total_value);
if (!on_line) {
if (last_position < 2000)
position = 0;
else
position = 4000;
}
last_position = position;
if(SENSOR_DEBUG)
{
printf("Total position: %u\n", total_position);
printf("Total value: %u\n", total_value);
printf("Position: %u\n", position);
for(i= 0; i<5;i++)
printf("Sensör %d: %u\n", i, sensors[i]);
printf("-----------------------\n");
}
return position;
}
int16_t read_one_sensor(int16_t sensor_id)
{
return read_adc(sensor_id);
}
int16_t read_hall_sensor(void)
{
if(!(HALL_PIN & _BV(HALL_INPUT)))
return 0;
else
{
HALL_PORT &= ~_BV(HALL_VDD);
_delay_ms(10);
HALL_PORT |= _BV(HALL_VDD);
//hall_triggered = true;
BUZZER_PORT |= _BV(BUZZER_PIN);
_delay_ms(5);
BUZZER_PORT &= ~_BV(BUZZER_PIN);
_delay_ms(10);
return 1;
}
}
void set_left_motor_pwm(int16_t pwm)
{
if(pwm == 0)
TCCR2 &= ~_BV(COM21) & ~_BV(COM20);
else
{
//TCNT2 = 0;
OCR2 = pwm;
TCCR2 |= _BV(COM21); // Clear OC2 on compare match, set OC2 at BOTTOM
TCCR2 &= ~_BV(COM20); // Clear OC2 on compare match, set OC2 at BOTTOM
}
}
void set_right_motor_pwm(int16_t pwm)
{
if(pwm == 0)
TCCR1A &= ~_BV(COM1A1) & ~_BV(COM1A0); // oc1a disable
else
{
//TCNT1 = 0;
OCR1A = pwm;
TCCR1A |= _BV(COM1A1); // Clear OC1A on compare match
TCCR1A &= ~_BV(COM1A0); // Clear OC1A on compare match
}
}
void set_motor_direction(char direction)
{
switch(direction)
{
case ILERI:
MOTOR_IN1_PORT |= _BV(MOTOR_IN1_PIN);
MOTOR_IN2_PORT &= ~_BV(MOTOR_IN2_PIN);
MOTOR_IN3_PORT &= ~_BV(MOTOR_IN3_PIN);
MOTOR_IN4_PORT |= _BV(MOTOR_IN4_PIN);
break;
case GERI:
MOTOR_IN1_PORT &= ~_BV(MOTOR_IN1_PIN);
MOTOR_IN2_PORT |= _BV(MOTOR_IN2_PIN);
MOTOR_IN3_PORT |= _BV(MOTOR_IN3_PIN);
MOTOR_IN4_PORT &= ~_BV(MOTOR_IN4_PIN);
break;
case SOL:
MOTOR_IN1_PORT &= ~_BV(MOTOR_IN1_PIN);
MOTOR_IN2_PORT |= _BV(MOTOR_IN2_PIN);
MOTOR_IN3_PORT &= ~_BV(MOTOR_IN3_PIN);
MOTOR_IN4_PORT |= _BV(MOTOR_IN4_PIN);
break;
case SAG:
MOTOR_IN1_PORT |= _BV(MOTOR_IN1_PIN);
MOTOR_IN2_PORT &= ~_BV(MOTOR_IN2_PIN);
MOTOR_IN3_PORT |= _BV(MOTOR_IN3_PIN);
MOTOR_IN4_PORT &= ~_BV(MOTOR_IN4_PIN);
break;
default:
break;
}
}
void set_left_motor(int16_t pwm)
{
unsigned char reverse = 0;
if (pwm < 0) {
reverse = 1;
pwm = -pwm;
}
if (pwm > 255)
pwm = 255;
set_left_motor_pwm(pwm);
if (!reverse)
{
set_motor_direction(GERI);
}
else
{
set_motor_direction(ILERI);
}
}
void set_right_motor(int16_t pwm)
{
unsigned char reverse = 0;
if (pwm < 0) {
reverse = 1;
pwm = -pwm;
}
if (pwm > 255)
pwm = 255;
set_right_motor_pwm(pwm);
if (!reverse)
{
set_motor_direction(ILERI);
}
else
{
set_motor_direction(GERI);
}
}
void set_motors(int16_t left, int16_t right)
{
left_power = left;
right_power = right;
set_left_motor(left);
set_right_motor(right);
}
void calibrate_sensors(void)
{
int16_t i;
int16_t value;
QRD_MIN_VALUE = QRD_MAX_VALUE = read_one_sensor(QRD_ORTA_PIN);
for (i = 0; i < 85; i++) {
if (i < 20 || i > 59)
set_motors(-100, 100);
else
set_motors(100, -100);
value = read_one_sensor(QRD_ORTA_PIN);
QRD_MAX_VALUE = (value > QRD_MAX_VALUE) ? value : QRD_MAX_VALUE;
QRD_MIN_VALUE = (value < QRD_MIN_VALUE) ? value : QRD_MIN_VALUE;
_delay_ms(15);
}
set_motors(0, 0);
}
void set_buzzer_pwm(uint8_t pwm)
{
if(pwm == 0)
TCCR1A &= ~_BV(COM1B1) & ~_BV(COM1B0); // oc2a disable
else
{
//TCNT1 = 0;
OCR1B = pwm;
TCCR1A |= _BV(COM1B1); // Clear OC1A on compare match
TCCR1A &= ~_BV(COM1B0); // Clear OC1A on compare match
}
}
int16_t read_adc(int16_t channel)
{
ADMUX &= 0b11100000;
ADMUX |= channel;
ADCSRA |= (1 << ADSC);
while( !(ADCSRA & (1<<ADSC)));
_delay_us(200);
return ADC;
}
void set_rgb(bool red, bool green, bool blue)
{
if(red)
LED_PORT |= _BV(RED_LED);
else
LED_PORT &= ~_BV(RED_LED);
if(green)
LED_PORT |= _BV(GREEN_LED);
else
LED_PORT &= ~_BV(GREEN_LED);
if(blue)
LED_PORT |= _BV(BLUE_LED);
else
LED_PORT &= ~_BV(BLUE_LED);
}
static void nunchukRequestData(int argc, char *argv[])
{
chprintf("ncreq\r\n");
}
static void getRtcTime(int argc, char *argv[])
{
chprintf("time\r\n");
}
static const ShellCommand commands[] = {
{"ncget", nunchukRequestData},
{"ftime", getRtcTime},
{NULL, NULL}
};
int main(void)
{
int16_t i;
app_init();
sei();
set_rgb(true, true, true);
_delay_ms(500);
set_rgb(false, false, false);
shell_init(NULL);
/*
set_right_motor_pwm(255);
set_left_motor_pwm(255);
set_motor_direction(GERI);
*/
while(1)
{
//chprintf("cosar:%d\r\n",100);
_delay_ms(500);
}
calibrate_sensors();
while (BlackButtonBit)
_delay_ms(20);
while(true)
{
read_line(sensors, 'W');
if(sensors[2] < 500)
set_rgb(true, true, true);
else
set_rgb(false, false, false);
}
_delay_ms(500);
if(SENSOR_DEBUG)
{
while (1){
read_line(sensors, 'W');
_delay_ms(1000);
}
}
while (1) {
if (PID_DEBUG) {
go_ahead('W');
continue;
}
switch (state) {
case LEARN:
go_ahead('W');
// birinci tur sonu mu?
if (sensors[2] > 800 && (sensors[0] < 300 || sensors[4] < 300)) {
state = BLACK;
path[path_length] = '\0';
break;
}
// ilerde yol var mı diye biraz daha ilerle
set_motors(40, 40);
_delay_ms(50);
read_line(sensors, 'W');
// dönülecek yönü ve robotun yönünü belirle
if (sensors[0] < 300) {
turn_direction = 'L';
direction++;
} else if (hall_value == 1) {
turn_direction = 'B';
direction += 2;
} else if (sensors[2] < 300) {
turn_direction = 'F';
} else if (sensors[4] < 300) {
turn_direction = 'R';
direction += 3;
}
turn(turn_direction);
path[path_length] = turn_direction;
path_length++;
// robotun yönü 4 olamaz
direction %= 4;
define_coordinate(direction);
optimize_path();
break;
case BLACK:
go_ahead('B');
// beyaz çizgiye tam otur
set_motors(50, 50);
_delay_ms(50);
state = SOLVE;
break;
case SOLVE:
go_ahead('W');
for (i = 0; i < path_length; i++) {
turn(path[i]);
go_ahead('W');
}
state = WAIT;
break;
case WAIT:
stopper();
set_motors(0, 0);
while(1);
break;
}
}
}
#ifdef HALL_CONNECTED
ISR(INT2_vect)
{
cli();
/*
set_rgb(0,0,0);
HALL_PORT &= ~_BV(HALL_VDD);
//PORTB &= ~_BV(3);
_delay_us(10);
HALL_PORT |= _BV(HALL_VDD);
//PORTB |= _BV(3);
*/
HALL_PORT &= ~_BV(HALL_VDD);
_delay_ms(10);
HALL_PORT |= _BV(HALL_VDD);
//hall_triggered = true;
BUZZER_PORT |= _BV(BUZZER_PIN);
_delay_ms(5);
BUZZER_PORT &= ~_BV(BUZZER_PIN);
_delay_ms(10);
sei();
}
#endif
Raw
pi_robot.h
#include <avr/io.h>
#include <stdio.h>
#define F_CPU 16000000UL
#undef BAUD
#define BAUD 115200UL
#include <util/setbaud.h>
#include <stdbool.h>
#include <util/delay.h>
#define LED_DDR DDRB
#define LED_PORT PORTB
#define RED_LED 1
#define GREEN_LED 0
#define BLUE_LED 4
#define MOTOR_EN1_DDR DDRD
#define MOTOR_EN1_PIN 7
#define MOTOR_EN1_PORT PORTD
#define MOTOR_EN2_DDR DDRD
#define MOTOR_EN2_PORT PORTD
#define MOTOR_EN2_PIN 5
#define MOTOR_IN1_DDR DDRC
#define MOTOR_IN1_PORT PORTC
#define MOTOR_IN1_PIN 2
#define MOTOR_IN2_DDR DDRD
#define MOTOR_IN2_PORT PORTD
#define MOTOR_IN2_PIN 6
#define MOTOR_IN3_DDR DDRD
#define MOTOR_IN3_PORT PORTD
#define MOTOR_IN3_PIN 3
#define MOTOR_IN4_DDR DDRC
#define MOTOR_IN4_PORT PORTC
#define MOTOR_IN4_PIN 3
#define BUZZER_DDR DDRD
#define BUZZER_PORT PORTD
#define BUZZER_PIN 4
#define BUTTON_DDR DDRC
#define BUTTON_PORT PORTC
#define BUTTON_PIN 7
#define QRD_DDR DDRA
#define QRD_PORT PORTA
#define QRD_EN_SOL_PIN 4
#define QRD_SOL_PIN 3
#define QRD_ORTA_PIN 2
#define QRD_SAG_PIN 0
#define QRD_EN_SAG_PIN 1
#define QRD_EN_SAG_PIN 1
#define QRD_EN_SAG_PIN 1
#define QRD_EN_SAG_PIN 1
#define HALL_CONNECTED 1
#ifdef HALL_CONNECTED
#define HALL_DDR DDRB
#define HALL_PORT PORTB
#define HALL_PIN PINB
#define HALL_INPUT 2
#define HALL_VDD 3
#define HALL_GND 5
#endif
#define LEARN 0
#define BLACK 1
#define SOLVE 2
#define WAIT 3
#define PID_DEBUG 0
#define SENSOR_DEBUG 0
#define POWER_DIFF_DEBUG 0
#define BlackButtonBit (PINC & (1<<7)) >> 7
#define ILERI 10
#define GERI 11
#define SOL 12
#define SAG 13
void delay_ms(int16_t);
void calibrate_sensors(void);
int16_t read_line(int16_t *sensors, unsigned char line_color);
int16_t read_one_sensor(int16_t sensor_id);
int16_t read_hall_sensor(void);
int16_t read_adc(int16_t);
void set_right_motor_pwm(int16_t pwm);
void set_right_motor(int16_t pwm);
void set_left_motor_pwm(int16_t pwm);
void set_left_motor(int16_t pwm);
void set_motors(int16_t left, int16_t right);
void go_ahead(unsigned char line_color);
void turn(unsigned char turn_dir);
void define_coordinate(int16_t dir);
void optimize_path(void);
void stopper(void);
void set_rgb(bool, bool, bool);
int QRD_MAX_VALUE;
int QRD_MIN_VALUE;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment