Psi Swarm Robot
/
PsiSwarmV8_CPP
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk
Fork of
PsiSwarmV7_CPP
by 
Psi Swarm Robot
Revision 4:1c621cb8cf0d, committed 2016-08-03
- Comitter:
- jah128
- Date:
- Wed Aug 03 12:25:23 2016 +0000
- Parent:
- 3:7c0d1f581757
- Child:
- 5:3cdd1a37cdd7
- Commit message:
- test;
Changed in this revision
--- a/demo.cpp Tue Mar 15 00:58:09 2016 +0000
+++ b/demo.cpp Wed Aug 03 12:25:23 2016 +0000
@@ -5,8 +5,9 @@
* (C) Dept. Electronics & Computer Science, University of York
* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
*
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
*
+ * Version 0.5 : Added motor calibration menu
* Version 0.4 : Added PsiSwarmBasic menu
* Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
* four directions alone.
@@ -124,7 +125,7 @@
case 1: //Up pressed
switch(level) {
case 0:
- if(top_menu != 6) {
+ if(top_menu != 8) {
level++;
sub_menu = 0;
} else {
@@ -286,7 +287,7 @@
case 2: //Down pressed
switch(level) {
case 0:
- if(top_menu != 6) {
+ if(top_menu != 8) {
level++;
sub_menu = 0;
} else {
@@ -452,10 +453,12 @@
switch(level) {
case 0:
if(top_menu == 0) {
- top_menu = 6;
- if(use_flash_basic == 1) top_menu = 7;
+ top_menu = 8;
}
- else top_menu --;
+ else {
+ if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
+ top_menu --;
+ }
break;
case 1:
switch(top_menu) {
@@ -480,6 +483,10 @@
if(sub_menu == 0) sub_menu = 4;
else sub_menu --;
break;
+ case 6: //Calibrate Menu
+ if(sub_menu == 0) sub_menu = 2;
+ else sub_menu --;
+ break;
case 7: //PsiBasic Menu
if(sub_menu == 0) sub_menu = psi_basic_file_count;
else sub_menu --;
@@ -492,7 +499,7 @@
switch(level) {
case 0:
top_menu ++;
- if((top_menu - use_flash_basic) > 6) top_menu = 0;
+ if((top_menu - use_flash_basic) > 7) top_menu = 0;
break;
case 1:
switch(top_menu) {
@@ -516,6 +523,10 @@
if(sub_menu == 4) sub_menu = 0;
else sub_menu ++;
break;
+ case 6: //Calibrate Menu
+ if(sub_menu == 2) sub_menu = 0;
+ else sub_menu ++;
+ break;
case 7: //PsiBasic Menu
if(sub_menu == psi_basic_file_count) sub_menu = 0;
else sub_menu ++;
@@ -526,7 +537,7 @@
case 16: //Select pressed
switch(level) {
case 0:
- if(top_menu != 6) {
+ if(top_menu != 8) {
level++;
sub_menu = 0;
} else {
@@ -584,6 +595,9 @@
case 5: // Demo Menu
if(sub_menu == 4) level = 0;
break;
+ case 6: // Calibrate Menu
+ if(sub_menu == 2) level = 0;
+ break;
case 7: // PsiBasic Menu
if(sub_menu == psi_basic_file_count) level = 0;
break;
@@ -617,11 +631,14 @@
strcpy(topline,"---CODE DEMOS---");
break;
case 6:
- strcpy(topline,"------EXIT------");
+ strcpy(topline,"-MOTOR CALIBRATE");
break;
case 7:
strcpy(topline,"---PSI BASIC----");
break;
+ case 8:
+ strcpy(topline,"------EXIT------");
+ break;
}
strcpy(bottomline,"");
break;
@@ -834,7 +851,18 @@
}
break;
case 6:
- strcpy(topline,"");
+ strcpy(topline,"-MOTOR CALIBRATE");
+ switch(sub_menu) {
+ case 0:
+ sprintf(bottomline,"RUN A3 TEST");
+ break;
+ case 1:
+ sprintf(bottomline,"ENTER VALUE");
+ break;
+ case 2:
+ sprintf(bottomline,"EXIT");
+ break;
+ }
break;
}
break;
--- a/demo.h Tue Mar 15 00:58:09 2016 +0000 +++ b/demo.h Wed Aug 03 12:25:23 2016 +0000 @@ -5,9 +5,9 @@ * (C) Dept. Electronics & Computer Science, University of York * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis * - * PsiSwarm Library Version: 0.41 + * PsiSwarm Library Version: 0.5 * - * March 2016 + * April 2016 * * */
--- a/eprom.cpp Tue Mar 15 00:58:09 2016 +0000
+++ b/eprom.cpp Wed Aug 03 12:25:23 2016 +0000
@@ -5,9 +5,9 @@
* (C) Dept. Electronics & Computer Science, University of York
* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
*
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
*
- * March 2016
+ * April 2016
*
* Functions for accessing the 64Kb EPROM chip and reading the reserved firmware block
*
@@ -105,6 +105,19 @@
has_temperature_sensor= firmware_bytes[19];
has_recharging_circuit= firmware_bytes[20];
has_433_radio= firmware_bytes[21];
+ if(firmware_version > 1.0){
+ motor_calibration_set = firmware_bytes[22];
+ if(motor_calibration_set == 1){
+ left_motor_calibration_value = (float) firmware_bytes[23] * 65536;
+ left_motor_calibration_value += ((float) firmware_bytes[24] * 256);
+ left_motor_calibration_value += firmware_bytes[25];
+ left_motor_calibration_value /= 16777216;
+ right_motor_calibration_value = (float) firmware_bytes[26] * 65536;
+ right_motor_calibration_value += ((float) firmware_bytes[27] * 256);
+ right_motor_calibration_value += firmware_bytes[28];
+ right_motor_calibration_value /= 16777216;
+ }
+ } else motor_calibration_set = 0;
return 1;
}
return 0;
--- a/motors.cpp Tue Mar 15 00:58:09 2016 +0000
+++ b/motors.cpp Wed Aug 03 12:25:23 2016 +0000
@@ -5,9 +5,9 @@
* (C) Dept. Electronics & Computer Science, University of York
* James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
*
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
*
- * March 2016
+ * April 2016
*
*
*/
@@ -229,11 +229,11 @@
} else {
if(motor_left_speed >= 0) {
motor_left_f.write(0);
- motor_left_r.write(IF_calibrated_speed(motor_left_speed));
+ motor_left_r.write(IF_calibrated_left_speed(motor_left_speed));
} else {
motor_left_r.write(0);
- motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
+ motor_left_f.write(IF_calibrated_left_speed(-motor_left_speed));
}
}
if(motor_right_brake) {
@@ -246,16 +246,36 @@
} else {
if(motor_right_speed >= 0) {
motor_right_f.write(0);
- motor_right_r.write(IF_calibrated_speed(motor_right_speed));
+ motor_right_r.write(IF_calibrated_right_speed(motor_right_speed));
} else {
motor_right_r.write(0);
- motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
+ motor_right_f.write(IF_calibrated_right_speed(-motor_right_speed));
}
}
}
+float IF_calibrated_left_speed(float speed)
+{
+ //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+ float adjusted = IF_calibrated_speed(speed);
+ if(use_motor_calibration){
+ adjusted *= left_motor_calibration_value;
+ }
+ return adjusted;
+}
+
+float IF_calibrated_right_speed(float speed)
+{
+ //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+ float adjusted = IF_calibrated_speed(speed);
+ if(use_motor_calibration){
+ adjusted *= right_motor_calibration_value;
+ }
+ return adjusted;
+}
+
float IF_calibrated_speed(float speed)
{
if(speed == 0) return 0;
--- a/motors.h Tue Mar 15 00:58:09 2016 +0000 +++ b/motors.h Wed Aug 03 12:25:23 2016 +0000 @@ -5,9 +5,9 @@ * (C) Dept. Electronics & Computer Science, University of York * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis * - * PsiSwarm Library Version: 0.41 + * PsiSwarm Library Version: 0.5 * - * March 2016 + * April 2016 * * */ @@ -35,6 +35,8 @@ void IF_end_time_based_action(); void IF_update_motors(); +float IF_calibrated_left_speed(float speed); +float IF_calibrated_right_speed(float speed); float IF_calibrated_speed(float speed); void IF_init_motors(void);
--- a/psiswarm.cpp Tue Mar 15 00:58:09 2016 +0000
+++ b/psiswarm.cpp Wed Aug 03 12:25:23 2016 +0000
@@ -82,6 +82,12 @@
char switch_byte;
char previous_switch_byte;
+
+char use_motor_calibration = USE_MOTOR_CALIBRATION;
+char motor_calibration_set;
+float left_motor_calibration_value = 1.0;
+float right_motor_calibration_value = 1.0;
+
char debug_mode = DEBUG_MODE;
char debug_output = DEBUG_OUTPUT_STREAM;
@@ -153,6 +159,23 @@
if(read_firmware() == 1) {
debug("Version %3.2f\n",firmware_version);
IF_get_hardware_description();
+ if(use_motor_calibration){
+ if(!motor_calibration_set){
+ if(firmware_version < 1.1){
+ debug("- WARNING: This firmware is incompatible with motor calibration");
+ debug("- WARNING: Please update the firmware to use this feature.");
+ use_motor_calibration = 0;
+ }
+ else {
+ debug("- WARNING: Motor calibration values have not been stored in firmware");
+ debug("- WARNING: Run calibration routine to use this feature.");
+ use_motor_calibration = 0;
+ }
+ }
+ else {
+ debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value);
+ }
+ }
} else {
debug("INVALID\n");
debug("- WARNING: Check firmware to enable robot features");
--- a/psiswarm.h Tue Mar 15 00:58:09 2016 +0000 +++ b/psiswarm.h Wed Aug 03 12:25:23 2016 +0000 @@ -5,9 +5,9 @@ * (C) Dept. Electronics & Computer Science, University of York * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis * - * PsiSwarm Library Version: 0.41 + * PsiSwarm Library Version: 0.5 * - * March 2016 + * April 2016 * * */ @@ -15,7 +15,7 @@ #ifndef PSISWARM_H #define PSISWARM_H -#define SOFTWARE_VERSION_CODE 0.41 +#define SOFTWARE_VERSION_CODE 0.50 #define PIC_ADDRESS 0x30 #define LCD_ADDRESS 0x7C @@ -147,6 +147,11 @@ extern float center_led_brightness; extern float backlight_brightness; +extern char use_motor_calibration; +extern char motor_calibration_set; +extern float left_motor_calibration_value; +extern float right_motor_calibration_value; + extern float firmware_version; extern float pcb_version; extern float serial_number;
--- a/settings.h Tue Mar 15 00:58:09 2016 +0000 +++ b/settings.h Wed Aug 03 12:25:23 2016 +0000 @@ -5,13 +5,18 @@ * (C) Dept. Electronics & Computer Science, University of York * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis * - * PsiSwarm Library Version: 0.41 + * PsiSwarm Library Version: 0.5 * - * March 2016 + * April 2016 * * */ +/* USE_MOTOR_CALIBRATION [1=on, recommended 0=off] + * If enabled, the actual motor speeds will be adjusted from the requested values using the motor calibration values stored in + * firmware (and set using the motor calibration code in the demo mode) + */ +#define USE_MOTOR_CALIBRATION 1 /* OFFSET_MOTORS [1=on, recommended 0=off] * The motors typically stall when the PWM output is below around 0.2