Ville Tiukuvaara
Controlling the servo expansion board at https://hackaday.io/project/157951-stm32-servo-expansion-board
Dependencies: SerialTerm Servo WiiNunchuck
Fork of
mbed-os-example-mbed5-blinky
by 
mbed-os-examples
Revision 67:6e183e1e9bcd, committed 2018-05-20
- Comitter:
- villetiukuvaara
- Date:
- Sun May 20 02:56:49 2018 +0000
- Parent:
- 66:e408ae75c632
- Commit message:
- Testing out servo control.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SerialTerm.lib Sun May 20 02:56:49 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Tobias/code/SerialTerm/#07da1704cfd8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Sun May 20 02:56:49 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/villetiukuvaara/code/Servo/#9a48389dcf50
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WiiNunchuck.lib Sun May 20 02:56:49 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/knaka/code/WiiNunchuck/#9652195745e8
--- a/main.cpp Thu May 10 05:15:08 2018 +0100
+++ b/main.cpp Sun May 20 02:56:49 2018 +0000
@@ -1,12 +1,151 @@
#include "mbed.h"
+#include "WiiNunchuck.h"
+#include "Servo.h"
+
+#define LED1 PC_10
+#define LED2 PC_12
+#define LED3 PC_11
+
+#define SERVO1 PA_6
+#define SERVO2 PA_7
+#define SERVO3 PB_11
+#define SERVO4 PB_1
+#define SERVO5 PB_10
+#define SERVO6 PB_3
+
+//#define PWM_PERIOD 20 // ms
+//#define PULSE_MIN 500.0 // us
+//#define PULSE_MAX 2500.0 // us
+
+
+#define SWITCH1 PC_8
+#define SWITCH2 PC_6
+
+#define SDA PC_1
+#define SCL PC_0
+
+#define POW_EN PA_4
+
+WiiNunchuck nchk(SDA, SCL);
+Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalInOut pow_en(POW_EN);
+
+InterruptIn switch1(SWITCH1);
+InterruptIn switch2(SWITCH2);
+
+Servo servo_vert(SERVO1);
+Servo servo_horz(SERVO2);
+Servo servo_rot(SERVO4);
+Servo servo_grab(SERVO5);
+
+int accx, accy, accz;
+unsigned char joyx, joyy;
+bool buttonx, buttonz;
+
+void init();
+void switch1_irq();
+void switch2_irq();
+void enable_servos(bool on);
+void servo_angle(PwmOut *servo, float angle);
+
+bool servos_enabled = false;
+
// main() runs in its own thread in the OS
-int main() {
- while (true) {
- led1 = !led1;
- wait(0.5);
+int main()
+{
+ init();
+
+ while(1)
+ {
+ char key = pc.getc();
+ if(key==0x1B)
+ { //If the keypress was an arrow key
+ key = pc.getc(); //Check again!
+ if (key == 0x5B)
+ {
+ key = pc.getc();
+ switch(key) { //Check to see which arrow key...
+ case 0x41: //It was the UP arrow key...
+ servo_vert = servo_vert + 0.01;
+ pc.printf("vert %f", servo_vert.read());
+ break;
+ case 0x42: //It was the DOWN arrow key...
+ servo_vert = servo_vert - 0.01;
+ pc.printf("vert %f", servo_vert.read());
+ break;
+ case 0x43: //It was the RIGHT arrow key...
+ servo_rot = servo_rot + 0.01;
+ pc.printf("\n\r RIGHT!");
+ break;
+ case 0x44: //It was the LEFT arrow key...
+ servo_rot = servo_rot - 0.01;
+ pc.printf("\n\r LEFT!");
+ break;
+ }
+ }
+ }
+
+ wait(0.01);
}
}
+void init()
+{
+ pow_en.output();
+ pow_en.mode(OpenDrain);
+ enable_servos(false);
+
+ servo_vert.calibrate(0.0007, 90)
+
+ servo_vert = 0.5;
+ servo_horz = 0.5;
+ servo_rot = 0.5;
+ servo_grab = 0.5;
+ /*servo_horz.position(90);
+ servo_rot.position(90);
+ servo_grab.position(90);*/
+
+ switch1.mode(PullUp);
+ switch1.fall(&switch1_irq);
+ switch2.mode(PullUp);
+ switch2.fall(&switch2_irq);
+
+ //led1 = 1;
+}
+
+void switch1_irq()
+{
+ if(servos_enabled)
+ enable_servos(false);
+ else
+ enable_servos(true);
+}
+
+void switch2_irq()
+{
+}
+
+void enable_servos(bool on)
+{
+ if(on)
+ {
+ pow_en.output();
+ pow_en = 0;
+ servos_enabled = true;
+ led2 = 1;
+ }
+ else
+ {
+ pow_en = 1;
+ //pow_en.mode(OpenDrain);
+ pow_en.input();
+ servos_enabled = false;
+ led2 = 0;
+ }
+}
+