NervousPuppySprintTwo
Dependencies: C12832_lcd Servo USBHost mbed
Fork of USBHostSerial_HelloWorld by
main.cpp@8:eaae889bc6eb, 2015-03-03 (annotated)
- Committer:
- davidqpinho
- Date:
- Tue Mar 03 19:32:09 2015 +0000
- Revision:
- 8:eaae889bc6eb
- Parent:
- 7:3b14ddcf6d8e
- Child:
- 9:ea3cfd16b4d9
servos and threads with mutex working along with rs232
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
samux | 0:7528379ed84d | 1 | #include "mbed.h" |
samux | 0:7528379ed84d | 2 | #include "USBHostSerial.h" |
hervel90 | 7:3b14ddcf6d8e | 3 | #include "C12832_lcd.h" |
hervel90 | 7:3b14ddcf6d8e | 4 | #include "Servo.h" |
samux | 0:7528379ed84d | 5 | |
davidqpinho | 8:eaae889bc6eb | 6 | #define RCOUNTERTIME if(posx<=1500) posx+=1; |
davidqpinho | 8:eaae889bc6eb | 7 | #define RTIME if(posx>=500) posx-=1; |
davidqpinho | 8:eaae889bc6eb | 8 | #define TILTBACK if(posy>=500) posy-=1; |
davidqpinho | 8:eaae889bc6eb | 9 | #define TILTFORW if(posy<=1000) posy+=1; |
davidqpinho | 8:eaae889bc6eb | 10 | #define TEMPORARY rotate.SetPosition(posx); tilt.SetPosition(posy); |
davidqpinho | 8:eaae889bc6eb | 11 | |
hervel90 | 7:3b14ddcf6d8e | 12 | DigitalOut led1(LED1); |
hervel90 | 7:3b14ddcf6d8e | 13 | DigitalOut led3(LED3); |
samux | 0:7528379ed84d | 14 | Serial pc(USBTX, USBRX); |
hervel90 | 7:3b14ddcf6d8e | 15 | C12832_LCD lcd; |
hervel90 | 7:3b14ddcf6d8e | 16 | |
hervel90 | 7:3b14ddcf6d8e | 17 | Servo tilt(p21); |
hervel90 | 7:3b14ddcf6d8e | 18 | Servo rotate(p22); |
hervel90 | 7:3b14ddcf6d8e | 19 | AnalogIn ainLeft(p15); |
hervel90 | 7:3b14ddcf6d8e | 20 | AnalogIn ainRight(p16); |
hervel90 | 7:3b14ddcf6d8e | 21 | |
hervel90 | 7:3b14ddcf6d8e | 22 | Mutex rx_mutex; |
davidqpinho | 8:eaae889bc6eb | 23 | Mutex tiltserv; |
davidqpinho | 8:eaae889bc6eb | 24 | Mutex rotateserv; |
hervel90 | 7:3b14ddcf6d8e | 25 | |
davidqpinho | 8:eaae889bc6eb | 26 | int posx = 1000,posy = 750; |
davidqpinho | 8:eaae889bc6eb | 27 | |
davidqpinho | 8:eaae889bc6eb | 28 | void rotate_servo(int x); |
davidqpinho | 8:eaae889bc6eb | 29 | |
davidqpinho | 8:eaae889bc6eb | 30 | void tild_thread(void const *args) |
davidqpinho | 8:eaae889bc6eb | 31 | { |
davidqpinho | 8:eaae889bc6eb | 32 | while (true) { |
davidqpinho | 8:eaae889bc6eb | 33 | tiltserv.lock(); |
davidqpinho | 8:eaae889bc6eb | 34 | tilt.SetPosition(posy); |
davidqpinho | 8:eaae889bc6eb | 35 | tiltserv.unlock(); |
davidqpinho | 8:eaae889bc6eb | 36 | } |
davidqpinho | 8:eaae889bc6eb | 37 | } |
davidqpinho | 8:eaae889bc6eb | 38 | |
davidqpinho | 8:eaae889bc6eb | 39 | void rotate_thread(void const *args) |
davidqpinho | 8:eaae889bc6eb | 40 | { |
davidqpinho | 8:eaae889bc6eb | 41 | while (true) { |
davidqpinho | 8:eaae889bc6eb | 42 | rotateserv.lock(); |
davidqpinho | 8:eaae889bc6eb | 43 | rotate.SetPosition(posx); |
davidqpinho | 8:eaae889bc6eb | 44 | rotateserv.unlock(); |
davidqpinho | 8:eaae889bc6eb | 45 | } |
davidqpinho | 8:eaae889bc6eb | 46 | } |
samux | 0:7528379ed84d | 47 | |
hervel90 | 7:3b14ddcf6d8e | 48 | int main() |
hervel90 | 7:3b14ddcf6d8e | 49 | { |
davidqpinho | 8:eaae889bc6eb | 50 | tilt.Enable(750,20000); |
davidqpinho | 8:eaae889bc6eb | 51 | rotate.Enable(1000,20000); |
davidqpinho | 8:eaae889bc6eb | 52 | Thread thr1(tild_thread); |
davidqpinho | 8:eaae889bc6eb | 53 | Thread thr2(rotate_thread); |
hervel90 | 7:3b14ddcf6d8e | 54 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 55 | char buffer[128]; |
davidqpinho | 8:eaae889bc6eb | 56 | rotate.SetPosition(1000); |
davidqpinho | 8:eaae889bc6eb | 57 | tilt.SetPosition(750); |
davidqpinho | 8:eaae889bc6eb | 58 | |
hervel90 | 7:3b14ddcf6d8e | 59 | while (true) |
hervel90 | 7:3b14ddcf6d8e | 60 | { |
hervel90 | 7:3b14ddcf6d8e | 61 | if(pc.readable()) |
hervel90 | 7:3b14ddcf6d8e | 62 | { |
hervel90 | 7:3b14ddcf6d8e | 63 | rx_mutex.lock(); |
hervel90 | 7:3b14ddcf6d8e | 64 | pc.gets(buffer,2); // set the number of bytes |
hervel90 | 7:3b14ddcf6d8e | 65 | |
hervel90 | 7:3b14ddcf6d8e | 66 | rx_mutex.unlock(); |
hervel90 | 7:3b14ddcf6d8e | 67 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 68 | lcd.printf(buffer); |
hervel90 | 7:3b14ddcf6d8e | 69 | } |
hervel90 | 7:3b14ddcf6d8e | 70 | int x=atoi(buffer); |
davidqpinho | 8:eaae889bc6eb | 71 | |
davidqpinho | 8:eaae889bc6eb | 72 | rotate_servo(x); |
davidqpinho | 8:eaae889bc6eb | 73 | } |
davidqpinho | 8:eaae889bc6eb | 74 | } |
davidqpinho | 8:eaae889bc6eb | 75 | |
davidqpinho | 8:eaae889bc6eb | 76 | |
davidqpinho | 8:eaae889bc6eb | 77 | |
davidqpinho | 8:eaae889bc6eb | 78 | void rotate_servo(int x){ |
davidqpinho | 8:eaae889bc6eb | 79 | switch (x) |
hervel90 | 7:3b14ddcf6d8e | 80 | { |
hervel90 | 7:3b14ddcf6d8e | 81 | case 0: |
hervel90 | 7:3b14ddcf6d8e | 82 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 83 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 84 | lcd.printf("Do nothing"); |
hervel90 | 7:3b14ddcf6d8e | 85 | |
hervel90 | 7:3b14ddcf6d8e | 86 | break; |
hervel90 | 7:3b14ddcf6d8e | 87 | |
hervel90 | 7:3b14ddcf6d8e | 88 | case 1: |
hervel90 | 7:3b14ddcf6d8e | 89 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 90 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 91 | lcd.printf("Right"); |
davidqpinho | 8:eaae889bc6eb | 92 | RCOUNTERTIME; |
davidqpinho | 8:eaae889bc6eb | 93 | |
hervel90 | 7:3b14ddcf6d8e | 94 | break; |
hervel90 | 7:3b14ddcf6d8e | 95 | |
hervel90 | 7:3b14ddcf6d8e | 96 | case 2: |
hervel90 | 7:3b14ddcf6d8e | 97 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 98 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 99 | lcd.printf("down & right"); |
davidqpinho | 8:eaae889bc6eb | 100 | RCOUNTERTIME; |
davidqpinho | 8:eaae889bc6eb | 101 | TILTFORW; |
davidqpinho | 8:eaae889bc6eb | 102 | |
hervel90 | 7:3b14ddcf6d8e | 103 | break; |
hervel90 | 7:3b14ddcf6d8e | 104 | |
hervel90 | 7:3b14ddcf6d8e | 105 | case 3: |
hervel90 | 7:3b14ddcf6d8e | 106 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 107 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 108 | lcd.printf("down"); |
davidqpinho | 8:eaae889bc6eb | 109 | TILTFORW; |
davidqpinho | 8:eaae889bc6eb | 110 | |
hervel90 | 7:3b14ddcf6d8e | 111 | break; |
hervel90 | 7:3b14ddcf6d8e | 112 | |
hervel90 | 7:3b14ddcf6d8e | 113 | case 4: |
hervel90 | 7:3b14ddcf6d8e | 114 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 115 | lcd.locate(0,1); |
davidqpinho | 8:eaae889bc6eb | 116 | lcd.printf("Down & left"); |
davidqpinho | 8:eaae889bc6eb | 117 | RTIME; |
davidqpinho | 8:eaae889bc6eb | 118 | TILTFORW; |
davidqpinho | 8:eaae889bc6eb | 119 | |
hervel90 | 7:3b14ddcf6d8e | 120 | break; |
hervel90 | 7:3b14ddcf6d8e | 121 | |
hervel90 | 7:3b14ddcf6d8e | 122 | case 5: |
hervel90 | 7:3b14ddcf6d8e | 123 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 124 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 125 | lcd.printf("left"); |
davidqpinho | 8:eaae889bc6eb | 126 | RTIME; |
davidqpinho | 8:eaae889bc6eb | 127 | |
davidqpinho | 8:eaae889bc6eb | 128 | |
davidqpinho | 8:eaae889bc6eb | 129 | |
hervel90 | 7:3b14ddcf6d8e | 130 | break; |
samux | 0:7528379ed84d | 131 | |
hervel90 | 7:3b14ddcf6d8e | 132 | case 6: |
hervel90 | 7:3b14ddcf6d8e | 133 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 134 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 135 | lcd.printf("up & left"); |
davidqpinho | 8:eaae889bc6eb | 136 | RTIME; |
davidqpinho | 8:eaae889bc6eb | 137 | TILTBACK; |
davidqpinho | 8:eaae889bc6eb | 138 | |
davidqpinho | 8:eaae889bc6eb | 139 | |
davidqpinho | 8:eaae889bc6eb | 140 | |
hervel90 | 7:3b14ddcf6d8e | 141 | break; |
hervel90 | 7:3b14ddcf6d8e | 142 | |
hervel90 | 7:3b14ddcf6d8e | 143 | case 7: |
hervel90 | 7:3b14ddcf6d8e | 144 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 145 | lcd.locate(0,1); |
hervel90 | 7:3b14ddcf6d8e | 146 | lcd.printf("up"); |
davidqpinho | 8:eaae889bc6eb | 147 | TILTBACK; |
davidqpinho | 8:eaae889bc6eb | 148 | |
davidqpinho | 8:eaae889bc6eb | 149 | |
davidqpinho | 8:eaae889bc6eb | 150 | |
hervel90 | 7:3b14ddcf6d8e | 151 | break; |
hervel90 | 7:3b14ddcf6d8e | 152 | |
hervel90 | 7:3b14ddcf6d8e | 153 | case 8: |
hervel90 | 7:3b14ddcf6d8e | 154 | lcd.cls(); |
hervel90 | 7:3b14ddcf6d8e | 155 | lcd.locate(0,1); |
davidqpinho | 8:eaae889bc6eb | 156 | lcd.printf("up & right"); |
davidqpinho | 8:eaae889bc6eb | 157 | TILTBACK; |
davidqpinho | 8:eaae889bc6eb | 158 | RCOUNTERTIME; |
davidqpinho | 8:eaae889bc6eb | 159 | |
davidqpinho | 8:eaae889bc6eb | 160 | |
hervel90 | 7:3b14ddcf6d8e | 161 | break; |
davidqpinho | 8:eaae889bc6eb | 162 | |
samux | 0:7528379ed84d | 163 | } |
davidqpinho | 8:eaae889bc6eb | 164 | |
davidqpinho | 8:eaae889bc6eb | 165 | } |
davidqpinho | 8:eaae889bc6eb | 166 | |
davidqpinho | 8:eaae889bc6eb | 167 |