Diff: main.cpp

Revision:

0:1ada11b00618

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 22 15:10:35 2017 +0000
@@ -0,0 +1,62 @@
+/***********************************************************************************
+/ Test program to demonstrate the Parallax standard and continuous rotation servos
+/
+/ The program oscillates the standard servo position and oscillates the speed and
+/ direction of the continuous rotation servo
+/
+/ Due to the smaller scale of proportional speed values for the continuous rotation
+/ servo, its resulting command magnitude is reduced by CR_SCALE
+/
+/ Due to a slight directional bias, the contuous rotation servo command is centred
+/ about 0.515, rather than the standard 0.5 value
+/ (on a scale of 0.0-1.0, 0.5 represents 'centred' for standard servos and 'stop'
+/ for continuous rotation servos)
+/***********************************************************************************/
+#include "mbed.h"
+#include "Servo.h"
+
+Serial pc(USBTX, USBRX); // USB serial interface
+Servo cr_srv(p21); // continuous rotation hobby servo
+Servo std_srv(p22); // standard hobby servo
+
+#define CR_SCALE 12.0
+
+void init_servo() {
+// calibrate the servos for +/-5ms over +/-45deg
+cr_srv.calibrate(0.0005,45);
+std_srv.calibrate(0.0005,45);
+}
+
+int main() {
+    
+float pos_val=0.5; // variable to hold position values
+float step_size=0.01; // position increment/decrement value
+float cr_srv_cmd=0.5; // continuous rotation servo command value (range: 0.0 - 1.0)
+float std_srv_cmd=0.5; // standard servo command value (range: 0.0 - 1.0)
+
+// set the USB serial interface baud rate
+pc.baud(921600);
+
+init_servo();
+
+while(1) {
+// use a sin function to create an oscillating servo postion with values
+// between 0.0 and 1.0 for the standard servo
+std_srv_cmd = sin(pos_val*2*3.14159)/2.0+0.5;
+// use a smaller scale for the continuous rotation servo, due to its limited sensitivity
+cr_srv_cmd = sin(pos_val*2*3.14159)/CR_SCALE+0.515;
+
+// sernd the servo command to the servos themselves
+cr_srv.write(cr_srv_cmd); // write to the continuous rotation servo
+std_srv.write(std_srv_cmd); // write to the standard servo
+
+// export the readings to a terminal program via the USB cable
+pc.printf("pos value: %6.4f, cr servo: %6.4f std servo: %6.4f\r", pos_val, cr_srv_cmd,
+std_srv_cmd);
+
+pos_val = pos_val + step_size; // increment/decrement the position
+if (pos_val > 1.0) pos_val = 0.0; // if the position is out of bounds, reset the pos value
+
+wait(0.250); // wait for 0.25s to give time for the display to show
+  }
+}
\ No newline at end of file