Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. fc0d09703d036e8fc2748caaf604f4954f5761b6
|
From: Xavier L. <Ba...@us...> - 2010-04-19 22:43:14
|
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 fc0d09703d036e8fc2748caaf604f4954f5761b6 (commit)
from 5f90539cdc0f9083a613e7004fa441489a337190 (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 fc0d09703d036e8fc2748caaf604f4954f5761b6
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 20 00:41:15 2010 +0200
Added a speed controller to the firmware
Serial COM and monitoring shell are deactivated beacause they seem to hang the program.
Krobot Junior can move in a triangle !
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
index 1b61e32..c6ef472 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/Makefile
@@ -81,6 +81,7 @@ CSRC += monitor.c
CSRC += encoder.c
CSRC += motor.c
CSRC += cpu_load.c
+CSRC += speed_control.c
CSRC += main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
index 69e2766..ec95407 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/main.c
@@ -1,6 +1,7 @@
/*
- Template program using ChibiOS/RT
-*/
+ * Control Software for the Krobot Junior MotherBoard
+ * Xavier Lagorce
+ */
#include <stdio.h>
#include <stdlib.h>
@@ -12,8 +13,8 @@
#include "monitor.h"
#include "encoder.h"
-#include "motor.h"
#include "cpu_load.h"
+#include "speed_control.h"
/*
* Global variables
@@ -27,10 +28,31 @@ static msg_t Thread1(void *arg) {
(void)arg;
while (TRUE) {
+ sc_setRefSpeed(MOTOR1, 0);
+ sc_setRefSpeed(MOTOR2, -360);
+ sc_setRefSpeed(MOTOR3, 360);
+ chThdSleepMilliseconds(2000);
+
+ sc_setRefSpeed(MOTOR1, 360);
+ sc_setRefSpeed(MOTOR2, 0);
+ sc_setRefSpeed(MOTOR3, -360);
+ chThdSleepMilliseconds(2000);
+
+
+ sc_setRefSpeed(MOTOR1, -360);
+ sc_setRefSpeed(MOTOR2, 360);
+ sc_setRefSpeed(MOTOR3, 0);
+ chThdSleepMilliseconds(2000);
+
+ sc_setRefSpeed(MOTOR1, 0);
+ sc_setRefSpeed(MOTOR2, 0);
+ sc_setRefSpeed(MOTOR3, 0);
+
palClearPad(IOPORT3, GPIOC_LED);
- chThdSleepMilliseconds(500);
+ chThdSleepMilliseconds(1000);
+
palSetPad(IOPORT3, GPIOC_LED);
- chThdSleepMilliseconds(500);
+ chThdSleepMilliseconds(1000);
}
return 0;
}
@@ -80,22 +102,20 @@ 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 encoder Interface
+ * Initialise the speed controller
*/
- encodersInit();
+ speedControlInit();
+
+ chThdSleepMilliseconds(2000);
- /*
- * Initialise the motor Interface
- */
- motorsInit();
/*
* Creates the blinker thread.
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
index 2acb14c..586e8dd 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.c
@@ -78,36 +78,6 @@ void getHandler(BaseChannel *chp, int argc, char* argv[]) {
(position)%10);
};
-void resetHandler(BaseChannel *chp, int argc, char* argv[]) {
-
- if (argc != 1) {
- shellPrintLine(chp, "Usage : reset numEncodeur.");
- return;
- }
- switch (argv[0][0]) {
- case '1':
- resetEncoderPosition(ENCODER1);
- shellPrintLine(chp, "Reset encodeur 1.");
- break;
- case '2':
- resetEncoderPosition(ENCODER2);
- shellPrintLine(chp, "Reset encodeur 2.");
- break;
- case '3':
- resetEncoderPosition(ENCODER3);
- shellPrintLine(chp, "Reset encodeur 3.");
- break;
- case 'a':
- resetEncoderPosition(ENCODER1);
- resetEncoderPosition(ENCODER2);
- resetEncoderPosition(ENCODER3);
- shellPrintLine(chp, "Reset de tous les encodeurs.");
- break;
- default:
- shellPrintLine(chp, "Il n'y a que 3 encodeurs !");
- }
-}
-
void setSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
uint8_t motor = 0, i;
@@ -141,7 +111,7 @@ void setSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
}
if (argv[1][0] == '-')
speed = -speed;
- motorSetSpeed(motor, speed);
+ sc_setRefSpeed(motor, speed);
iprintf("set speed : %d%d%d%d%d\r\n", (speed/10000)%10,
(speed/1000)%10,
(speed/100)%10,
@@ -149,42 +119,34 @@ void setSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
(speed)%10);
}
-void motorEnableHandler(BaseChannel *chp, int argc, char* argv[]) {
+void getSpeedHandler(BaseChannel *chp, int argc, char* argv[]) {
- uint8_t motor = 0;
+ int speed;
- if (argc != 2) {
- shellPrintLine(chp, "Usage : motor numMoteur enabled.");
+ if (argc != 1) {
+ shellPrintLine(chp, "Usage : getSpeed numMoteur.");
return;
}
switch (argv[0][0]) {
case '1':
- motor = MOTOR1;
+ speed = sc_getRealSpeed(MOTOR1);
break;
case '2':
- motor = MOTOR2;
+ speed = sc_getRealSpeed(MOTOR2);
break;
case '3':
- motor = MOTOR3;
- break;
- case 'a':
- motor = MOTOR1 | MOTOR2 | MOTOR3;
+ speed = sc_getRealSpeed(MOTOR3);
break;
default:
shellPrintLine(chp, "Mauvais moteur spécifié");
return;
}
- switch (argv[1][0]) {
- case '0':
- disableMotor(motor);
- break;
- case '1':
- enableMotor(motor);
- break;
- default:
- shellPrintLine(chp, "gné ?");
- return;
- }
+
+ iprintf("vitesse : %d%d%d%d%d\r\n", (speed/10000)%10,
+ (speed/1000)%10,
+ (speed/100)%10,
+ (speed/10)%10,
+ (speed)%10);
}
@@ -195,9 +157,8 @@ static const ShellCommand commands[] = {
{"bonjour", bonjourHandler},
{"load", loadHandler},
{"get", getHandler},
- {"reset", resetHandler},
{"setSpeed", setSpeedHandler},
- {"motorEnable", motorEnableHandler},
+ {"getSpeed", getSpeedHandler},
{NULL, NULL}
};
@@ -208,6 +169,6 @@ static const ShellConfig shellConfig = {
void monitorInit(void) {
shellInit();
- shellCreate(&shellConfig, THD_WA_SIZE(512), NORMALPRIO);
- cdtp = chThdCreateFromHeap(NULL, THD_WA_SIZE(512), NORMALPRIO + 1, consoleThread, NULL);
+ shellCreate(&shellConfig, THD_WA_SIZE(256), NORMALPRIO);
+ cdtp = chThdCreateFromHeap(NULL, THD_WA_SIZE(128), NORMALPRIO + 1, consoleThread, NULL);
}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
index 13f273b..7651066 100644
--- a/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/monitor.h
@@ -21,6 +21,7 @@
#include "encoder.h"
#include "motor.h"
#include "cpu_load.h"
+#include "speed_control.h"
extern Thread *cdtp;
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.c b/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.c
new file mode 100644
index 0000000..38d9b26
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.c
@@ -0,0 +1,156 @@
+/*
+ * Speed control
+ * Xavier Lagorce
+ */
+
+#include "speed_control.h"
+
+#define MAX(x,y) ((x) > (y) ? (x) : (y))
+#define MIN(x,y) ((x) > (y) ? (y) : (x))
+
+volatile int ref_speeds[3] = {0, 0, 0};
+volatile int last_errors[3] = {0, 0, 0};
+volatile int last_commands[3] = {0, 0, 0};
+
+volatile int cur_speeds[3] = {0, 0, 0};
+volatile uint16_t prev_positions[3] = {0, 0, 0};
+
+/*
+ * Speed controller thread
+ */
+static msg_t ThreadSpeedController(void *arg) {
+
+ systime_t time;
+ int commands[3] = {0, 0, 0};
+ int errors[3] = {0, 0, 0};
+ uint16_t positions[3] = {0, 0, 0};
+
+ (void)arg;
+ time = chTimeNow();
+
+ while (TRUE) {
+ time += MS2ST(Te);
+
+ positions[0] = getEncoderPosition(ENCODER1);
+ positions[1] = getEncoderPosition(ENCODER2);
+ positions[2] = getEncoderPosition(ENCODER3);
+
+ cur_speeds[0] = (int16_t)(K_v*((int32_t)positions[0] - (int32_t)prev_positions[0]));
+ cur_speeds[1] = (int16_t)(K_v*((int32_t)positions[1] - (int32_t)prev_positions[1]));
+ cur_speeds[2] = (int16_t)(K_v*((int32_t)positions[2] - (int32_t)prev_positions[2]));
+
+ errors[0] = ref_speeds[0] - cur_speeds[0];
+ errors[1] = ref_speeds[1] - cur_speeds[1];
+ errors[2] = ref_speeds[2] - cur_speeds[2];
+
+ //--> Command computation
+ commands[0] = (K_Pn + K_In)*errors[0] - K_Pn*last_errors[0] + last_commands[0];
+ commands[1] = (K_Pn + K_In)*errors[1] - K_Pn*last_errors[1] + last_commands[1];
+ commands[2] = (K_Pn + K_In)*errors[2] - K_Pn*last_errors[2] + last_commands[2];
+ //--> End of command computation
+
+ if (commands[0] >= 0)
+ commands[0] = MIN(MAX_COMMAND, commands[0]);
+ else
+ commands[0] = MAX(-MAX_COMMAND, commands[0]);
+ if (commands[1] >= 0)
+ commands[1] = MIN(MAX_COMMAND, commands[1]);
+ else
+ commands[1] = MAX(-MAX_COMMAND, commands[1]);
+ if (commands[2] >= 0)
+ commands[2] = MIN(MAX_COMMAND, commands[2]);
+ else
+ commands[2] = MAX(-MAX_COMMAND, commands[2]);
+
+ last_commands[0] = commands[0];
+ last_commands[1] = commands[1];
+ last_commands[2] = commands[2];
+
+ if (commands[0] >= -DEAD_ZONE && commands[0] <= DEAD_ZONE)
+ commands[0] = 0;
+ if (commands[1] >= -DEAD_ZONE && commands[1] <= DEAD_ZONE)
+ commands[1] = 0;
+ if (commands[2] >= -DEAD_ZONE && commands[2] <= DEAD_ZONE)
+ commands[2] = 0;
+
+ prev_positions[0] = positions[0];
+ prev_positions[1] = positions[1];
+ prev_positions[2] = positions[2];
+
+ last_errors[0] = errors[0];
+ last_errors[1] = errors[1];
+ last_errors[2] = errors[2];
+
+
+ motorSetSpeed(MOTOR1, commands[0]);
+ motorSetSpeed(MOTOR2, commands[1]);
+ motorSetSpeed(MOTOR3, commands[2]);
+
+ chThdSleepUntil(time);
+ }
+ return 0;
+}
+
+
+void speedControlInit(void) {
+
+ encodersInit();
+ motorsInit();
+
+ enableMotor(MOTOR1 | MOTOR2 | MOTOR3);
+ motorSetSpeed(MOTOR1 | MOTOR2 | MOTOR3, 0);
+ resetEncoderPosition(ENCODER1 | ENCODER2 | ENCODER3);
+
+ prev_positions[0] = getEncoderPosition(ENCODER1);
+ prev_positions[1] = getEncoderPosition(ENCODER2);
+ prev_positions[2] = getEncoderPosition(ENCODER3);
+
+ chThdCreateFromHeap(NULL, 256, HIGHPRIO, ThreadSpeedController, NULL);
+}
+
+void sc_setRefSpeed(uint8_t motor, int speed) {
+
+ if (motor & MOTOR1) {
+ ref_speeds[0] = speed;
+ }
+ if (motor & MOTOR2) {
+ ref_speeds[1] = speed;
+ }
+ if (motor & MOTOR3) {
+ ref_speeds[2] = speed;
+ }
+}
+
+int sc_getRealSpeed(uint8_t motor) {
+
+ switch(motor) {
+ case MOTOR1:
+ return cur_speeds[0];
+ break;
+ case MOTOR2:
+ return cur_speeds[1];
+ break;
+ case MOTOR3:
+ return cur_speeds[2];
+ break;
+ default:
+ return 0;
+ }
+}
+
+uint16_t sc_getPosition(uint8_t motor) {
+
+ switch(motor) {
+ case MOTOR1:
+ return prev_positions[0];
+ break;
+ case MOTOR2:
+ return prev_positions[1];
+ break;
+ case MOTOR3:
+ return prev_positions[2];
+ break;
+ default:
+ return 0;
+ }
+}
diff --git a/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.h b/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.h
new file mode 100644
index 0000000..9a39128
--- /dev/null
+++ b/elec/boards/MotherBoard_KrobotJr2010/Firmware/speed_control.h
@@ -0,0 +1,31 @@
+/*
+ * Speed control
+ * Xavier Lagorce
+ */
+
+#ifndef HEADER__SPEEDCONTROL
+#define HEADER__SPEEDCONTROL
+
+#define K_P 20
+#define K_I 1
+#define Te 50
+
+#define K_v (1000/Te)
+#define K_Pn K_P
+#define K_In 5//(K_I)*Te
+
+#define DEAD_ZONE 100
+#define MAX_COMMAND 35000
+
+#include "ch.h"
+#include "motor.h"
+#include "encoder.h"
+
+extern volatile int ref_speeds[3];
+
+void speedControlInit(void);
+void sc_setRefSpeed(uint8_t motor, int speed);
+int sc_getRealSpeed(uint8_t motor);
+uint16_t sc_getPosition(uint8_t motor);
+
+#endif
hooks/post-receive
--
krobot
|
