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
main.cpp
- Committer:
- villetiukuvaara
- Date:
- 2018-05-20
- Revision:
- 67:6e183e1e9bcd
- Parent:
- 29:0b58d21e87d6
File content as of revision 67:6e183e1e9bcd:
#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() { 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; } }