Psi Swarm Robot
Practical Robotics Modular Robot Library
Revision 5:6da8daaeb9f7, committed 2017-01-02
- Comitter:
- jah128
- Date:
- Mon Jan 02 22:56:34 2017 +0000
- Parent:
- 4:c2e933d53bea
- Child:
- 6:732aa91eb555
- Commit message:
- Add motor pwm serial command;
Changed in this revision
--- a/led.cpp Mon Jan 02 15:17:22 2017 +0000
+++ b/led.cpp Mon Jan 02 22:56:34 2017 +0000
@@ -529,5 +529,20 @@
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
+ case 12:
+ // Alternate red:green
+ if(animation_state == 0) _set_leds( 64, 0, 64, 0, 64, 0, 64, 0, 0, 64, 0, 64, 0, 64, 0, 64);
+ if(animation_state == 1) _set_leds(128, 0,128, 0,128, 0,128, 0, 0,128, 0,128, 0,128, 0,128);
+ if(animation_state == 2) _set_leds(255, 0,255, 0,255, 0,255, 0, 0,255, 0,255, 0,255, 0,255);
+ if(animation_state == 6) _set_leds(128, 0,128, 0,128, 0,128, 0, 0,128, 0,128, 0,128, 0,128);
+ if(animation_state == 7) _set_leds( 64, 0, 64, 0, 64, 0, 64, 0, 0, 64, 0, 64, 0, 64, 0, 64);
+ if(animation_state == 8) _set_leds( 0, 64, 0, 64, 0, 64, 0, 64, 64, 0, 64, 0, 64, 0, 64, 0);
+ if(animation_state == 9) _set_leds( 0,128, 0,128, 0,128, 0,128,128, 0,128, 0,128, 0,128, 0);
+ if(animation_state == 10) _set_leds( 0,255, 0,255, 0,255, 0,255,255, 0,255, 0,255, 0,255, 0);
+ if(animation_state == 14) _set_leds( 0,128, 0,128, 0,128, 0,128,128, 0,128, 0,128, 0,128, 0);
+ if(animation_state == 15) _set_leds( 0, 64, 0, 64, 0, 64, 0, 64, 64, 0, 64, 0, 64, 0, 64, 0);
+ animation_state ++;
+ if(animation_state == 16)animation_state = 0;
+ break;
}
}
\ No newline at end of file
--- a/motors.cpp Mon Jan 02 15:17:22 2017 +0000
+++ b/motors.cpp Mon Jan 02 22:56:34 2017 +0000
@@ -13,6 +13,7 @@
DigitalOut hb2_r(p30);
float left_motor_speed = 0;
float right_motor_speed = 0;
+int motor_pwm_period = 0;
float Motors::get_left_motor_speed()
{
@@ -131,10 +132,16 @@
}
}
+void Motors::set_pwm_period(int period_in){
+ motor_pwm_period = period_in;
+ enable_l.period_us(motor_pwm_period);
+ enable_r.period_us(motor_pwm_period);
+}
void Motors::init(){
- enable_l.period_us(MOTOR_PWM_PERIOD_US);
- enable_r.period_us(MOTOR_PWM_PERIOD_US);
+ if(motor_pwm_period == 0) motor_pwm_period = MOTOR_PWM_PERIOD_US;
+ enable_l.period_us(motor_pwm_period);
+ enable_r.period_us(motor_pwm_period);
enable_l = 0;
enable_r = 0;
wake_up();
--- a/motors.h Mon Jan 02 15:17:22 2017 +0000
+++ b/motors.h Mon Jan 02 22:56:34 2017 +0000
@@ -121,6 +121,11 @@
* @returns The adjusted speed value from 0.0 to 1.0, increased by STALL_OFFSET then scaled to fit
*/
float get_adjusted_speed(float speed_in);
+
+ /**
+ * Sets the PWM period in microseconds (minimum value set to 10 for 100kHZ PWM)
+ */
+ void set_pwm_period(int period_in);
};
--- a/robot.cpp Mon Jan 02 15:17:22 2017 +0000
+++ b/robot.cpp Mon Jan 02 22:56:34 2017 +0000
@@ -98,13 +98,13 @@
//Wait for R-Pi pin to become high
debug("Waiting for Raspberry Pi Zero...\n");
- led.start_animation(9,40);
+ led.start_animation(0,3);
while(rpi1==0) wait_us(50);
led.stop_animation();
led.all_off();
case_led_ticker.detach();
case_led = 1;
- led.start_animation(0,25);
+ led.start_animation(5,25);
debug("Initialisation finished: Starting program. \n");
}
--- a/robot.h Mon Jan 02 15:17:22 2017 +0000 +++ b/robot.h Mon Jan 02 22:56:34 2017 +0000 @@ -44,7 +44,7 @@ // H-Bridge should work at upto 100kHz (10uS) but note it seems to behave unusually at frequencies close to but above this // Slower speeds work a bit faster but noisier -#define MOTOR_PWM_PERIOD_US 400 +#define MOTOR_PWM_PERIOD_US 10 #define USE_STALL_OFFSET 1 #define STALL_OFFSET 0.22
--- a/serialcomms.cpp Mon Jan 02 15:17:22 2017 +0000
+++ b/serialcomms.cpp Mon Jan 02 22:56:34 2017 +0000
@@ -7,6 +7,7 @@
char pc_command_message[3];
char allow_commands = 1;
Timeout pc_command_timeout;
+char acknowledge_messages = 0;
void SerialComms::setup_serial_interfaces()
{
@@ -30,7 +31,6 @@
float dec;
float l_dec;
float r_dec;
- int irp_delay;
char colour_string[7];
char ret_message[50];
char send_message = 0;
@@ -124,32 +124,27 @@
motors.set_right_motor_speed(r_dec);
} else command_status = 2;
break;
-
- // LED COMMANDS
case 10:
- strcpy(command,"SET LED STATES");
- sprintf(subcommand,"G:%s R:%s",_char_to_binary_char(message[1]), _char_to_binary_char(message[2]));
+ strcpy(command,"SET PWM PERIOD");
+ int period = message[1] * 256 + message[2];
+ if(period < 10) period == 10;
+ sprintf(subcommand,"%d uS",period);
if(allow_commands) {
command_status = 1;
- // led.set_leds(message[1],message[2]);
+ motors.set_pwm_period(period);
} else command_status = 2;
break;
- case 11:
- strcpy(command,"SET RED LED STATES");
- sprintf(subcommand,"%s",_char_to_binary_char(message[1]));
+ // LED COMMANDS
+ case 12:
+ strcpy(command,"SET LED ANIMATION");
+ sprintf(subcommand,"M:%d S:%d",message[1], message[2]);
if(allow_commands) {
command_status = 1;
- // led.set_red_leds(message[1]);
+ led.start_animation(message[1],message[2]);
} else command_status = 2;
break;
- case 12:
- strcpy(command,"SET GREEN LED STATES");
- sprintf(subcommand,"%s",_char_to_binary_char(message[1]));
- if(allow_commands) {
- command_status = 1;
- // led.set_green_leds(message[1]);
- } else command_status = 2;
- break;
+
+
case 13:
strcpy(command,"SET LED");
switch(message[2]) {
@@ -305,6 +300,7 @@
robot.debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
}
} else {
+ if(acknowledge_messages){
switch(interface) {
case 0:
pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
@@ -313,6 +309,7 @@
bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
break;
}
+ }
switch(command_status) {
case 0:
robot.debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);