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

Files at this revision

API Documentation at this revision

Comitter:
villetiukuvaara
Date:
Sun May 20 02:56:49 2018 +0000
Parent:
66:e408ae75c632
Commit message:
Testing out servo control.

Changed in this revision

SerialTerm.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
WiiNunchuck.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r e408ae75c632 -r 6e183e1e9bcd SerialTerm.lib
--- /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
diff -r e408ae75c632 -r 6e183e1e9bcd Servo.lib
--- /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
diff -r e408ae75c632 -r 6e183e1e9bcd WiiNunchuck.lib
--- /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
diff -r e408ae75c632 -r 6e183e1e9bcd main.cpp
--- 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; 
+    }
+}
+