Donal / Mbed 2 deprecated Lamp_03

Demo for lamp

Dependencies:   C12832_lcd PixelArray WS2812 mbed-rtos mbed

main.cpp@0:2cf2aa8c0d7c, 2015-04-23 (annotated)

Committer:
cathal66
Date:
Thu Apr 23 15:56:42 2015 +0000
Revision:
0:2cf2aa8c0d7c
Demo for lamp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cathal66 0:2cf2aa8c0d7c 1 #include "mbed.h"
cathal66 0:2cf2aa8c0d7c 2 #include "rtos.h"
cathal66 0:2cf2aa8c0d7c 3 #include "WS2812.h"
cathal66 0:2cf2aa8c0d7c 4 #include "PixelArray.h"
cathal66 0:2cf2aa8c0d7c 5
cathal66 0:2cf2aa8c0d7c 6 #define WS2812_BUF 10
cathal66 0:2cf2aa8c0d7c 7 #define NUM_COLORS 1
cathal66 0:2cf2aa8c0d7c 8 #define NUM_LEDS_PER_COLOR 10
cathal66 0:2cf2aa8c0d7c 9 PixelArray px(WS2812_BUF);
cathal66 0:2cf2aa8c0d7c 10
cathal66 0:2cf2aa8c0d7c 11 // See the program page for information on the timing numbers
cathal66 0:2cf2aa8c0d7c 12 // The given numbers are for the K64F
cathal66 0:2cf2aa8c0d7c 13 WS2812 ws(p9, WS2812_BUF, 5, 10, 10, 15);
cathal66 0:2cf2aa8c0d7c 14
cathal66 0:2cf2aa8c0d7c 15 PwmOut Servo_Y(p22);
cathal66 0:2cf2aa8c0d7c 16 PwmOut Servo_X(p21);
cathal66 0:2cf2aa8c0d7c 17 PwmOut Servo_Z(p23);
cathal66 0:2cf2aa8c0d7c 18
cathal66 0:2cf2aa8c0d7c 19 Serial pc(USBTX, USBRX); // tx, rx
cathal66 0:2cf2aa8c0d7c 20
cathal66 0:2cf2aa8c0d7c 21
cathal66 0:2cf2aa8c0d7c 22 float Servo_z_smooth;
cathal66 0:2cf2aa8c0d7c 23
cathal66 0:2cf2aa8c0d7c 24 void Servo_Z_Thread(void const *args) {
cathal66 0:2cf2aa8c0d7c 25 float Sevro_Pos;
cathal66 0:2cf2aa8c0d7c 26 float Sevro_Pos_New = 0.1;
cathal66 0:2cf2aa8c0d7c 27 float Sevro_Pos_Old = 0.01;
cathal66 0:2cf2aa8c0d7c 28
cathal66 0:2cf2aa8c0d7c 29 while (true) {
cathal66 0:2cf2aa8c0d7c 30
cathal66 0:2cf2aa8c0d7c 31 Sevro_Pos_New = Servo_z_smooth;
cathal66 0:2cf2aa8c0d7c 32
cathal66 0:2cf2aa8c0d7c 33 if(Sevro_Pos_Old <= Sevro_Pos_New)
cathal66 0:2cf2aa8c0d7c 34 {
cathal66 0:2cf2aa8c0d7c 35 for(float i=Sevro_Pos_Old*10000; i<=Sevro_Pos_New*10000; i=i+25)
cathal66 0:2cf2aa8c0d7c 36 {
cathal66 0:2cf2aa8c0d7c 37 Sevro_Pos = i/10000;
cathal66 0:2cf2aa8c0d7c 38 Servo_Z = Sevro_Pos;
cathal66 0:2cf2aa8c0d7c 39 pc.printf("Servo_Z1: %1.6f %1.6f %1.6f\n\r ",Sevro_Pos, i,Sevro_Pos_Old);
cathal66 0:2cf2aa8c0d7c 40 //Thread::wait(50);
cathal66 0:2cf2aa8c0d7c 41 }
cathal66 0:2cf2aa8c0d7c 42 }
cathal66 0:2cf2aa8c0d7c 43 else
cathal66 0:2cf2aa8c0d7c 44 {
cathal66 0:2cf2aa8c0d7c 45 for(float i=Sevro_Pos_Old*10000; i>=Sevro_Pos_New*10000; i=i-25)
cathal66 0:2cf2aa8c0d7c 46 {
cathal66 0:2cf2aa8c0d7c 47 Sevro_Pos = i/10000;
cathal66 0:2cf2aa8c0d7c 48 Servo_Z = Sevro_Pos;
cathal66 0:2cf2aa8c0d7c 49 pc.printf("Servo_Z2: %1.6f \n\r",Sevro_Pos);
cathal66 0:2cf2aa8c0d7c 50 //Thread::wait(50);
cathal66 0:2cf2aa8c0d7c 51 }
cathal66 0:2cf2aa8c0d7c 52 }
cathal66 0:2cf2aa8c0d7c 53 Sevro_Pos_Old = Sevro_Pos_New;
cathal66 0:2cf2aa8c0d7c 54 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 55 }
cathal66 0:2cf2aa8c0d7c 56 }
cathal66 0:2cf2aa8c0d7c 57
cathal66 0:2cf2aa8c0d7c 58
cathal66 0:2cf2aa8c0d7c 59
cathal66 0:2cf2aa8c0d7c 60 void LED_All_Colour(int red , int green ,int blue , int bright)
cathal66 0:2cf2aa8c0d7c 61 {
cathal66 0:2cf2aa8c0d7c 62 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
cathal66 0:2cf2aa8c0d7c 63
cathal66 0:2cf2aa8c0d7c 64 int colorbuf[NUM_COLORS] = {0x000000};
cathal66 0:2cf2aa8c0d7c 65 //int colorbuf[NUM_COLORS] = {0x000000,0x00f0ff,0x00ff00,0x00ffff,0xffffff,0x00ff00,0x00ffff,0x0000ff,0xff00ff};
cathal66 0:2cf2aa8c0d7c 66
cathal66 0:2cf2aa8c0d7c 67 red <<= 16;
cathal66 0:2cf2aa8c0d7c 68 green <<= 8;
cathal66 0:2cf2aa8c0d7c 69
cathal66 0:2cf2aa8c0d7c 70 colorbuf[0] = red + green + blue;
cathal66 0:2cf2aa8c0d7c 71
cathal66 0:2cf2aa8c0d7c 72 pc.printf("Colour: %6x red:%6x Green:%6x Blue:%6x Bright:%x \n\r",colorbuf[0], red , green , blue, bright);
cathal66 0:2cf2aa8c0d7c 73 // for each of the colours (j) write out 10 of them
cathal66 0:2cf2aa8c0d7c 74 // the pixels are written at the colour*10, plus the colour position
cathal66 0:2cf2aa8c0d7c 75 // all modulus 60 so it wraps around
cathal66 0:2cf2aa8c0d7c 76 for (int i = 0; i < WS2812_BUF; i++) {
cathal66 0:2cf2aa8c0d7c 77 px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
cathal66 0:2cf2aa8c0d7c 78 }
cathal66 0:2cf2aa8c0d7c 79
cathal66 0:2cf2aa8c0d7c 80 // now all the colours are computed, add a fade effect using intensity scaling
cathal66 0:2cf2aa8c0d7c 81 // compute and write the II value for each pixel
cathal66 0:2cf2aa8c0d7c 82 for (int j=0; j<WS2812_BUF; j++) {
cathal66 0:2cf2aa8c0d7c 83 // px.SetI(pixel position, II value)
cathal66 0:2cf2aa8c0d7c 84 //px.SetI(j%WS2812_BUF, 0xff+(0xf*(j%NUM_LEDS_PER_COLOR))); //full brightness
cathal66 0:2cf2aa8c0d7c 85 px.SetI(j%WS2812_BUF, bright +(bright*(j%NUM_LEDS_PER_COLOR))); //control brightness
cathal66 0:2cf2aa8c0d7c 86 }
cathal66 0:2cf2aa8c0d7c 87 //set the colour of the LEDs
cathal66 0:2cf2aa8c0d7c 88 for (int z=WS2812_BUF; z >= 0 ; z--) {
cathal66 0:2cf2aa8c0d7c 89 ws.write_offsets(px.getBuf(),z,z,z);
cathal66 0:2cf2aa8c0d7c 90
cathal66 0:2cf2aa8c0d7c 91
cathal66 0:2cf2aa8c0d7c 92 }
cathal66 0:2cf2aa8c0d7c 93 }
cathal66 0:2cf2aa8c0d7c 94
cathal66 0:2cf2aa8c0d7c 95
cathal66 0:2cf2aa8c0d7c 96 void LED_Colour_Pos(int position, int red , int green ,int blue , int bright)
cathal66 0:2cf2aa8c0d7c 97 {
cathal66 0:2cf2aa8c0d7c 98 ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
cathal66 0:2cf2aa8c0d7c 99
cathal66 0:2cf2aa8c0d7c 100 int colorbuf = 0x000000;
cathal66 0:2cf2aa8c0d7c 101
cathal66 0:2cf2aa8c0d7c 102 red <<= 16;
cathal66 0:2cf2aa8c0d7c 103 green <<= 8;
cathal66 0:2cf2aa8c0d7c 104
cathal66 0:2cf2aa8c0d7c 105 colorbuf = red + green + blue;
cathal66 0:2cf2aa8c0d7c 106
cathal66 0:2cf2aa8c0d7c 107 pc.printf("Colour: %6x red:%6x Green:%6x Blue:%6x Bright:%x \n\r",colorbuf, red , green , blue, bright);
cathal66 0:2cf2aa8c0d7c 108 // for each of the colours (j) write out 10 of them
cathal66 0:2cf2aa8c0d7c 109 // the pixels are written at the colour*10, plus the colour position
cathal66 0:2cf2aa8c0d7c 110 // all modulus 60 so it wraps around
cathal66 0:2cf2aa8c0d7c 111 px.Set(position, colorbuf);
cathal66 0:2cf2aa8c0d7c 112
cathal66 0:2cf2aa8c0d7c 113 // now all the colours are computed, add a fade effect using intensity scaling
cathal66 0:2cf2aa8c0d7c 114 // compute and write the II value for each pixel
cathal66 0:2cf2aa8c0d7c 115 //px.SetI(pixel position, II value)
cathal66 0:2cf2aa8c0d7c 116 px.SetI(position, bright); //control brightness
cathal66 0:2cf2aa8c0d7c 117 }
cathal66 0:2cf2aa8c0d7c 118
cathal66 0:2cf2aa8c0d7c 119 void LED_Colour_Pos_Set()
cathal66 0:2cf2aa8c0d7c 120 {
cathal66 0:2cf2aa8c0d7c 121 //set the colour of the LEDs
cathal66 0:2cf2aa8c0d7c 122 for (int z=WS2812_BUF-1; z >= 0 ; z--)
cathal66 0:2cf2aa8c0d7c 123 {
cathal66 0:2cf2aa8c0d7c 124 ws.write_offsets(px.getBuf(),z,z,z);
cathal66 0:2cf2aa8c0d7c 125 }
cathal66 0:2cf2aa8c0d7c 126 }
cathal66 0:2cf2aa8c0d7c 127
cathal66 0:2cf2aa8c0d7c 128
cathal66 0:2cf2aa8c0d7c 129 void Smooth_Fade(int h , int speed)
cathal66 0:2cf2aa8c0d7c 130 {
cathal66 0:2cf2aa8c0d7c 131 for(int j = 0; j<=h;j++)
cathal66 0:2cf2aa8c0d7c 132 {
cathal66 0:2cf2aa8c0d7c 133 for(int i = 0;i<=255;i=i+speed)
cathal66 0:2cf2aa8c0d7c 134 {
cathal66 0:2cf2aa8c0d7c 135 LED_All_Colour(i, 0 ,255-i , 255);
cathal66 0:2cf2aa8c0d7c 136 //wait(0.01);
cathal66 0:2cf2aa8c0d7c 137 }
cathal66 0:2cf2aa8c0d7c 138 for(int i = 0;i<=255;i=i+speed)
cathal66 0:2cf2aa8c0d7c 139 {
cathal66 0:2cf2aa8c0d7c 140 LED_All_Colour(255-i, i ,0 , 255);
cathal66 0:2cf2aa8c0d7c 141 //wait(0.01);
cathal66 0:2cf2aa8c0d7c 142 }
cathal66 0:2cf2aa8c0d7c 143 for(int i = 0;i<=255;i=i+speed)
cathal66 0:2cf2aa8c0d7c 144 {
cathal66 0:2cf2aa8c0d7c 145 LED_All_Colour(0, 255-i ,i , 255);
cathal66 0:2cf2aa8c0d7c 146 //wait(0.01);
cathal66 0:2cf2aa8c0d7c 147 }
cathal66 0:2cf2aa8c0d7c 148 }
cathal66 0:2cf2aa8c0d7c 149 }
cathal66 0:2cf2aa8c0d7c 150
cathal66 0:2cf2aa8c0d7c 151
cathal66 0:2cf2aa8c0d7c 152 void Red_Blue_Flash(int h)
cathal66 0:2cf2aa8c0d7c 153 {
cathal66 0:2cf2aa8c0d7c 154 for(int i = 0;i<=h;i++)
cathal66 0:2cf2aa8c0d7c 155 {
cathal66 0:2cf2aa8c0d7c 156
cathal66 0:2cf2aa8c0d7c 157 LED_All_Colour(255, 0 ,0 , 50);
cathal66 0:2cf2aa8c0d7c 158 wait(0.02);
cathal66 0:2cf2aa8c0d7c 159 LED_All_Colour(255, 0 ,0 , 255);
cathal66 0:2cf2aa8c0d7c 160 wait(0.02);
cathal66 0:2cf2aa8c0d7c 161 LED_All_Colour(0, 0 , 255 , 50);
cathal66 0:2cf2aa8c0d7c 162 wait(0.02);
cathal66 0:2cf2aa8c0d7c 163 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:2cf2aa8c0d7c 164 wait(0.02);
cathal66 0:2cf2aa8c0d7c 165 }
cathal66 0:2cf2aa8c0d7c 166 }
cathal66 0:2cf2aa8c0d7c 167
cathal66 0:2cf2aa8c0d7c 168
cathal66 0:2cf2aa8c0d7c 169 int main() {
cathal66 0:2cf2aa8c0d7c 170 Thread thread_Servo_Z(Servo_Z_Thread );
cathal66 0:2cf2aa8c0d7c 171
cathal66 0:2cf2aa8c0d7c 172
cathal66 0:2cf2aa8c0d7c 173 while (true) {
cathal66 0:2cf2aa8c0d7c 174 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 175 //////////////////////////////////// X //////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 176 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 177 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 178 Servo_X = 0.06;
cathal66 0:2cf2aa8c0d7c 179 Servo_z_smooth = 0.01;
cathal66 0:2cf2aa8c0d7c 180 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:2cf2aa8c0d7c 181 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:2cf2aa8c0d7c 182 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:2cf2aa8c0d7c 183 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:2cf2aa8c0d7c 184 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:2cf2aa8c0d7c 185 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:2cf2aa8c0d7c 186 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:2cf2aa8c0d7c 187 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:2cf2aa8c0d7c 188 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:2cf2aa8c0d7c 189 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:2cf2aa8c0d7c 190 LED_Colour_Pos_Set();
cathal66 0:2cf2aa8c0d7c 191 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4 \n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 192 Thread::wait(1000);
cathal66 0:2cf2aa8c0d7c 193 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 194 Servo_X = 0.06;
cathal66 0:2cf2aa8c0d7c 195 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 196 LED_Colour_Pos(0,128,255,0,128);
cathal66 0:2cf2aa8c0d7c 197 LED_Colour_Pos(1,128,50,0,255);
cathal66 0:2cf2aa8c0d7c 198 LED_Colour_Pos(2,0,255,128,128);
cathal66 0:2cf2aa8c0d7c 199 LED_Colour_Pos(3,128,0,128,255);
cathal66 0:2cf2aa8c0d7c 200 LED_Colour_Pos(4,128,128,255,128);
cathal66 0:2cf2aa8c0d7c 201 LED_Colour_Pos(5,255,255,0,255);
cathal66 0:2cf2aa8c0d7c 202 LED_Colour_Pos(6,128,50,127,255);
cathal66 0:2cf2aa8c0d7c 203 LED_Colour_Pos(7,255,255,0,128);
cathal66 0:2cf2aa8c0d7c 204 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:2cf2aa8c0d7c 205 LED_Colour_Pos(9,0,128,255,128);
cathal66 0:2cf2aa8c0d7c 206 LED_Colour_Pos_Set();
cathal66 0:2cf2aa8c0d7c 207 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 208 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 209 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 210 Servo_X = 0.08;
cathal66 0:2cf2aa8c0d7c 211 Servo_z_smooth = 0.08;
cathal66 0:2cf2aa8c0d7c 212 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 213 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 214 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 215 Servo_X = 0.05;
cathal66 0:2cf2aa8c0d7c 216 Servo_z_smooth = 0.1;
cathal66 0:2cf2aa8c0d7c 217 LED_All_Colour(255, 0 , 0 , 255);
cathal66 0:2cf2aa8c0d7c 218 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 219 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 220 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 221 Servo_X = 0.05;
cathal66 0:2cf2aa8c0d7c 222 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 223 LED_All_Colour(255, 255 , 0 , 255);
cathal66 0:2cf2aa8c0d7c 224 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 225 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 226 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 227 Servo_X = 0.09;
cathal66 0:2cf2aa8c0d7c 228 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 229 LED_All_Colour(0, 0 , 255 , 255);
cathal66 0:2cf2aa8c0d7c 230 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 231 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 232 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 233 Servo_X = 0.05;
cathal66 0:2cf2aa8c0d7c 234 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 235 LED_All_Colour(0, 255 , 0 , 255);
cathal66 0:2cf2aa8c0d7c 236 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 237 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 238 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 239 Servo_X = 0.09;
cathal66 0:2cf2aa8c0d7c 240 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 241 LED_Colour_Pos(0,255,0,0,128);
cathal66 0:2cf2aa8c0d7c 242 LED_Colour_Pos(1,0,255,0,128);
cathal66 0:2cf2aa8c0d7c 243 LED_Colour_Pos(2,0,0,255,128);
cathal66 0:2cf2aa8c0d7c 244 LED_Colour_Pos(3,255,255,0,128);
cathal66 0:2cf2aa8c0d7c 245 LED_Colour_Pos(4,255,0,255,128);
cathal66 0:2cf2aa8c0d7c 246 LED_Colour_Pos(5,128,255,0,128);
cathal66 0:2cf2aa8c0d7c 247 LED_Colour_Pos(6,128,50,0,255);
cathal66 0:2cf2aa8c0d7c 248 LED_Colour_Pos(7,0,255,128,128);
cathal66 0:2cf2aa8c0d7c 249 LED_Colour_Pos(8,128,0,128,255);
cathal66 0:2cf2aa8c0d7c 250 LED_Colour_Pos(9,128,128,255,128);
cathal66 0:2cf2aa8c0d7c 251 LED_Colour_Pos_Set();
cathal66 0:2cf2aa8c0d7c 252 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 253 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 254 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 255 Servo_X = 0.05;
cathal66 0:2cf2aa8c0d7c 256 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 257 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 258 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 259 Servo_Y = 0.05;
cathal66 0:2cf2aa8c0d7c 260 Servo_X = 0.08;
cathal66 0:2cf2aa8c0d7c 261 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 262 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 263 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 264
cathal66 0:2cf2aa8c0d7c 265 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 266 //////////////////////////////////// y //////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 267 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 268 Servo_Y = 0.055;
cathal66 0:2cf2aa8c0d7c 269 Servo_X = 0.08;
cathal66 0:2cf2aa8c0d7c 270 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 271 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 272 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 273 Servo_Y = 0.08;
cathal66 0:2cf2aa8c0d7c 274 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 275 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 276 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 277 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 278 Servo_Y = 0.055;
cathal66 0:2cf2aa8c0d7c 279 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 280 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 281 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 282 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 283 Servo_Y = 0.065;
cathal66 0:2cf2aa8c0d7c 284 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 285 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 286 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 287 Thread::wait(1000);
cathal66 0:2cf2aa8c0d7c 288 Servo_Y = 0.055;
cathal66 0:2cf2aa8c0d7c 289 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 290 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 291 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 292 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 293 Servo_Y = 0.065;
cathal66 0:2cf2aa8c0d7c 294 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 295 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 296 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 297 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 298 Servo_Y = 0.055;
cathal66 0:2cf2aa8c0d7c 299 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 300 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 301 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 302 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 303 Servo_Y = 0.065;
cathal66 0:2cf2aa8c0d7c 304 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 305 Servo_z_smooth = 0.05;
cathal66 0:2cf2aa8c0d7c 306 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 307 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 308 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 309 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 310 Servo_z_smooth = 0.09;
cathal66 0:2cf2aa8c0d7c 311 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 312 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 313
cathal66 0:2cf2aa8c0d7c 314 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 315 //////////////////////////////////// Z //////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 316 /////////////////////////////////////////////////////////////////////////////////////
cathal66 0:2cf2aa8c0d7c 317 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 318 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 319 Servo_z_smooth = 0.08;
cathal66 0:2cf2aa8c0d7c 320 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 321 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 322 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 323 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 324 Servo_z_smooth = 0.1;
cathal66 0:2cf2aa8c0d7c 325 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 326 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 327 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 328 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 329 Servo_z_smooth = 0.07;
cathal66 0:2cf2aa8c0d7c 330 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 331 Thread::wait(500);
cathal66 0:2cf2aa8c0d7c 332 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 333 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 334 Servo_z_smooth = 0.11;
cathal66 0:2cf2aa8c0d7c 335 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 336 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 337 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 338 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 339 Servo_z_smooth = 0.07;
cathal66 0:2cf2aa8c0d7c 340 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 341 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 342 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 343 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 344 Servo_z_smooth = 0.11;
cathal66 0:2cf2aa8c0d7c 345 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 346 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 347 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 348 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 349 Servo_z_smooth = 0.07;
cathal66 0:2cf2aa8c0d7c 350 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 351 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 352 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 353 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 354 Servo_z_smooth = 0.11;
cathal66 0:2cf2aa8c0d7c 355 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 356 Thread::wait(750);
cathal66 0:2cf2aa8c0d7c 357 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 358 Servo_X = 0.075;
cathal66 0:2cf2aa8c0d7c 359 Servo_z_smooth = 0.07;
cathal66 0:2cf2aa8c0d7c 360 printf("Servo X: %f 1.4 Servo Y: %f 1.4 Servo Z: %f 1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 361 Thread::wait(1500);
cathal66 0:2cf2aa8c0d7c 362 Servo_Y = 0.06;
cathal66 0:2cf2aa8c0d7c 363 Servo_X = 0.75;
cathal66 0:2cf2aa8c0d7c 364 Servo_z_smooth = 0.03;
cathal66 0:2cf2aa8c0d7c 365 printf("Servo X: %f1.4 Servo Y: %f1.4 Servo Z: %f1.4\n\r",Servo_X, Servo_Y, Servo_z_smooth);
cathal66 0:2cf2aa8c0d7c 366 Thread::wait(1500);
cathal66 0:2cf2aa8c0d7c 367 }
cathal66 0:2cf2aa8c0d7c 368 }