Graphical demo for the LPC4088 Experiment Base Board with one of the Display Expansion Kits. This program displays a number of dots in a 3D-projected wave.
WaveDemo.cpp@0:d1e4929f6956, 2014-10-03 (annotated)
- Committer:
- embeddedartists
- Date:
- Fri Oct 03 13:37:30 2014 +0000
- Revision:
- 0:d1e4929f6956
First version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
embeddedartists | 0:d1e4929f6956 | 1 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 2 | * Includes |
embeddedartists | 0:d1e4929f6956 | 3 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 4 | |
embeddedartists | 0:d1e4929f6956 | 5 | #include "mbed.h" |
embeddedartists | 0:d1e4929f6956 | 6 | |
embeddedartists | 0:d1e4929f6956 | 7 | #include "LcdController.h" |
embeddedartists | 0:d1e4929f6956 | 8 | #include "EaLcdBoard.h" |
embeddedartists | 0:d1e4929f6956 | 9 | #include "WaveDemo.h" |
embeddedartists | 0:d1e4929f6956 | 10 | |
embeddedartists | 0:d1e4929f6956 | 11 | #include <math.h> |
embeddedartists | 0:d1e4929f6956 | 12 | |
embeddedartists | 0:d1e4929f6956 | 13 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 14 | * Typedefs and defines |
embeddedartists | 0:d1e4929f6956 | 15 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 16 | |
embeddedartists | 0:d1e4929f6956 | 17 | #define PI2 6.283185307179586476925286766559 |
embeddedartists | 0:d1e4929f6956 | 18 | |
embeddedartists | 0:d1e4929f6956 | 19 | /* Red color mask, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 20 | #define REDMASK 0xF800 |
embeddedartists | 0:d1e4929f6956 | 21 | /* Red shift value, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 22 | #define REDSHIFT 11 |
embeddedartists | 0:d1e4929f6956 | 23 | /* Green color mask, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 24 | #define GREENMASK 0x07E0 |
embeddedartists | 0:d1e4929f6956 | 25 | /* Green shift value, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 26 | #define GREENSHIFT 5 |
embeddedartists | 0:d1e4929f6956 | 27 | /* Blue color mask, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 28 | #define BLUEMASK 0x001F |
embeddedartists | 0:d1e4929f6956 | 29 | /* Blue shift value, 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 30 | #define BLUESHIFT 0 |
embeddedartists | 0:d1e4929f6956 | 31 | |
embeddedartists | 0:d1e4929f6956 | 32 | /* Number of colors in 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 33 | #define NUM_COLORS 65536 |
embeddedartists | 0:d1e4929f6956 | 34 | /* Number of red colors in 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 35 | #define RED_COLORS 0x20 |
embeddedartists | 0:d1e4929f6956 | 36 | /* Number of green colors in 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 37 | #define GREEN_COLORS 0x40 |
embeddedartists | 0:d1e4929f6956 | 38 | /* Number of blue colors in 565 mode */ |
embeddedartists | 0:d1e4929f6956 | 39 | #define BLUE_COLORS 0x20 |
embeddedartists | 0:d1e4929f6956 | 40 | |
embeddedartists | 0:d1e4929f6956 | 41 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 42 | * Local variables |
embeddedartists | 0:d1e4929f6956 | 43 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 44 | |
embeddedartists | 0:d1e4929f6956 | 45 | |
embeddedartists | 0:d1e4929f6956 | 46 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 47 | * External variables |
embeddedartists | 0:d1e4929f6956 | 48 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 49 | |
embeddedartists | 0:d1e4929f6956 | 50 | |
embeddedartists | 0:d1e4929f6956 | 51 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 52 | * Local functions |
embeddedartists | 0:d1e4929f6956 | 53 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 54 | |
embeddedartists | 0:d1e4929f6956 | 55 | uint16_t WaveDemo::fastsqrt(uint32_t n) const { |
embeddedartists | 0:d1e4929f6956 | 56 | uint16_t c = 0x8000; |
embeddedartists | 0:d1e4929f6956 | 57 | uint16_t g = 0x8000; |
embeddedartists | 0:d1e4929f6956 | 58 | for(;;) { |
embeddedartists | 0:d1e4929f6956 | 59 | if(g*g > n) |
embeddedartists | 0:d1e4929f6956 | 60 | g ^= c; |
embeddedartists | 0:d1e4929f6956 | 61 | c >>= 1; |
embeddedartists | 0:d1e4929f6956 | 62 | if(c == 0) |
embeddedartists | 0:d1e4929f6956 | 63 | return g; |
embeddedartists | 0:d1e4929f6956 | 64 | g |= c; |
embeddedartists | 0:d1e4929f6956 | 65 | } |
embeddedartists | 0:d1e4929f6956 | 66 | } |
embeddedartists | 0:d1e4929f6956 | 67 | |
embeddedartists | 0:d1e4929f6956 | 68 | void WaveDemo::matrix(int16_t xyz[3][N], uint8_t rgb[3][N]) { |
embeddedartists | 0:d1e4929f6956 | 69 | static uint32_t t = 0; |
embeddedartists | 0:d1e4929f6956 | 70 | uint16_t i; |
embeddedartists | 0:d1e4929f6956 | 71 | int16_t x = -SCALE; |
embeddedartists | 0:d1e4929f6956 | 72 | int16_t y = -SCALE; |
embeddedartists | 0:d1e4929f6956 | 73 | uint16_t d; |
embeddedartists | 0:d1e4929f6956 | 74 | uint16_t s; |
embeddedartists | 0:d1e4929f6956 | 75 | |
embeddedartists | 0:d1e4929f6956 | 76 | for(i = 0; i < N; i++) { |
embeddedartists | 0:d1e4929f6956 | 77 | |
embeddedartists | 0:d1e4929f6956 | 78 | xyz[0][i] = x; |
embeddedartists | 0:d1e4929f6956 | 79 | xyz[1][i] = y; |
embeddedartists | 0:d1e4929f6956 | 80 | |
embeddedartists | 0:d1e4929f6956 | 81 | d = fastsqrt(x * x + y * y); |
embeddedartists | 0:d1e4929f6956 | 82 | s = sine[(t * 30) % SCALE] + SCALE; |
embeddedartists | 0:d1e4929f6956 | 83 | |
embeddedartists | 0:d1e4929f6956 | 84 | xyz[2][i] = sine[(d + s) % SCALE] * |
embeddedartists | 0:d1e4929f6956 | 85 | sine[(t * 10) % SCALE] / SCALE / 2; |
embeddedartists | 0:d1e4929f6956 | 86 | |
embeddedartists | 0:d1e4929f6956 | 87 | rgb[0][i] = (cosi[xyz[2][i] + SCALE / 2] + SCALE) * |
embeddedartists | 0:d1e4929f6956 | 88 | (RED_COLORS - 1) / SCALE / 2; |
embeddedartists | 0:d1e4929f6956 | 89 | |
embeddedartists | 0:d1e4929f6956 | 90 | rgb[1][i] = (cosi[(xyz[2][i] + SCALE / 2 + 2 * SCALE / 3) % SCALE] + SCALE) * |
embeddedartists | 0:d1e4929f6956 | 91 | (GREEN_COLORS - 1) / SCALE / 2; |
embeddedartists | 0:d1e4929f6956 | 92 | |
embeddedartists | 0:d1e4929f6956 | 93 | rgb[2][i] = (cosi[(xyz[2][i] + SCALE / 2 + SCALE / 3) % SCALE] + SCALE) * |
embeddedartists | 0:d1e4929f6956 | 94 | (BLUE_COLORS - 1) / SCALE / 2; |
embeddedartists | 0:d1e4929f6956 | 95 | |
embeddedartists | 0:d1e4929f6956 | 96 | x += INCREMENT; |
embeddedartists | 0:d1e4929f6956 | 97 | if(x >= SCALE) { |
embeddedartists | 0:d1e4929f6956 | 98 | x = -SCALE; |
embeddedartists | 0:d1e4929f6956 | 99 | y += INCREMENT; |
embeddedartists | 0:d1e4929f6956 | 100 | } |
embeddedartists | 0:d1e4929f6956 | 101 | |
embeddedartists | 0:d1e4929f6956 | 102 | } |
embeddedartists | 0:d1e4929f6956 | 103 | t++; |
embeddedartists | 0:d1e4929f6956 | 104 | } |
embeddedartists | 0:d1e4929f6956 | 105 | |
embeddedartists | 0:d1e4929f6956 | 106 | void WaveDemo::rotate(int16_t xyz[3][N], uint8_t rgb[3][N], |
embeddedartists | 0:d1e4929f6956 | 107 | uint16_t angleX, uint16_t angleY, uint16_t angleZ) { |
embeddedartists | 0:d1e4929f6956 | 108 | uint16_t i; |
embeddedartists | 0:d1e4929f6956 | 109 | int16_t tmpX; |
embeddedartists | 0:d1e4929f6956 | 110 | int16_t tmpY; |
embeddedartists | 0:d1e4929f6956 | 111 | int16_t sinx = sine[angleX]; |
embeddedartists | 0:d1e4929f6956 | 112 | int16_t cosx = cosi[angleX]; |
embeddedartists | 0:d1e4929f6956 | 113 | int16_t siny = sine[angleY]; |
embeddedartists | 0:d1e4929f6956 | 114 | int16_t cosy = cosi[angleY]; |
embeddedartists | 0:d1e4929f6956 | 115 | int16_t sinz = sine[angleZ]; |
embeddedartists | 0:d1e4929f6956 | 116 | int16_t cosz = cosi[angleZ]; |
embeddedartists | 0:d1e4929f6956 | 117 | |
embeddedartists | 0:d1e4929f6956 | 118 | for(i = 0; i < N; i++) { |
embeddedartists | 0:d1e4929f6956 | 119 | tmpX = (xyz[0][i] * cosx - xyz[2][i] * sinx) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 120 | xyz[2][i] = (xyz[0][i] * sinx + xyz[2][i] * cosx) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 121 | xyz[0][i] = tmpX; |
embeddedartists | 0:d1e4929f6956 | 122 | |
embeddedartists | 0:d1e4929f6956 | 123 | tmpY = (xyz[1][i] * cosy - xyz[2][i] * siny) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 124 | xyz[2][i] = (xyz[1][i] * siny + xyz[2][i] * cosy) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 125 | xyz[1][i] = tmpY; |
embeddedartists | 0:d1e4929f6956 | 126 | |
embeddedartists | 0:d1e4929f6956 | 127 | tmpX = (xyz[0][i] * cosz - xyz[1][i] * sinz) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 128 | xyz[1][i] = (xyz[0][i] * sinz + xyz[1][i] * cosz) / SCALE; |
embeddedartists | 0:d1e4929f6956 | 129 | xyz[0][i] = tmpX; |
embeddedartists | 0:d1e4929f6956 | 130 | } |
embeddedartists | 0:d1e4929f6956 | 131 | } |
embeddedartists | 0:d1e4929f6956 | 132 | |
embeddedartists | 0:d1e4929f6956 | 133 | void WaveDemo::draw(int16_t xyz[3][N], uint8_t rgb[3][N]) { |
embeddedartists | 0:d1e4929f6956 | 134 | // static uint16_t oldProjX[N] = {0}; |
embeddedartists | 0:d1e4929f6956 | 135 | // static uint16_t oldProjY[N] = {0}; |
embeddedartists | 0:d1e4929f6956 | 136 | // static uint8_t oldDotSize[N] = {0}; |
embeddedartists | 0:d1e4929f6956 | 137 | uint16_t i; |
embeddedartists | 0:d1e4929f6956 | 138 | uint16_t projX; |
embeddedartists | 0:d1e4929f6956 | 139 | uint16_t projY; |
embeddedartists | 0:d1e4929f6956 | 140 | uint16_t projZ; |
embeddedartists | 0:d1e4929f6956 | 141 | uint16_t dotSize; |
embeddedartists | 0:d1e4929f6956 | 142 | static uint8_t cnt=0; |
embeddedartists | 0:d1e4929f6956 | 143 | |
embeddedartists | 0:d1e4929f6956 | 144 | if (cnt == 0) |
embeddedartists | 0:d1e4929f6956 | 145 | { |
embeddedartists | 0:d1e4929f6956 | 146 | cnt = 1; |
embeddedartists | 0:d1e4929f6956 | 147 | pFrmBuf = pFrmBuf1; |
embeddedartists | 0:d1e4929f6956 | 148 | } |
embeddedartists | 0:d1e4929f6956 | 149 | else if (cnt == 1) |
embeddedartists | 0:d1e4929f6956 | 150 | { |
embeddedartists | 0:d1e4929f6956 | 151 | cnt = 2; |
embeddedartists | 0:d1e4929f6956 | 152 | pFrmBuf = pFrmBuf2; |
embeddedartists | 0:d1e4929f6956 | 153 | } |
embeddedartists | 0:d1e4929f6956 | 154 | else |
embeddedartists | 0:d1e4929f6956 | 155 | { |
embeddedartists | 0:d1e4929f6956 | 156 | cnt = 0; |
embeddedartists | 0:d1e4929f6956 | 157 | pFrmBuf = pFrmBuf3; |
embeddedartists | 0:d1e4929f6956 | 158 | } |
embeddedartists | 0:d1e4929f6956 | 159 | |
embeddedartists | 0:d1e4929f6956 | 160 | graphics.setFrameBuffer(pFrmBuf); |
embeddedartists | 0:d1e4929f6956 | 161 | |
embeddedartists | 0:d1e4929f6956 | 162 | memset((void*)(pFrmBuf), 0x00, windowX * windowY * 2); |
embeddedartists | 0:d1e4929f6956 | 163 | |
embeddedartists | 0:d1e4929f6956 | 164 | for(i = 0; i < N; i++) { |
embeddedartists | 0:d1e4929f6956 | 165 | projZ = SCALE - (xyz[2][i] + SCALE) / 4; //4; |
embeddedartists | 0:d1e4929f6956 | 166 | projX = windowX / 2 + (xyz[0][i] * projZ / SCALE) / 40; //EA 25; |
embeddedartists | 0:d1e4929f6956 | 167 | projY = windowY / 2 + (xyz[1][i] * projZ / SCALE) / 40; //EA 25; |
embeddedartists | 0:d1e4929f6956 | 168 | dotSize = 3 - (xyz[2][i] + SCALE) * 2 / SCALE; |
embeddedartists | 0:d1e4929f6956 | 169 | //EA put_circle(oldProjX[i], oldProjY[i], 0, dotSize, 1); |
embeddedartists | 0:d1e4929f6956 | 170 | |
embeddedartists | 0:d1e4929f6956 | 171 | if((projX > dotSize) && |
embeddedartists | 0:d1e4929f6956 | 172 | (projY > dotSize) && |
embeddedartists | 0:d1e4929f6956 | 173 | (projX < (windowX - dotSize)) && |
embeddedartists | 0:d1e4929f6956 | 174 | (projY < (windowY - dotSize))) { |
embeddedartists | 0:d1e4929f6956 | 175 | |
embeddedartists | 0:d1e4929f6956 | 176 | graphics.put_circle(projX, projY, (rgb[0][i] << REDSHIFT) + (rgb[1][i] << GREENSHIFT) + (rgb[2][i] << BLUESHIFT), dotSize, 1); |
embeddedartists | 0:d1e4929f6956 | 177 | |
embeddedartists | 0:d1e4929f6956 | 178 | // oldProjX[i] = projX; |
embeddedartists | 0:d1e4929f6956 | 179 | // oldProjY[i] = projY; |
embeddedartists | 0:d1e4929f6956 | 180 | // oldDotSize[i] = dotSize; |
embeddedartists | 0:d1e4929f6956 | 181 | } |
embeddedartists | 0:d1e4929f6956 | 182 | } |
embeddedartists | 0:d1e4929f6956 | 183 | } |
embeddedartists | 0:d1e4929f6956 | 184 | |
embeddedartists | 0:d1e4929f6956 | 185 | |
embeddedartists | 0:d1e4929f6956 | 186 | |
embeddedartists | 0:d1e4929f6956 | 187 | /****************************************************************************** |
embeddedartists | 0:d1e4929f6956 | 188 | * Public functions |
embeddedartists | 0:d1e4929f6956 | 189 | *****************************************************************************/ |
embeddedartists | 0:d1e4929f6956 | 190 | WaveDemo::WaveDemo(uint8_t *pFrameBuf, uint16_t dispWidth, uint16_t dispHeight) |
embeddedartists | 0:d1e4929f6956 | 191 | : graphics((uint16_t *)pFrameBuf, dispWidth, dispHeight) |
embeddedartists | 0:d1e4929f6956 | 192 | { |
embeddedartists | 0:d1e4929f6956 | 193 | this->windowX = dispWidth; |
embeddedartists | 0:d1e4929f6956 | 194 | this->windowY = dispHeight; |
embeddedartists | 0:d1e4929f6956 | 195 | this->pFrmBuf = (uint16_t *)pFrameBuf; |
embeddedartists | 0:d1e4929f6956 | 196 | this->pFrmBuf1 = (uint16_t *)pFrameBuf; |
embeddedartists | 0:d1e4929f6956 | 197 | this->pFrmBuf2 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*2); |
embeddedartists | 0:d1e4929f6956 | 198 | this->pFrmBuf3 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*4); |
embeddedartists | 0:d1e4929f6956 | 199 | |
embeddedartists | 0:d1e4929f6956 | 200 | for(uint16_t i = 0; i < SCALE; i++) { |
embeddedartists | 0:d1e4929f6956 | 201 | sine[i] = (int)(sin(PI2 * i / SCALE) * SCALE); |
embeddedartists | 0:d1e4929f6956 | 202 | cosi[i] = (int)(cos(PI2 * i / SCALE) * SCALE); |
embeddedartists | 0:d1e4929f6956 | 203 | } |
embeddedartists | 0:d1e4929f6956 | 204 | } |
embeddedartists | 0:d1e4929f6956 | 205 | |
embeddedartists | 0:d1e4929f6956 | 206 | void WaveDemo::run(EaLcdBoardGPIO& lcdBoard, uint32_t loops, uint32_t delayMs) |
embeddedartists | 0:d1e4929f6956 | 207 | { |
embeddedartists | 0:d1e4929f6956 | 208 | printf("WaveDemo, %d loops, %dms delay\n", loops, delayMs); |
embeddedartists | 0:d1e4929f6956 | 209 | |
embeddedartists | 0:d1e4929f6956 | 210 | int16_t angleX = 0; |
embeddedartists | 0:d1e4929f6956 | 211 | int16_t angleY = 1000; |
embeddedartists | 0:d1e4929f6956 | 212 | int16_t angleZ = 0; |
embeddedartists | 0:d1e4929f6956 | 213 | |
embeddedartists | 0:d1e4929f6956 | 214 | int16_t speedX = 0; |
embeddedartists | 0:d1e4929f6956 | 215 | int16_t speedY = 0; |
embeddedartists | 0:d1e4929f6956 | 216 | int16_t speedZ = 0; |
embeddedartists | 0:d1e4929f6956 | 217 | |
embeddedartists | 0:d1e4929f6956 | 218 | int16_t xyz[3][N]; |
embeddedartists | 0:d1e4929f6956 | 219 | uint8_t rgb[3][N]; |
embeddedartists | 0:d1e4929f6956 | 220 | |
embeddedartists | 0:d1e4929f6956 | 221 | loops = 2*820; |
embeddedartists | 0:d1e4929f6956 | 222 | for(int32_t n=0;n<loops;n++) { |
embeddedartists | 0:d1e4929f6956 | 223 | |
embeddedartists | 0:d1e4929f6956 | 224 | matrix(xyz, rgb); |
embeddedartists | 0:d1e4929f6956 | 225 | |
embeddedartists | 0:d1e4929f6956 | 226 | rotate(xyz, rgb, angleX, angleY, angleZ); |
embeddedartists | 0:d1e4929f6956 | 227 | |
embeddedartists | 0:d1e4929f6956 | 228 | draw(xyz, rgb); |
embeddedartists | 0:d1e4929f6956 | 229 | //update framebuffer |
embeddedartists | 0:d1e4929f6956 | 230 | lcdBoard.setFrameBuffer((uint32_t)this->pFrmBuf); |
embeddedartists | 0:d1e4929f6956 | 231 | |
embeddedartists | 0:d1e4929f6956 | 232 | #if 0 |
embeddedartists | 0:d1e4929f6956 | 233 | if(joyState & JOYSTICK_RIGHT) |
embeddedartists | 0:d1e4929f6956 | 234 | speedX -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 235 | else if(joyState & JOYSTICK_LEFT) |
embeddedartists | 0:d1e4929f6956 | 236 | speedX += SPEED; |
embeddedartists | 0:d1e4929f6956 | 237 | else if(joyState & JOYSTICK_UP) |
embeddedartists | 0:d1e4929f6956 | 238 | speedY -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 239 | else if(joyState & JOYSTICK_DOWN) |
embeddedartists | 0:d1e4929f6956 | 240 | speedY += SPEED; |
embeddedartists | 0:d1e4929f6956 | 241 | else if(ledState & KEY1) |
embeddedartists | 0:d1e4929f6956 | 242 | speedZ -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 243 | else if(ledState & KEY2) |
embeddedartists | 0:d1e4929f6956 | 244 | speedZ += SPEED; |
embeddedartists | 0:d1e4929f6956 | 245 | else if(ledState & KEY3) { |
embeddedartists | 0:d1e4929f6956 | 246 | speedX = 0; |
embeddedartists | 0:d1e4929f6956 | 247 | speedY = 0; |
embeddedartists | 0:d1e4929f6956 | 248 | speedZ = 0; |
embeddedartists | 0:d1e4929f6956 | 249 | angleX = 0; |
embeddedartists | 0:d1e4929f6956 | 250 | angleY = 0; |
embeddedartists | 0:d1e4929f6956 | 251 | angleZ = 0; |
embeddedartists | 0:d1e4929f6956 | 252 | } else |
embeddedartists | 0:d1e4929f6956 | 253 | #endif |
embeddedartists | 0:d1e4929f6956 | 254 | { |
embeddedartists | 0:d1e4929f6956 | 255 | if(speedX > 0) |
embeddedartists | 0:d1e4929f6956 | 256 | speedX -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 257 | else if(speedX < 0) |
embeddedartists | 0:d1e4929f6956 | 258 | speedX += SPEED; |
embeddedartists | 0:d1e4929f6956 | 259 | |
embeddedartists | 0:d1e4929f6956 | 260 | if(speedY > 0) |
embeddedartists | 0:d1e4929f6956 | 261 | speedY -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 262 | else if(speedY < 0) |
embeddedartists | 0:d1e4929f6956 | 263 | speedY += SPEED; |
embeddedartists | 0:d1e4929f6956 | 264 | |
embeddedartists | 0:d1e4929f6956 | 265 | if(speedZ > 0) |
embeddedartists | 0:d1e4929f6956 | 266 | speedZ -= SPEED; |
embeddedartists | 0:d1e4929f6956 | 267 | else if(speedZ < 0) |
embeddedartists | 0:d1e4929f6956 | 268 | speedZ += SPEED; |
embeddedartists | 0:d1e4929f6956 | 269 | } |
embeddedartists | 0:d1e4929f6956 | 270 | |
embeddedartists | 0:d1e4929f6956 | 271 | angleX += 0; //speedX; |
embeddedartists | 0:d1e4929f6956 | 272 | angleY += 0; //speedY; |
embeddedartists | 0:d1e4929f6956 | 273 | angleZ += 2; //speedZ; |
embeddedartists | 0:d1e4929f6956 | 274 | |
embeddedartists | 0:d1e4929f6956 | 275 | if(angleX >= SCALE) |
embeddedartists | 0:d1e4929f6956 | 276 | angleX -= SCALE; |
embeddedartists | 0:d1e4929f6956 | 277 | else if(angleX < 0) |
embeddedartists | 0:d1e4929f6956 | 278 | angleX += SCALE; |
embeddedartists | 0:d1e4929f6956 | 279 | |
embeddedartists | 0:d1e4929f6956 | 280 | if(angleY >= SCALE) |
embeddedartists | 0:d1e4929f6956 | 281 | angleY -= SCALE; |
embeddedartists | 0:d1e4929f6956 | 282 | else if(angleY < 0) |
embeddedartists | 0:d1e4929f6956 | 283 | angleY += SCALE; |
embeddedartists | 0:d1e4929f6956 | 284 | |
embeddedartists | 0:d1e4929f6956 | 285 | if(angleZ >= SCALE) |
embeddedartists | 0:d1e4929f6956 | 286 | angleZ -= SCALE; |
embeddedartists | 0:d1e4929f6956 | 287 | else if(angleZ < 0) |
embeddedartists | 0:d1e4929f6956 | 288 | angleZ += SCALE; |
embeddedartists | 0:d1e4929f6956 | 289 | |
embeddedartists | 0:d1e4929f6956 | 290 | } |
embeddedartists | 0:d1e4929f6956 | 291 | //wait_ms(delayMs); |
embeddedartists | 0:d1e4929f6956 | 292 | wait_ms(1000); |
embeddedartists | 0:d1e4929f6956 | 293 | |
embeddedartists | 0:d1e4929f6956 | 294 | } |
embeddedartists | 0:d1e4929f6956 | 295 |