![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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@67:6e183e1e9bcd, 2018-05-20 (annotated)
- Committer:
- villetiukuvaara
- Date:
- Sun May 20 02:56:49 2018 +0000
- Revision:
- 67:6e183e1e9bcd
- Parent:
- 29:0b58d21e87d6
Testing out servo control.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jonathan Austin |
0:2757d7abb7d9 | 1 | #include "mbed.h" |
villetiukuvaara | 67:6e183e1e9bcd | 2 | #include "WiiNunchuck.h" |
villetiukuvaara | 67:6e183e1e9bcd | 3 | #include "Servo.h" |
villetiukuvaara | 67:6e183e1e9bcd | 4 | |
villetiukuvaara | 67:6e183e1e9bcd | 5 | #define LED1 PC_10 |
villetiukuvaara | 67:6e183e1e9bcd | 6 | #define LED2 PC_12 |
villetiukuvaara | 67:6e183e1e9bcd | 7 | #define LED3 PC_11 |
villetiukuvaara | 67:6e183e1e9bcd | 8 | |
villetiukuvaara | 67:6e183e1e9bcd | 9 | #define SERVO1 PA_6 |
villetiukuvaara | 67:6e183e1e9bcd | 10 | #define SERVO2 PA_7 |
villetiukuvaara | 67:6e183e1e9bcd | 11 | #define SERVO3 PB_11 |
villetiukuvaara | 67:6e183e1e9bcd | 12 | #define SERVO4 PB_1 |
villetiukuvaara | 67:6e183e1e9bcd | 13 | #define SERVO5 PB_10 |
villetiukuvaara | 67:6e183e1e9bcd | 14 | #define SERVO6 PB_3 |
villetiukuvaara | 67:6e183e1e9bcd | 15 | |
villetiukuvaara | 67:6e183e1e9bcd | 16 | //#define PWM_PERIOD 20 // ms |
villetiukuvaara | 67:6e183e1e9bcd | 17 | //#define PULSE_MIN 500.0 // us |
villetiukuvaara | 67:6e183e1e9bcd | 18 | //#define PULSE_MAX 2500.0 // us |
villetiukuvaara | 67:6e183e1e9bcd | 19 | |
villetiukuvaara | 67:6e183e1e9bcd | 20 | |
villetiukuvaara | 67:6e183e1e9bcd | 21 | #define SWITCH1 PC_8 |
villetiukuvaara | 67:6e183e1e9bcd | 22 | #define SWITCH2 PC_6 |
villetiukuvaara | 67:6e183e1e9bcd | 23 | |
villetiukuvaara | 67:6e183e1e9bcd | 24 | #define SDA PC_1 |
villetiukuvaara | 67:6e183e1e9bcd | 25 | #define SCL PC_0 |
villetiukuvaara | 67:6e183e1e9bcd | 26 | |
villetiukuvaara | 67:6e183e1e9bcd | 27 | #define POW_EN PA_4 |
villetiukuvaara | 67:6e183e1e9bcd | 28 | |
villetiukuvaara | 67:6e183e1e9bcd | 29 | WiiNunchuck nchk(SDA, SCL); |
villetiukuvaara | 67:6e183e1e9bcd | 30 | Serial pc(USBTX, USBRX); |
Jonathan Austin |
0:2757d7abb7d9 | 31 | |
Jonathan Austin |
0:2757d7abb7d9 | 32 | DigitalOut led1(LED1); |
villetiukuvaara | 67:6e183e1e9bcd | 33 | DigitalOut led2(LED2); |
villetiukuvaara | 67:6e183e1e9bcd | 34 | DigitalOut led3(LED3); |
villetiukuvaara | 67:6e183e1e9bcd | 35 | DigitalInOut pow_en(POW_EN); |
villetiukuvaara | 67:6e183e1e9bcd | 36 | |
villetiukuvaara | 67:6e183e1e9bcd | 37 | InterruptIn switch1(SWITCH1); |
villetiukuvaara | 67:6e183e1e9bcd | 38 | InterruptIn switch2(SWITCH2); |
villetiukuvaara | 67:6e183e1e9bcd | 39 | |
villetiukuvaara | 67:6e183e1e9bcd | 40 | Servo servo_vert(SERVO1); |
villetiukuvaara | 67:6e183e1e9bcd | 41 | Servo servo_horz(SERVO2); |
villetiukuvaara | 67:6e183e1e9bcd | 42 | Servo servo_rot(SERVO4); |
villetiukuvaara | 67:6e183e1e9bcd | 43 | Servo servo_grab(SERVO5); |
villetiukuvaara | 67:6e183e1e9bcd | 44 | |
villetiukuvaara | 67:6e183e1e9bcd | 45 | int accx, accy, accz; |
villetiukuvaara | 67:6e183e1e9bcd | 46 | unsigned char joyx, joyy; |
villetiukuvaara | 67:6e183e1e9bcd | 47 | bool buttonx, buttonz; |
villetiukuvaara | 67:6e183e1e9bcd | 48 | |
villetiukuvaara | 67:6e183e1e9bcd | 49 | void init(); |
villetiukuvaara | 67:6e183e1e9bcd | 50 | void switch1_irq(); |
villetiukuvaara | 67:6e183e1e9bcd | 51 | void switch2_irq(); |
villetiukuvaara | 67:6e183e1e9bcd | 52 | void enable_servos(bool on); |
villetiukuvaara | 67:6e183e1e9bcd | 53 | void servo_angle(PwmOut *servo, float angle); |
villetiukuvaara | 67:6e183e1e9bcd | 54 | |
villetiukuvaara | 67:6e183e1e9bcd | 55 | bool servos_enabled = false; |
villetiukuvaara | 67:6e183e1e9bcd | 56 | |
Jonathan Austin |
0:2757d7abb7d9 | 57 | |
Jonathan Austin |
1:846c97078558 | 58 | // main() runs in its own thread in the OS |
villetiukuvaara | 67:6e183e1e9bcd | 59 | int main() |
villetiukuvaara | 67:6e183e1e9bcd | 60 | { |
villetiukuvaara | 67:6e183e1e9bcd | 61 | init(); |
villetiukuvaara | 67:6e183e1e9bcd | 62 | |
villetiukuvaara | 67:6e183e1e9bcd | 63 | while(1) |
villetiukuvaara | 67:6e183e1e9bcd | 64 | { |
villetiukuvaara | 67:6e183e1e9bcd | 65 | char key = pc.getc(); |
villetiukuvaara | 67:6e183e1e9bcd | 66 | if(key==0x1B) |
villetiukuvaara | 67:6e183e1e9bcd | 67 | { //If the keypress was an arrow key |
villetiukuvaara | 67:6e183e1e9bcd | 68 | key = pc.getc(); //Check again! |
villetiukuvaara | 67:6e183e1e9bcd | 69 | if (key == 0x5B) |
villetiukuvaara | 67:6e183e1e9bcd | 70 | { |
villetiukuvaara | 67:6e183e1e9bcd | 71 | key = pc.getc(); |
villetiukuvaara | 67:6e183e1e9bcd | 72 | switch(key) { //Check to see which arrow key... |
villetiukuvaara | 67:6e183e1e9bcd | 73 | case 0x41: //It was the UP arrow key... |
villetiukuvaara | 67:6e183e1e9bcd | 74 | servo_vert = servo_vert + 0.01; |
villetiukuvaara | 67:6e183e1e9bcd | 75 | pc.printf("vert %f", servo_vert.read()); |
villetiukuvaara | 67:6e183e1e9bcd | 76 | break; |
villetiukuvaara | 67:6e183e1e9bcd | 77 | case 0x42: //It was the DOWN arrow key... |
villetiukuvaara | 67:6e183e1e9bcd | 78 | servo_vert = servo_vert - 0.01; |
villetiukuvaara | 67:6e183e1e9bcd | 79 | pc.printf("vert %f", servo_vert.read()); |
villetiukuvaara | 67:6e183e1e9bcd | 80 | break; |
villetiukuvaara | 67:6e183e1e9bcd | 81 | case 0x43: //It was the RIGHT arrow key... |
villetiukuvaara | 67:6e183e1e9bcd | 82 | servo_rot = servo_rot + 0.01; |
villetiukuvaara | 67:6e183e1e9bcd | 83 | pc.printf("\n\r RIGHT!"); |
villetiukuvaara | 67:6e183e1e9bcd | 84 | break; |
villetiukuvaara | 67:6e183e1e9bcd | 85 | case 0x44: //It was the LEFT arrow key... |
villetiukuvaara | 67:6e183e1e9bcd | 86 | servo_rot = servo_rot - 0.01; |
villetiukuvaara | 67:6e183e1e9bcd | 87 | pc.printf("\n\r LEFT!"); |
villetiukuvaara | 67:6e183e1e9bcd | 88 | break; |
villetiukuvaara | 67:6e183e1e9bcd | 89 | } |
villetiukuvaara | 67:6e183e1e9bcd | 90 | } |
villetiukuvaara | 67:6e183e1e9bcd | 91 | } |
villetiukuvaara | 67:6e183e1e9bcd | 92 | |
villetiukuvaara | 67:6e183e1e9bcd | 93 | wait(0.01); |
Jonathan Austin |
0:2757d7abb7d9 | 94 | } |
Jonathan Austin |
0:2757d7abb7d9 | 95 | } |
Jonathan Austin |
1:846c97078558 | 96 | |
villetiukuvaara | 67:6e183e1e9bcd | 97 | void init() |
villetiukuvaara | 67:6e183e1e9bcd | 98 | { |
villetiukuvaara | 67:6e183e1e9bcd | 99 | pow_en.output(); |
villetiukuvaara | 67:6e183e1e9bcd | 100 | pow_en.mode(OpenDrain); |
villetiukuvaara | 67:6e183e1e9bcd | 101 | enable_servos(false); |
villetiukuvaara | 67:6e183e1e9bcd | 102 | |
villetiukuvaara | 67:6e183e1e9bcd | 103 | servo_vert.calibrate(0.0007, 90) |
villetiukuvaara | 67:6e183e1e9bcd | 104 | |
villetiukuvaara | 67:6e183e1e9bcd | 105 | servo_vert = 0.5; |
villetiukuvaara | 67:6e183e1e9bcd | 106 | servo_horz = 0.5; |
villetiukuvaara | 67:6e183e1e9bcd | 107 | servo_rot = 0.5; |
villetiukuvaara | 67:6e183e1e9bcd | 108 | servo_grab = 0.5; |
villetiukuvaara | 67:6e183e1e9bcd | 109 | /*servo_horz.position(90); |
villetiukuvaara | 67:6e183e1e9bcd | 110 | servo_rot.position(90); |
villetiukuvaara | 67:6e183e1e9bcd | 111 | servo_grab.position(90);*/ |
villetiukuvaara | 67:6e183e1e9bcd | 112 | |
villetiukuvaara | 67:6e183e1e9bcd | 113 | switch1.mode(PullUp); |
villetiukuvaara | 67:6e183e1e9bcd | 114 | switch1.fall(&switch1_irq); |
villetiukuvaara | 67:6e183e1e9bcd | 115 | switch2.mode(PullUp); |
villetiukuvaara | 67:6e183e1e9bcd | 116 | switch2.fall(&switch2_irq); |
villetiukuvaara | 67:6e183e1e9bcd | 117 | |
villetiukuvaara | 67:6e183e1e9bcd | 118 | //led1 = 1; |
villetiukuvaara | 67:6e183e1e9bcd | 119 | } |
villetiukuvaara | 67:6e183e1e9bcd | 120 | |
villetiukuvaara | 67:6e183e1e9bcd | 121 | void switch1_irq() |
villetiukuvaara | 67:6e183e1e9bcd | 122 | { |
villetiukuvaara | 67:6e183e1e9bcd | 123 | if(servos_enabled) |
villetiukuvaara | 67:6e183e1e9bcd | 124 | enable_servos(false); |
villetiukuvaara | 67:6e183e1e9bcd | 125 | else |
villetiukuvaara | 67:6e183e1e9bcd | 126 | enable_servos(true); |
villetiukuvaara | 67:6e183e1e9bcd | 127 | } |
villetiukuvaara | 67:6e183e1e9bcd | 128 | |
villetiukuvaara | 67:6e183e1e9bcd | 129 | void switch2_irq() |
villetiukuvaara | 67:6e183e1e9bcd | 130 | { |
villetiukuvaara | 67:6e183e1e9bcd | 131 | } |
villetiukuvaara | 67:6e183e1e9bcd | 132 | |
villetiukuvaara | 67:6e183e1e9bcd | 133 | void enable_servos(bool on) |
villetiukuvaara | 67:6e183e1e9bcd | 134 | { |
villetiukuvaara | 67:6e183e1e9bcd | 135 | if(on) |
villetiukuvaara | 67:6e183e1e9bcd | 136 | { |
villetiukuvaara | 67:6e183e1e9bcd | 137 | pow_en.output(); |
villetiukuvaara | 67:6e183e1e9bcd | 138 | pow_en = 0; |
villetiukuvaara | 67:6e183e1e9bcd | 139 | servos_enabled = true; |
villetiukuvaara | 67:6e183e1e9bcd | 140 | led2 = 1; |
villetiukuvaara | 67:6e183e1e9bcd | 141 | } |
villetiukuvaara | 67:6e183e1e9bcd | 142 | else |
villetiukuvaara | 67:6e183e1e9bcd | 143 | { |
villetiukuvaara | 67:6e183e1e9bcd | 144 | pow_en = 1; |
villetiukuvaara | 67:6e183e1e9bcd | 145 | //pow_en.mode(OpenDrain); |
villetiukuvaara | 67:6e183e1e9bcd | 146 | pow_en.input(); |
villetiukuvaara | 67:6e183e1e9bcd | 147 | servos_enabled = false; |
villetiukuvaara | 67:6e183e1e9bcd | 148 | led2 = 0; |
villetiukuvaara | 67:6e183e1e9bcd | 149 | } |
villetiukuvaara | 67:6e183e1e9bcd | 150 | } |
villetiukuvaara | 67:6e183e1e9bcd | 151 |