Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. cca377a8e66eb33b0a243b4807ff421bf6d24d0e
|
From: Xavier L. <Ba...@us...> - 2010-05-17 15:28:17
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via cca377a8e66eb33b0a243b4807ff421bf6d24d0e (commit)
via d2e6aa0c441efc08c30ce1243aaa53f2f0096b7b (commit)
via a9ddabc53c3289331e69ca738e4c8896b66f8452 (commit)
from 10395230bcd78851504981612ca1ed3b82bdc8e2 (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit cca377a8e66eb33b0a243b4807ff421bf6d24d0e
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 17 17:25:18 2010 +0200
Modifications during the French Robotic Cup :
* The lift is working
* AX12 are not currently operationals
* a new board has been added to use chibios PORT definition system
* The monitor is now running again
Partial Match logic has been implemented to do the necessary things for the cup. They need to be rewritten cleanly !
commit d2e6aa0c441efc08c30ce1243aaa53f2f0096b7b
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon May 17 17:15:15 2010 +0200
Adapted Firmware to work with the real boards (not the developpement ones)
commit a9ddabc53c3289331e69ca738e4c8896b66f8452
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 14 00:37:06 2010 +0200
typo correction
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/encoder.c b/elec/boards/Controller_HolonomicDrive/Firmware/encoder.c
index 59d306a..7c1fd72 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/encoder.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/encoder.c
@@ -24,12 +24,12 @@ void encodersInit(void) {
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
//Setup timer for quadrature encoder interface
- //Encoder1 A channel at PA0.6 (Ch1)
- // B channel at PA0.7 (Ch2)
- //Encoder2 A channel at PA0.8 (Ch1)
- // B channel at PA0.9 (Ch2)
- //Encoder3 A channel at PB0.6 (Ch1)
- // B channel at PB0.7 (Ch2)
+ //Encoder1 A channel at PA8 (Ch1)
+ // B channel at PA9 (Ch2) -> TIM1
+ //Encoder2 A channel at PA6 (Ch1)
+ // B channel at PA7 (Ch2) -> TIM3
+ //Encoder3 A channel at PB6 (Ch1)
+ // B channel at PB7 (Ch2) -> TIM4
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/main.c b/elec/boards/Controller_HolonomicDrive/Firmware/main.c
index 7988cf8..71d05e5 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/main.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/main.c
@@ -46,7 +46,7 @@ static msg_t Thread1(void *arg) {
static void TimerHandler(eventid_t id) {
(void)id;
- if (palReadPad(IOPORT1, GPIOA_BUTTON)) {
+ if (!palReadPad(IOPORT1, GPIOA_BUTTON)) {
palClearPad(IOPORT3, GPIOC_LED);
chThdSleepMilliseconds(100);
@@ -96,7 +96,7 @@ static void TimerHandler(eventid_t id) {
setScrew(0, 0, 282, 282, 0);
chThdSleepMilliseconds(1500);
turn(0);
- }
+ }
}
/*
@@ -122,12 +122,12 @@ int main(int argc, char **argv) {
/*
* Activates the serial driver 2 using the driver default configuration.
*/
- sdStart(&SD2, NULL);
+ //sdStart(&SD2, NULL);
/*
* Initialise the monitor
*/
- monitorInit();
+ //monitorInit();
/*
* Initialise the CAN monitor
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
index 9618d84..748356f 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
@@ -19,7 +19,7 @@ void VectorB0(void) {
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
// End of motor1 pulse
- GPIO_ResetBits(GPIOC, GPIO_Pin_13);
+ GPIO_ResetBits(GPIOA, GPIO_Pin_1);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
{
@@ -27,7 +27,7 @@ void VectorB0(void) {
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
// End of motor2 pulse
- GPIO_ResetBits(GPIOC, GPIO_Pin_3);
+ GPIO_ResetBits(GPIOA, GPIO_Pin_2);
}
else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
{
@@ -35,7 +35,7 @@ void VectorB0(void) {
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
// End of motor3 pulse
- GPIO_ResetBits(GPIOC, GPIO_Pin_9);
+ GPIO_ResetBits(GPIOA, GPIO_Pin_3);
}
else {
// TIM_IT_Update
@@ -43,7 +43,7 @@ void VectorB0(void) {
// Clear TIM2 Update interrupt pending bit
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
- GPIO_SetBits(GPIOC, GPIO_Pin_3 | GPIO_Pin_9 | GPIO_Pin_13);
+ GPIO_SetBits(GPIOA, GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3);
}
}
@@ -67,17 +67,17 @@ void motorsInit(void) {
//Motor1 : STBY PC10
// IN1 PB0
// IN2 PB1
- // PWM PC13
+ // PWM PA1
//Motor2 : STBY PC0
// IN1 PC1
// IN2 PC2
- // PWM PC3
+ // PWM PA2
//Motor3 : STBY PC6
// IN1 PC7
// IN2 PC8
- // PWM PC9
- GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6
- | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_13);
+ // PWM PA3
+ GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_6
+ | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_10 | GPIO_Pin_13);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
@@ -85,6 +85,9 @@ void motorsInit(void) {
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;
+ GPIO_Init(GPIOA, &GPIO_InitStructure);
+
// Default value of H-Bridge configuration
GPIO_ResetBits(GPIOB, GPIO_Pin_0 | GPIO_Pin_1);
GPIO_ResetBits(GPIOC, GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_10
@@ -93,7 +96,7 @@ void motorsInit(void) {
// TimeBase configuration
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_Period = 36000; // 20 kHz
+ TIM_TimeBaseStructure.TIM_Period = 36000; // 2 kHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
index eb14e9b..0d7fbdc 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
@@ -57,7 +57,7 @@ LDSCRIPT= ch.ld
# Imported source files
CHIBIOS = chibios
-include $(CHIBIOS)/boards/OLIMEX_STM32_P103/board.mk
+include $(CHIBIOS)/boards/KROBOTJR_MOTHERBOARD2010/board.mk
include $(CHIBIOS)/os/hal/platforms/STM32/platform.mk
include $(CHIBIOS)/os/hal/hal.mk
include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F10x/port.mk
@@ -79,6 +79,8 @@ CSRC += $(CHIBIOS)/os/various/syscalls.c
CSRC += $(CHIBIOS)/os/various/shell.c
CSRC += monitor.c
CSRC += can_monitor.c
+CSRC += lift.c
+CSRC += ax12.c
CSRC += main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
@@ -160,7 +162,7 @@ CPPWARN = -Wall -Wextra
#
# List all default C defines here, like -D_DEBUG=1
-DDEFS = -DSTDOUT_SD=SD2 -DSTDIN_SD=SD2
+DDEFS = -DSTDOUT_SD=SD1 -DSTDIN_SD=SD1
# List all default ASM defines here, like -D_DEBUG=1
DADEFS =
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.c
new file mode 100644
index 0000000..848c303
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.c
@@ -0,0 +1,135 @@
+/*
+ * Serial communication with AX12 digital servomotors
+ * Xavier Lagorce
+ */
+
+#include "ax12.h"
+
+BaseChannel *ax12_chp = (BaseChannel*)&SD2;
+
+typedef enum {
+ RD_READY = 0,
+ RD_BEGIN = 1,
+ RD_ID = 2,
+ RD_LEN = 3,
+ RD_ERROR = 4,
+ RD_PAYLOAD = 5,
+ RD_CHECKSUM = 6,
+} rdstate_t;
+
+
+static WORKING_AREA(waThreadAX12Rec, 256);
+static msg_t ThreadAX12Rec(void *arg) {
+
+ rdstate_t state = RD_READY;
+ uint8_t id=0, len=0, count=0, error=0, chksum=0, i;
+ uint8_t payload[10];
+ Thread *waiter = NULL, *msg = NULL;
+
+ (void)arg;
+ while (TRUE) {
+ uint8_t c = (uint8_t)chIOGet(ax12_chp);
+
+ msg = (Thread*)chMsgGet();
+ if (msg != NULL) {
+ waiter = msg;
+ chMsgRelease(RDY_OK);
+ }
+
+ switch (state) {
+ case RD_READY:
+ if (c == 0xFF)
+ state = RD_BEGIN;
+ break;
+ case RD_BEGIN:
+ if (c == 0xFF)
+ state = RD_ID;
+ else
+ state = RD_READY;
+ break;
+ case RD_ID:
+ id = c;
+ state = RD_LEN;
+ break;
+ case RD_LEN:
+ len = c - 2;
+ count = 0;
+ state = RD_ERROR;
+ break;
+ case RD_ERROR:
+ error = c;
+ if (len == 0)
+ state = RD_CHECKSUM;
+ else
+ state = RD_PAYLOAD;
+ break;
+ case RD_PAYLOAD:
+ count++;
+ payload[count] = c;
+ if (count == len)
+ state = RD_CHECKSUM;
+ break;
+ case RD_CHECKSUM:
+ chksum = id+len+error;
+ for (i=0; i < len ; i++)
+ chksum += payload[i];
+ chksum = ~chksum;
+ if (waiter != NULL) {
+ if (c != chksum) {
+ chEvtSignal(waiter, EVT_AX12_ERROR);
+ }
+ else {
+ chEvtSignal(waiter, EVT_AX12_PACKET);
+ chMsgSend(waiter, (msg_t)payload);
+ }
+ waiter = NULL;
+ }
+ state = RD_READY;
+ break;
+ default:
+ state = RD_READY;
+ break;
+ }
+ }
+ return 0;
+}
+
+void ax12Init(void) {
+ const SerialConfig ax12_config =
+ {
+ AX12_BAUDRATE,
+ 0,
+ USART_CR2_STOP1_BITS | USART_CR2_LINEN,
+ 0
+ };
+
+ palSetPadMode(IOPORT1, 1, PAL_MODE_OUTPUT_PUSHPULL);
+ palClearPad(IOPORT1, 1); // reading mode
+
+ sdStart(&SD2, &ax12_config);
+ chThdCreateStatic(waThreadAX12Rec, sizeof(waThreadAX12Rec), NORMALPRIO+1, ThreadAX12Rec, NULL);
+}
+
+void ax12SendPacket(uint8_t id, uint8_t instruction, uint8_t len, uint8_t *params) {
+ uint8_t chksum, i;
+
+ chksum = id + len + 2;
+ for (i = 0; i < len; i++)
+ chksum += params[i];
+ chksum = ~chksum;
+
+ // Writing mode
+ //palClearPad(IOPORT1, 1);
+
+ chIOPut(ax12_chp, 0xFF);
+ chIOPut(ax12_chp, 0xFF);
+ chIOPut(ax12_chp, id);
+ chIOPut(ax12_chp, len+2);
+ chIOPut(ax12_chp, instruction);
+ for (i=0; i<len; i++)
+ chIOPut(ax12_chp, params[i]);
+ chIOPut(ax12_chp, chksum);
+
+ // Back to reading mode
+ //palSetPad(IOPORT1, 1);
+}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.h
new file mode 100644
index 0000000..e46a9fb
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/ax12.h
@@ -0,0 +1,118 @@
+/*
+ * Serial communication with AX12 digital servomotors
+ * Xavier Lagorce
+ */
+
+#ifndef HEADER__AX12
+#define HEADER__AX12
+
+#define AX12_BAUDRATE 1000000
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "ch.h"
+#include "hal.h"
+
+// --- Control Table Address ---
+// EEPROM area
+#define P_MODEL_NUMBER 0
+#define P_MODEL_NUMBER_L 0
+#define P_MODEL_NUMBER_H 1
+#define P_VERSION 2
+#define P_ID 3
+#define P_BAUD_RATE 4
+#define P_RETURN_DELAY_TIME 5
+#define P_CW_ANGLE_LIMIT 6
+#define P_CW_ANGLE_LIMIT_L 6
+#define P_CW_ANGLE_LIMIT_H 7
+#define P_CCW_ANGLE_LIMIT 8
+#define P_CCW_ANGLE_LIMIT_L 8
+#define P_CCW_ANGLE_LIMIT_H 9
+#define P_SYSTEM_DATA2 10
+#define P_LIMIT_TEMPERATURE 11
+#define P_DOWN_LIMIT_VOLTAGE 12
+#define P_UP_LIMIT_VOLTAGE 13
+#define P_MAX_TORQUE 14
+#define P_MAX_TORQUE_L 14
+#define P_MAX_TORQUE_H 15
+#define P_RETURN_LEVEL 16
+#define P_ALARM_LED 17
+#define P_ALARM_SHUTDOWN 18
+#define P_OPERATING_MODE 19
+#define P_DOWN_CALIBRATION 20
+#define P_DOWN_CALIBRATION_L 20
+#define P_DOWN_CALIBRATION_H 21
+#define P_UP_CALIBRATION 22
+#define P_UP_CALIBRATION_L 22
+#define P_UP_CALIBRATION_H 23
+
+// RAM area
+#define P_TORQUE_ENABLE 24
+#define P_LED 25
+#define P_CW_COMPLIANCE_MARGIN 26
+#define P_CCW_COMPLIANCE_MARGIN 27
+#define P_CW_COMPLIANCE_SLOPE 28
+#define P_CCW_COMPLIANCE_SLOPE 29
+#define P_GOAL_POSITION 30
+#define P_GOAL_POSITION_L 30
+#define P_GOAL_POSITION_H 31
+#define P_GOAL_SPEED 32
+#define P_GOAL_SPEED_L 32
+#define P_GOAL_SPEED_H 33
+#define P_TORQUE_LIMIT 34
+#define P_TORQUE_LIMIT_L 34
+#define P_TORQUE_LIMIT_H 35
+#define P_PRESENT_POSITION 36
+#define P_PRESENT_POSITION_L 36
+#define P_PRESENT_POSITION_H 37
+#define P_PRESENT_SPEED 38
+#define P_PRESENT_SPEED_L 38
+#define P_PRESENT_SPEED_H 39
+#define P_PRESENT_LOAD 40
+#define P_PRESENT_LOAD_L 40
+#define P_PRESENT_LOAD_H 41
+#define P_PRESENT_VOLTAGE 42
+#define P_PRESENT_TEMPERATURE 43
+#define P_REGISTERED_INSTRUCTION 44
+#define P_PAUSE_TIME 45
+#define P_MOVING 46
+#define P_LOCK 47
+#define P_PUNCH 48
+#define P_PUNCH_L 48
+#define P_PUNCH_H 49
+
+// --- Instruction ---
+#define INST_PING 0x01
+#define INST_READ 0x02
+#define INST_WRITE 0x03
+#define INST_REG_WRITE 0x04
+#define INST_ACTION 0x05
+#define INST_RESET 0x06
+#define INST_DIGITAL_RESET 0x07
+#define INST_SYSTEM_READ 0x0C
+#define INST_SYSTEM_WRITE 0x0D
+#define INST_SYNC_WRITE 0x83
+#define INST_SYNC_REG_WRITE 0x84
+
+// --- Error ---
+#define ERR_INPUT_VOLTAGE 1
+#define ERR_ANGLE_LIMIT 2
+#define ERR_OVERHEATING 4
+#define ERR_RANGE 8
+#define ERR_CHECKSUM 16
+#define ERR_OVERLOAD 32
+#define ERR_INSTRUCTION 64
+
+// --- Valeurs specifiques ---
+#define ID_BROADCAST 0xFE
+
+// Événements
+#define EVT_AX12_ERROR 1
+#define EVT_AX12_PACKET 2
+
+void ax12Init(void);
+void ax12SendPacket(uint8_t id, uint8_t instruction, uint8_t len, uint8_t *params);
+
+#endif
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/can_monitor.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/can_monitor.c
index 928a08f..8c40190 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/can_monitor.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/can_monitor.c
@@ -135,7 +135,7 @@ void canSetSpeeds(int32_t motor1, int32_t motor2, int32_t motor3) {
speedMsg_t *speedMsg;
txmsg.cf_IDE = CAN_IDE_EXT;
- txmsg.cf_EID = 0x300;
+ txmsg.cf_EID = 0x400;
txmsg.cf_RTR = CAN_RTR_DATA;
txmsg.cf_DLC = 8;
speedMsg = (speedMsg_t*)(&(txmsg.cf_data8[0]));
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.c
new file mode 100644
index 0000000..77f9581
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.c
@@ -0,0 +1,49 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010 Giovanni Di Sirio.
+
+ This file is part of ChibiOS/RT.
+
+ ChibiOS/RT 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.
+
+ ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "ch.h"
+#include "hal.h"
+
+/*
+ * Early initialization code.
+ * This initialization is performed just after reset before BSS and DATA
+ * segments initialization.
+ */
+void hwinit0(void) {
+
+ stm32_clock_init();
+}
+
+/*
+ * Late initialization code.
+ * This initialization is performed after BSS and DATA segments initialization
+ * and before invoking the main() function.
+ */
+void hwinit1(void) {
+
+ /*
+ * HAL initialization.
+ */
+ halInit();
+
+ /*
+ * ChibiOS/RT initialization.
+ */
+ chSysInit();
+}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.h
new file mode 100644
index 0000000..b0d7b76
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.h
@@ -0,0 +1,130 @@
+/*
+ ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010 Giovanni Di Sirio.
+
+ This file is part of ChibiOS/RT.
+
+ ChibiOS/RT 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.
+
+ ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _BOARD_H_
+#define _BOARD_H_
+
+/*
+ * Setup for the Olimex STM33-P103 proto board.
+ */
+
+/*
+ * Board identifier.
+ */
+#define BOARD_OLIMEX_STM32_P103
+#define BOARD_NAME "Olimex STM32-P103"
+
+/*
+ * Board frequencies.
+ */
+#define LSECLK 32768
+#define HSECLK 8000000
+#define HSICLK 8000000
+
+/*
+ * IO pins assignments.
+ */
+#define GPIOA_BUTTON 0
+#define GPIOA_SPI1NSS 4
+
+#define GPIOB_SPI2NSS 12
+
+#define GPIOC_MMCWP 6
+#define GPIOC_MMCCP 7
+#define GPIOC_CANCNTL 10
+#define GPIOC_DISC 11
+#define GPIOC_LED 12
+
+/*
+ * I/O ports initial setup, this configuration is established soon after reset
+ * in the initialization code.
+ *
+ * The digits have the following meaning:
+ * 0 - Analog input.
+ * 1 - Push Pull output 10MHz.
+ * 2 - Push Pull output 2MHz.
+ * 3 - Push Pull output 50MHz.
+ * 4 - Digital input.
+ * 5 - Open Drain output 10MHz.
+ * 6 - Open Drain output 2MHz.
+ * 7 - Open Drain output 50MHz.
+ * 8 - Digital input with PullUp or PullDown resistor depending on ODR.
+ * 9 - Alternate Push Pull output 10MHz.
+ * A - Alternate Push Pull output 2MHz.
+ * B - Alternate Push Pull output 50MHz.
+ * C - Reserved.
+ * D - Alternate Open Drain output 10MHz.
+ * E - Alternate Open Drain output 2MHz.
+ * F - Alternate Open Drain output 50MHz.
+ * Please refer to the STM32 Reference Manual for details.
+ */
+
+/*
+ * Port A setup.
+ * Everything input with pull-up except:
+ * PA0 - Normal input (BUTTON).
+ * PA2 - Alternate output (USART2 TX).
+ * PA3 - Normal input (USART2 RX).
+ */
+#define VAL_GPIOACRL 0x88884B38 /* PA7...PA0 */
+#define VAL_GPIOACRH 0x888884B8 /* PA15...PA8 */
+#define VAL_GPIOAODR 0xFFFFFFFF
+
+/*
+ * Port B setup.
+ * Everything input with pull-up except:
+ * PB13 - Alternate output (MMC SPI2 SCK).
+ * PB14 - Normal input (MMC SPI2 MISO).
+ * PB15 - Alternate output (MMC SPI2 MOSI).
+ */
+#define VAL_GPIOBCRL 0x88888888 /* PB7...PB0 */
+#define VAL_GPIOBCRH 0xB4B88888 /* PB15...PB8 */
+#define VAL_GPIOBODR 0xFFFFFFFF
+
+/*
+ * Port C setup.
+ * Everything input with pull-up except:
+ * PC6 - Normal input because there is an external resistor.
+ * PC7 - Normal input because there is an external resistor.
+ * PC11 - Push Pull output (CAN CNTRL).
+ * PC12 - Push Pull output (LED).
+ */
+#define VAL_GPIOCCRL 0x44888888 /* PC7...PC0 */
+#define VAL_GPIOCCRH 0x88833888 /* PC15...PC8 */
+#define VAL_GPIOCODR 0xFFFFFFFF
+
+/*
+ * Port D setup.
+ * Everything input with pull-up except:
+ * PD0 - Normal input (XTAL).
+ * PD1 - Normal input (XTAL).
+ */
+#define VAL_GPIODCRL 0x88888844 /* PD7...PD0 */
+#define VAL_GPIODCRH 0x88888888 /* PD15...PD8 */
+#define VAL_GPIODODR 0xFFFFFFFF
+
+/*
+ * Port E setup.
+ * Everything input with pull-up except:
+ */
+#define VAL_GPIOECRL 0x88888888 /* PE7...PE0 */
+#define VAL_GPIOECRH 0x88888888 /* PE15...PE8 */
+#define VAL_GPIOEODR 0xFFFFFFFF
+
+#endif /* _BOARD_H_ */
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.mk b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.mk
new file mode 100644
index 0000000..e9ed947
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/chibios/boards/KROBOTJR_MOTHERBOARD2010/board.mk
@@ -0,0 +1,5 @@
+# List of all the board related files.
+BOARDSRC = ${CHIBIOS}/boards/KROBOTJR_MOTHERBOARD2010/board.c
+
+# Required include directories
+BOARDINC = ${CHIBIOS}/boards/KROBOTJR_MOTHERBOARD2010
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/halconf.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/halconf.h
index 40eceea..ba5f1cb 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/halconf.h
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/halconf.h
@@ -66,7 +66,7 @@
* @brief Enables the ADC subsystem.
*/
#if !defined(CH_HAL_USE_ADC) || defined(__DOXYGEN__)
-#define CH_HAL_USE_ADC FALSE
+#define CH_HAL_USE_ADC TRUE
#endif
/*===========================================================================*/
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.c
new file mode 100644
index 0000000..6f7d7cd
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.c
@@ -0,0 +1,72 @@
+/*
+ * Lift management on Krobot Jr
+ * Xavier Lagorce
+ */
+
+#include "lift.h"
+
+void liftInit(void) {
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+ uint16_t PrescalerValue = 0;
+
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
+ RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
+
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ // Compute the prescaler value
+ PrescalerValue = (uint16_t) (72000000 / 3276800) - 1;
+ // Time base configuration
+ TIM_TimeBaseStructure.TIM_Period = 65455;
+ TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+
+ TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+
+ // PWM1 Mode configuration: Channel1
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 3272;//3272;//4909;//6545;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+
+ TIM_OC1Init(TIM3, &TIM_OCInitStructure);
+
+ TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ // PWM1 Mode configuration: Channel2
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+
+ TIM_OC2Init(TIM3, &TIM_OCInitStructure);
+
+ TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ // PWM1 Mode configuration: Channel3
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+
+ TIM_OC3Init(TIM3, &TIM_OCInitStructure);
+
+ TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ // PWM1 Mode configuration: Channel4
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+
+ TIM_OC4Init(TIM3, &TIM_OCInitStructure);
+
+ TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
+ TIM_ARRPreloadConfig(TIM3, ENABLE);
+
+ // TIM3 enable counter
+ TIM_Cmd(TIM3, ENABLE);
+
+}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.h
new file mode 100644
index 0000000..d4f467f
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/lift.h
@@ -0,0 +1,18 @@
+/*
+ * Lift management on Krobot Jr
+ * Xavier Lagorce
+ */
+
+#ifndef HEADER__LIFT
+#define HEADER__LIFT
+
+#include "ch.h"
+#include "hal.h"
+#include "stm32f10x.h"
+#include "stm32f10x_tim.h"
+#include "stm32f10x_gpio.h"
+#include "stm32f10x_rcc.h"
+
+void liftInit(void);
+
+#endif
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
index 7b9de77..e15ff48 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
@@ -9,11 +9,15 @@
#include "ch.h"
#include "hal.h"
+#include "pal.h"
#include "evtimer.h"
#include "monitor.h"
#include "can_monitor.h"
+#include "lift.h"
+#include "ax12.h"
+
/*
* Global variables
*/
@@ -36,29 +40,192 @@ static msg_t Thread1(void *arg) {
return 0;
}
+#define ADC_GRP1_NUM_CHANNELS 8
+#define ADC_GRP1_BUF_DEPTH 16
+
+static adcsample_t samples[ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH];
+static Thread *adctp;
+
+const ADCConfig adccfg = {};
+const ADCConversionGroup adcgrpcfg = {
+ TRUE,
+ ADC_GRP1_NUM_CHANNELS,
+ 0,
+ ADC_CR2_EXTSEL_SWSTART | ADC_CR2_TSVREFE | ADC_CR2_CONT,
+ 0,
+ 0,
+ ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS),
+ ADC_SQR2_SQ7_N(ADC_CHANNEL_SENSOR) | ADC_SQR2_SQ6_N(ADC_CHANNEL_VREFINT),
+ ADC_SQR3_SQ5_N(ADC_CHANNEL_IN11) | ADC_SQR3_SQ4_N(ADC_CHANNEL_IN10) |
+ ADC_SQR3_SQ3_N(ADC_CHANNEL_IN11) | ADC_SQR3_SQ2_N(ADC_CHANNEL_IN10) |
+ ADC_SQR3_SQ1_N(ADC_CHANNEL_IN11) | ADC_SQR3_SQ0_N(ADC_CHANNEL_IN10)
+};
+
+/*
+ * ADC continuous conversion thread.
+ */
+size_t nx = 0, ny = 0;
+static void adccallback(adcsample_t *buffer, size_t n) {
+
+ static uint16_t count = 0;
+
+ if (buffer[11] >= 2400 ) {
+ palClearPad(IOPORT3, GPIOC_LED);
+ //canSetScrew(0,0,0,0,0);
+ //while(1);
+ count++;
+ }
+ else
+ palSetPad(IOPORT3, GPIOC_LED);
+
+ if (count >= 10) {
+ canSetScrew(0,0,0,0,0);
+ while(1);
+ }
+}
+
+static WORKING_AREA(adc_continuous_wa, 256);
+static msg_t adc_continuous_thread(void *p){
+
+ (void)p;
+ palSetGroupMode(IOPORT3,
+ PAL_PORT_BIT(0) | PAL_PORT_BIT(1),
+ PAL_MODE_INPUT_ANALOG);
+ adcStart(&ADCD1, &adccfg);
+ while(1) {
+ adcStartConversion(&ADCD1, &adcgrpcfg, samples,
+ ADC_GRP1_BUF_DEPTH, adccallback);
+ adcWaitConversion(&ADCD1, TIME_INFINITE);
+ adcStop(&ADCD1);}
+ return 0;
+}
+
+
+static WORKING_AREA(waThread2, 1024);
+static msg_t Thread2(void *arg) {
+
+ /*TIM_OCInitTypeDef TIM_OCInitStructure;
+
+ // PWM1 Mode configuration: Channel1
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+
+ (void)arg;
+ while (TRUE) {
+
+ TIM_OCInitStructure.TIM_Pulse = 3272;
+ TIM_OC1Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
+ TIM_ARRPreloadConfig(TIM3, ENABLE);
+ chThdSleepMilliseconds(10000);
+
+ TIM_OCInitStructure.TIM_Pulse = 6545;
+ TIM_OC1Init(TIM3, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
+ TIM_ARRPreloadConfig(TIM3, ENABLE);
+ chThdSleepMilliseconds(10000);
+ }
+ };*/
+
+ adctp = chThdCreateStatic(adc_continuous_wa, sizeof(adc_continuous_wa),
+ NORMALPRIO + 9, adc_continuous_thread, NULL);
+
+ chThdSleep(MS2ST(85000));
+ canSetScrew(0, 0, 0, 0, 0);
+ while(1);
+
+ return 0;
+}
+
+
/*
* Executed as event handler at 500mS intervals.
*/
static void TimerHandler(eventid_t id) {
- CANTxFrame txmsg;
- moveMsg_t *moveMsg;
+ uint8_t color;
(void)id;
- if (palReadPad(IOPORT1, GPIOA_BUTTON)) {
-
- txmsg.cf_IDE = CAN_IDE_EXT;
- txmsg.cf_EID = 0x300;
- txmsg.cf_RTR = CAN_RTR_DATA;
- txmsg.cf_DLC = 8;
- moveMsg = (moveMsg_t*)(&(txmsg.cf_data8[0]));
- moveMsg->move.ptX = 0;
- moveMsg->move.ptY = 200;
- moveMsg->move.vX = 0;
- moveMsg->move.vY = 0;
- moveMsg->move.omega = 60;
-
- canTransmit(&CAND1, &txmsg, MS2ST(100));
+
+ if (!palReadPad(IOPORT3, 8)) {
+ if (palReadPad(IOPORT3, 6))
+ color = 0;
+ else
+ color = 1;
+
+ chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO+1, Thread2, NULL);
+
+
+ canSetScrew(0, 0, 0, -174, 0);
+ chThdSleep(MS2ST(2000));
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleep(MS2ST(100));
+ if (color == 0) {
+ canSetScrew(0, 0, 0, 0, 40);
+ chThdSleep(MS2ST(1600));
+ } else
+ {
+ canSetScrew(0, 0, 0, 0, -40);
+ chThdSleep(MS2ST(1600));
+ }
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleep(MS2ST(100));
+ canSetScrew(0, 0, 0, -300, 0);
+ chThdSleep(MS2ST(15000));
+ canSetScrew(0, 0, 0, 0, 0);
}
+ /*if (!palReadPad(IOPORT1, GPIOA_BUTTON)) {
+
+ palClearPad(IOPORT3, GPIOC_LED);
+ chThdSleepMilliseconds(100);
+ palSetPad(IOPORT3, GPIOC_LED);
+ chThdSleepMilliseconds(100);
+ palClearPad(IOPORT3, GPIOC_LED);
+ chThdSleepMilliseconds(100);
+ palSetPad(IOPORT3, GPIOC_LED);
+ chThdSleepMilliseconds(100);
+ palClearPad(IOPORT3, GPIOC_LED);
+ chThdSleepMilliseconds(100);
+ palSetPad(IOPORT3, GPIOC_LED);
+
+
+ chThdSleepMilliseconds(2000);
+
+ canSetScrew(0, 0, 141, 141, 0);
+ chThdSleepMilliseconds(5000);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleepMilliseconds(200);
+
+ canSetScrew(450, 200, 0, 0, 40);
+ chThdSleepMilliseconds(3000);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleepMilliseconds(200);
+
+ canSetScrew(0, 0, 0, 0, 60);
+ chThdSleepMilliseconds(2000);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleepMilliseconds(200);
+
+ canSetScrew(0, 0, 200, 0, 0);
+ chThdSleepMilliseconds(1500);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleepMilliseconds(200);
+
+ canSetScrew(0, 0, 0, 200, 0);
+ chThdSleepMilliseconds(3500);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ chThdSleepMilliseconds(200);
+
+ canSetScrew(0, 0, 282, 282, 0);
+ chThdSleepMilliseconds(1500);
+
+ canSetScrew(0, 0, 0, 0, 0);
+ }*/
}
/*
@@ -77,9 +244,9 @@ int main(int argc, char **argv) {
(void)argv;
/*
- * Activates the serial driver 2 using the driver default configuration.
+ * Activates the serial driver 1 using the driver default configuration.
*/
- sdStart(&SD2, NULL);
+ sdStart(&SD1, NULL);
/*
* Initialise the monitor
@@ -93,14 +260,35 @@ int main(int argc, char **argv) {
canMonitorStart();
/*
+ * Initialise the lift
+ */
+ liftInit();
+ ax12Init();
+
+ //chThdSleep(MS2ST(2000));
+
+ /*
+ * Init pins
+ */
+ palSetGroupMode(IOPORT3, PAL_PORT_BIT(6) | PAL_PORT_BIT(8), PAL_MODE_INPUT_PULLDOWN);
+ palSetGroupMode(IOPORT3, PAL_PORT_BIT(0) | PAL_PORT_BIT(2), PAL_MODE_INPUT_ANALOG);
+ adcStart(&ADCD1, NULL);
+ //adctp = chThdCreateStatic(adc_continuous_wa, sizeof(adc_continuous_wa),
+ // NORMALPRIO + 9, adc_continuous_thread, NULL);
+ /*
* Creates the blinker thread.
*/
- chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO+1, Thread1, NULL);
+ //chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO+1, Thread1, NULL);
+ //chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO+1, Thread2, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
+
+ uint8_t params[] = {P_GOAL_POSITION, 0x00, 0x02, 0x00, 0x02};
+ ax12SendPacket(1, INST_WRITE, 5, params);
+
evtInit(&evt, MS2ST(500)); /* Initializes an event timer object. */
evtStart(&evt); /* Starts the event timer. */
chEvtRegister(&evt.et_es, &el0, 0); /* Registers on the timer event source. */
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/mcuconf.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/mcuconf.h
index 2a72a33..0a1c8b0 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/mcuconf.h
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/mcuconf.h
@@ -49,7 +49,7 @@
/*
* ADC driver system settings.
*/
-#define USE_STM32_ADC1 FALSE
+#define USE_STM32_ADC1 TRUE
#define STM32_ADC1_DMA_PRIORITY 3
#define STM32_ADC1_IRQ_PRIORITY 0x50
#define STM32_ADC1_DMA_ERROR_HOOK() chSysHalt()
@@ -75,7 +75,7 @@
/*
* SERIAL driver system settings.
*/
-#define USE_STM32_USART1 FALSE
+#define USE_STM32_USART1 TRUE
#define USE_STM32_USART2 TRUE
#define USE_STM32_USART3 FALSE
#define STM32_USART1_PRIORITY 0xC0
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
index 2954fb1..4f3950f 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
@@ -6,9 +6,9 @@
#ifndef HEADER__MONITOR
#define HEADER__MONITOR
-// Default to USART2
+// Default to USART1
#ifndef MONITOR_USART
-#define MONITOR_USART SD2
+#define MONITOR_USART SD1
#endif
#include <stdio.h>
hooks/post-receive
--
krobot
|
