16A_Autopancakemaker
Dependencies: DigitDisplay Servo ds1307 mbed stepper
main.cpp@0:e7aedcbababd, 2015-12-12 (annotated)
- Committer:
- pruek
- Date:
- Sat Dec 12 07:40:53 2015 +0000
- Revision:
- 0:e7aedcbababd
A16 Autopancakemaker
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pruek | 0:e7aedcbababd | 1 | #include "mbed.h" |
pruek | 0:e7aedcbababd | 2 | #include "stepper.h" |
pruek | 0:e7aedcbababd | 3 | #include "Servo.h" |
pruek | 0:e7aedcbababd | 4 | #include <string> |
pruek | 0:e7aedcbababd | 5 | #include "DigitDisplay.h" |
pruek | 0:e7aedcbababd | 6 | #include "ds1307.h" |
pruek | 0:e7aedcbababd | 7 | #define ACCEL_ON 1 |
pruek | 0:e7aedcbababd | 8 | #define ACCEL_OFF 0 |
pruek | 0:e7aedcbababd | 9 | #define SPEED_X 300 |
pruek | 0:e7aedcbababd | 10 | #define SPEED_Y 300 |
pruek | 0:e7aedcbababd | 11 | #define X_STEPS_PER_MM 86 |
pruek | 0:e7aedcbababd | 12 | #define Y_STEPS_PER_MM 86 |
pruek | 0:e7aedcbababd | 13 | #define PI 3.14 |
pruek | 0:e7aedcbababd | 14 | #define Resolution 500 |
pruek | 0:e7aedcbababd | 15 | #define CURVE_SECTION_MM 5 |
pruek | 0:e7aedcbababd | 16 | #define Curve_feq_MM 5 |
pruek | 0:e7aedcbababd | 17 | //Communication |
pruek | 0:e7aedcbababd | 18 | Serial pc(SERIAL_TX,SERIAL_RX); |
pruek | 0:e7aedcbababd | 19 | Serial Nucleo(D8,D2); |
pruek | 0:e7aedcbababd | 20 | DS1307 thistime(D14,D15); |
pruek | 0:e7aedcbababd | 21 | //clock |
pruek | 0:e7aedcbababd | 22 | DigitalOut buzz(D6); |
pruek | 0:e7aedcbababd | 23 | |
pruek | 0:e7aedcbababd | 24 | DigitDisplay display(D7, D5); |
pruek | 0:e7aedcbababd | 25 | |
pruek | 0:e7aedcbababd | 26 | Ticker tick; |
pruek | 0:e7aedcbababd | 27 | int year = 1; |
pruek | 0:e7aedcbababd | 28 | int month = 1; |
pruek | 0:e7aedcbababd | 29 | int date =1; |
pruek | 0:e7aedcbababd | 30 | int day = 1; |
pruek | 0:e7aedcbababd | 31 | int hour = 17; |
pruek | 0:e7aedcbababd | 32 | int minute = 28; |
pruek | 0:e7aedcbababd | 33 | int second = 0; |
pruek | 0:e7aedcbababd | 34 | int Coundown = 30; |
pruek | 0:e7aedcbababd | 35 | //Servo |
pruek | 0:e7aedcbababd | 36 | Servo Pen_1(A0); |
pruek | 0:e7aedcbababd | 37 | //Servo Pen_2(A1); |
pruek | 0:e7aedcbababd | 38 | |
pruek | 0:e7aedcbababd | 39 | //Stepper |
pruek | 0:e7aedcbababd | 40 | stepper x(PB_13,PB_14); |
pruek | 0:e7aedcbababd | 41 | stepper y(PA_12,PC_5); |
pruek | 0:e7aedcbababd | 42 | |
pruek | 0:e7aedcbababd | 43 | //Limit Switch |
pruek | 0:e7aedcbababd | 44 | DigitalIn X_MIN(PC_0,PullUp); |
pruek | 0:e7aedcbababd | 45 | DigitalIn X_MAX(PC_1,PullUp); |
pruek | 0:e7aedcbababd | 46 | DigitalIn Y_MIN(PB_0,PullUp); |
pruek | 0:e7aedcbababd | 47 | DigitalIn Y_MAX(PA_4,PullUp); |
pruek | 0:e7aedcbababd | 48 | |
pruek | 0:e7aedcbababd | 49 | struct FloatPoint { |
pruek | 0:e7aedcbababd | 50 | float x; |
pruek | 0:e7aedcbababd | 51 | float y; |
pruek | 0:e7aedcbababd | 52 | float sum; |
pruek | 0:e7aedcbababd | 53 | }; |
pruek | 0:e7aedcbababd | 54 | |
pruek | 0:e7aedcbababd | 55 | FloatPoint current_steps; |
pruek | 0:e7aedcbababd | 56 | FloatPoint target_steps; |
pruek | 0:e7aedcbababd | 57 | FloatPoint delta_steps; |
pruek | 0:e7aedcbababd | 58 | |
pruek | 0:e7aedcbababd | 59 | FloatPoint current_mm; |
pruek | 0:e7aedcbababd | 60 | FloatPoint target_mm; |
pruek | 0:e7aedcbababd | 61 | FloatPoint delta_mm; |
pruek | 0:e7aedcbababd | 62 | |
pruek | 0:e7aedcbababd | 63 | FloatPoint fp; |
pruek | 0:e7aedcbababd | 64 | FloatPoint MAX; |
pruek | 0:e7aedcbababd | 65 | FloatPoint MIN; |
pruek | 0:e7aedcbababd | 66 | |
pruek | 0:e7aedcbababd | 67 | |
pruek | 0:e7aedcbababd | 68 | |
pruek | 0:e7aedcbababd | 69 | float z; |
pruek | 0:e7aedcbababd | 70 | float curve_section = CURVE_SECTION_MM; |
pruek | 0:e7aedcbababd | 71 | int Set = 0; |
pruek | 0:e7aedcbababd | 72 | bool x_direction; |
pruek | 0:e7aedcbababd | 73 | bool y_direction; |
pruek | 0:e7aedcbababd | 74 | |
pruek | 0:e7aedcbababd | 75 | long max_delta; |
pruek | 0:e7aedcbababd | 76 | long x_counter; |
pruek | 0:e7aedcbababd | 77 | long y_counter; |
pruek | 0:e7aedcbababd | 78 | |
pruek | 0:e7aedcbababd | 79 | int Mode_G = 0; |
pruek | 0:e7aedcbababd | 80 | int Mode_$ = 0; |
pruek | 0:e7aedcbababd | 81 | int SIZE = 0; |
pruek | 0:e7aedcbababd | 82 | int s; |
pruek | 0:e7aedcbababd | 83 | int state_pen; |
pruek | 0:e7aedcbababd | 84 | float i,j; |
pruek | 0:e7aedcbababd | 85 | |
pruek | 0:e7aedcbababd | 86 | int serch(char ch,string cmd); |
pruek | 0:e7aedcbababd | 87 | void Taonoi_Begin (); |
pruek | 0:e7aedcbababd | 88 | bool Taonoican_Step_x(long current, long target, int8_t direction_x); |
pruek | 0:e7aedcbababd | 89 | bool Taonoican_Step_y(long current, long target, int8_t direction_y); |
pruek | 0:e7aedcbababd | 90 | void Taonoi_Move(); |
pruek | 0:e7aedcbababd | 91 | void calculate_deltas(); |
pruek | 0:e7aedcbababd | 92 | void Curve (float i, float j); |
pruek | 0:e7aedcbababd | 93 | void Taonoi_Square(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN); |
pruek | 0:e7aedcbababd | 94 | void Taonoi_Around(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN); |
pruek | 0:e7aedcbababd | 95 | void Taonoi_Ellipse(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN); |
pruek | 0:e7aedcbababd | 96 | void beatup(); |
pruek | 0:e7aedcbababd | 97 | void percent_coundown(); |
pruek | 0:e7aedcbababd | 98 | |
pruek | 0:e7aedcbababd | 99 | |
pruek | 0:e7aedcbababd | 100 | int main() |
pruek | 0:e7aedcbababd | 101 | { |
pruek | 0:e7aedcbababd | 102 | pc.baud(115200); |
pruek | 0:e7aedcbababd | 103 | Nucleo.baud(9600); |
pruek | 0:e7aedcbababd | 104 | pc.printf("Hello\n"); |
pruek | 0:e7aedcbababd | 105 | string data ; |
pruek | 0:e7aedcbababd | 106 | |
pruek | 0:e7aedcbababd | 107 | Taonoi_Begin(); |
pruek | 0:e7aedcbababd | 108 | //Taonoi_Around(50,0,70,0); |
pruek | 0:e7aedcbababd | 109 | //Taonoi_Square(50,0,70,0); |
pruek | 0:e7aedcbababd | 110 | //Taonoi_Oval(50,0,100,30); |
pruek | 0:e7aedcbababd | 111 | MAX.x = 0; |
pruek | 0:e7aedcbababd | 112 | MAX.y = 0; |
pruek | 0:e7aedcbababd | 113 | MIN.x = 200; |
pruek | 0:e7aedcbababd | 114 | MIN.x = 200; |
pruek | 0:e7aedcbababd | 115 | thistime.gettime( &second, &minute, &hour, &day, &date, &month, &year); |
pruek | 0:e7aedcbababd | 116 | display.write(0, hour / 10); |
pruek | 0:e7aedcbababd | 117 | display.write(1, hour % 10); |
pruek | 0:e7aedcbababd | 118 | display.write(2, minute / 10); |
pruek | 0:e7aedcbababd | 119 | display.write(3, minute % 10); |
pruek | 0:e7aedcbababd | 120 | display.setColon(true); |
pruek | 0:e7aedcbababd | 121 | tick.attach(&beatup, 0.5); |
pruek | 0:e7aedcbababd | 122 | |
pruek | 0:e7aedcbababd | 123 | while(1) { |
pruek | 0:e7aedcbababd | 124 | fp.x = 0.0; |
pruek | 0:e7aedcbababd | 125 | fp.y = 0.0; |
pruek | 0:e7aedcbababd | 126 | |
pruek | 0:e7aedcbababd | 127 | if(Nucleo.readable()) { |
pruek | 0:e7aedcbababd | 128 | char ch = Nucleo.getc(); |
pruek | 0:e7aedcbababd | 129 | if(ch!='\r') { |
pruek | 0:e7aedcbababd | 130 | pc.putc(ch); |
pruek | 0:e7aedcbababd | 131 | data.push_back(ch); |
pruek | 0:e7aedcbababd | 132 | } else { |
pruek | 0:e7aedcbababd | 133 | if(data.find('G') != std::string::npos) { |
pruek | 0:e7aedcbababd | 134 | Mode_G = serch('G',data); |
pruek | 0:e7aedcbababd | 135 | } |
pruek | 0:e7aedcbababd | 136 | if(data.find('$') != std::string::npos) { |
pruek | 0:e7aedcbababd | 137 | Mode_$ = serch('$',data); |
pruek | 0:e7aedcbababd | 138 | } |
pruek | 0:e7aedcbababd | 139 | if(data.find('X') != std::string::npos) { |
pruek | 0:e7aedcbababd | 140 | //target_steps.x = serch('X',data); |
pruek | 0:e7aedcbababd | 141 | target_mm.x = serch('X',data); |
pruek | 0:e7aedcbababd | 142 | fp.x = target_mm.x; |
pruek | 0:e7aedcbababd | 143 | } |
pruek | 0:e7aedcbababd | 144 | if(data.find('Y') != std::string::npos) { |
pruek | 0:e7aedcbababd | 145 | target_mm.y = serch('Y',data); |
pruek | 0:e7aedcbababd | 146 | fp.y = target_mm.y; |
pruek | 0:e7aedcbababd | 147 | } |
pruek | 0:e7aedcbababd | 148 | if(data.find('Z') != std::string::npos) { |
pruek | 0:e7aedcbababd | 149 | z = serch('Z',data); |
pruek | 0:e7aedcbababd | 150 | } |
pruek | 0:e7aedcbababd | 151 | if(data.find('I') != std::string::npos) { |
pruek | 0:e7aedcbababd | 152 | i = serch('I',data); |
pruek | 0:e7aedcbababd | 153 | } |
pruek | 0:e7aedcbababd | 154 | |
pruek | 0:e7aedcbababd | 155 | if(data.find('J') != std::string::npos) { |
pruek | 0:e7aedcbababd | 156 | j = serch('J',data); |
pruek | 0:e7aedcbababd | 157 | } |
pruek | 0:e7aedcbababd | 158 | if(data.find('S') != std::string::npos) { |
pruek | 0:e7aedcbababd | 159 | s = serch('S',data); |
pruek | 0:e7aedcbababd | 160 | if(s==1) |
pruek | 0:e7aedcbababd | 161 | SIZE = 0; |
pruek | 0:e7aedcbababd | 162 | else if (s==2) |
pruek | 0:e7aedcbababd | 163 | SIZE = 20; |
pruek | 0:e7aedcbababd | 164 | else if (s==3) |
pruek | 0:e7aedcbababd | 165 | SIZE = 40; |
pruek | 0:e7aedcbababd | 166 | } |
pruek | 0:e7aedcbababd | 167 | data.clear(); |
pruek | 0:e7aedcbababd | 168 | if(Mode_G == 2 || Mode_G == 3) { |
pruek | 0:e7aedcbababd | 169 | Pen_1 = z; |
pruek | 0:e7aedcbababd | 170 | if(state_pen == 1 && z == 0) |
pruek | 0:e7aedcbababd | 171 | wait(0.5); |
pruek | 0:e7aedcbababd | 172 | else if (state_pen == 0 && z == 1) |
pruek | 0:e7aedcbababd | 173 | wait(0.5); |
pruek | 0:e7aedcbababd | 174 | state_pen = z; |
pruek | 0:e7aedcbababd | 175 | Curve(i,j); |
pruek | 0:e7aedcbababd | 176 | Nucleo.putc('#'); |
pruek | 0:e7aedcbababd | 177 | Mode_G = 0; |
pruek | 0:e7aedcbababd | 178 | } else { |
pruek | 0:e7aedcbababd | 179 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 180 | Pen_1 = z; |
pruek | 0:e7aedcbababd | 181 | if(state_pen == 1 && z == 0) |
pruek | 0:e7aedcbababd | 182 | wait(0.5); |
pruek | 0:e7aedcbababd | 183 | else if (state_pen == 0 && z == 1) |
pruek | 0:e7aedcbababd | 184 | wait(0.5); |
pruek | 0:e7aedcbababd | 185 | state_pen = z; |
pruek | 0:e7aedcbababd | 186 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 187 | Nucleo.putc('#'); |
pruek | 0:e7aedcbababd | 188 | } |
pruek | 0:e7aedcbababd | 189 | if(Mode_$ == 1) { |
pruek | 0:e7aedcbababd | 190 | Taonoi_Square(MAX.x,MIN.x,MAX.y,MIN.y); |
pruek | 0:e7aedcbababd | 191 | Nucleo.putc('$'); |
pruek | 0:e7aedcbababd | 192 | Mode_$ = 0; |
pruek | 0:e7aedcbababd | 193 | } else if(Mode_$ == 2) { |
pruek | 0:e7aedcbababd | 194 | Taonoi_Around(MAX.x,MIN.x,MAX.y,MIN.y); |
pruek | 0:e7aedcbababd | 195 | Nucleo.putc('$'); |
pruek | 0:e7aedcbababd | 196 | Mode_$ = 0; |
pruek | 0:e7aedcbababd | 197 | } else if(Mode_$ == 3) { |
pruek | 0:e7aedcbababd | 198 | Taonoi_Ellipse(MAX.x,MIN.x,MAX.y,MIN.y); |
pruek | 0:e7aedcbababd | 199 | Nucleo.putc('$'); |
pruek | 0:e7aedcbababd | 200 | Mode_$ = 0; |
pruek | 0:e7aedcbababd | 201 | |
pruek | 0:e7aedcbababd | 202 | } |
pruek | 0:e7aedcbababd | 203 | } |
pruek | 0:e7aedcbababd | 204 | } |
pruek | 0:e7aedcbababd | 205 | |
pruek | 0:e7aedcbababd | 206 | } |
pruek | 0:e7aedcbababd | 207 | } |
pruek | 0:e7aedcbababd | 208 | |
pruek | 0:e7aedcbababd | 209 | int serch(char ch,string cmd) |
pruek | 0:e7aedcbababd | 210 | { |
pruek | 0:e7aedcbababd | 211 | size_t pos = cmd.find(ch); |
pruek | 0:e7aedcbababd | 212 | size_t epos = cmd.find(' '); |
pruek | 0:e7aedcbababd | 213 | string temp = cmd.substr(pos+1,epos-pos); |
pruek | 0:e7aedcbababd | 214 | //pc.printf("\n===: %s\n\n",temp.c_str()); |
pruek | 0:e7aedcbababd | 215 | return atoi(temp.c_str()); |
pruek | 0:e7aedcbababd | 216 | |
pruek | 0:e7aedcbababd | 217 | } |
pruek | 0:e7aedcbababd | 218 | void Taonoi_Begin() |
pruek | 0:e7aedcbababd | 219 | { |
pruek | 0:e7aedcbababd | 220 | Pen_1 = 1; |
pruek | 0:e7aedcbababd | 221 | target_mm.x = -400; |
pruek | 0:e7aedcbababd | 222 | target_mm.y = -400; |
pruek | 0:e7aedcbababd | 223 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 224 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 225 | current_steps.x = 0; |
pruek | 0:e7aedcbababd | 226 | current_steps.y = 0; |
pruek | 0:e7aedcbababd | 227 | target_mm.x = 50;//50; |
pruek | 0:e7aedcbababd | 228 | target_mm.y = 100;//100; |
pruek | 0:e7aedcbababd | 229 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 230 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 231 | current_steps.x = 0; |
pruek | 0:e7aedcbababd | 232 | current_steps.y = 0; |
pruek | 0:e7aedcbababd | 233 | target_mm.x = 0; |
pruek | 0:e7aedcbababd | 234 | target_mm.y = 0; |
pruek | 0:e7aedcbababd | 235 | target_steps.x = 0; |
pruek | 0:e7aedcbababd | 236 | target_steps.y = 0; |
pruek | 0:e7aedcbababd | 237 | Set = 1; |
pruek | 0:e7aedcbababd | 238 | } |
pruek | 0:e7aedcbababd | 239 | bool Taonoican_Step_x(long current, long target, int8_t direction_x) |
pruek | 0:e7aedcbababd | 240 | { |
pruek | 0:e7aedcbababd | 241 | if (target == current) |
pruek | 0:e7aedcbababd | 242 | return false; |
pruek | 0:e7aedcbababd | 243 | //stop us if we're at home and still going |
pruek | 0:e7aedcbababd | 244 | else if (!X_MIN.read() && !direction_x) |
pruek | 0:e7aedcbababd | 245 | return false; |
pruek | 0:e7aedcbababd | 246 | //stop us if we're at max and still going |
pruek | 0:e7aedcbababd | 247 | else if (!X_MAX.read() && direction_x) |
pruek | 0:e7aedcbababd | 248 | return false; |
pruek | 0:e7aedcbababd | 249 | |
pruek | 0:e7aedcbababd | 250 | //default to being able to step |
pruek | 0:e7aedcbababd | 251 | return true; |
pruek | 0:e7aedcbababd | 252 | } |
pruek | 0:e7aedcbababd | 253 | |
pruek | 0:e7aedcbababd | 254 | bool Taonoican_Step_y(long current, long target, int8_t direction_y) |
pruek | 0:e7aedcbababd | 255 | { |
pruek | 0:e7aedcbababd | 256 | if (target == current) |
pruek | 0:e7aedcbababd | 257 | return false; |
pruek | 0:e7aedcbababd | 258 | //stop us if we're at home and still going |
pruek | 0:e7aedcbababd | 259 | else if (!Y_MIN.read() && !direction_y) |
pruek | 0:e7aedcbababd | 260 | return false; |
pruek | 0:e7aedcbababd | 261 | //stop us if we're at max and still going |
pruek | 0:e7aedcbababd | 262 | else if (!Y_MAX.read() && direction_y) |
pruek | 0:e7aedcbababd | 263 | return false; |
pruek | 0:e7aedcbababd | 264 | |
pruek | 0:e7aedcbababd | 265 | //default to being able to step |
pruek | 0:e7aedcbababd | 266 | return true; |
pruek | 0:e7aedcbababd | 267 | } |
pruek | 0:e7aedcbababd | 268 | |
pruek | 0:e7aedcbababd | 269 | void Taonoi_Move() |
pruek | 0:e7aedcbababd | 270 | { |
pruek | 0:e7aedcbababd | 271 | //figure out our deltas |
pruek | 0:e7aedcbababd | 272 | max_delta = delta_steps.x > delta_steps.y ? delta_steps.x : delta_steps.y; |
pruek | 0:e7aedcbababd | 273 | //init stuff. |
pruek | 0:e7aedcbababd | 274 | long x_counter = -max_delta / 2; |
pruek | 0:e7aedcbababd | 275 | long y_counter = -max_delta / 2; |
pruek | 0:e7aedcbababd | 276 | |
pruek | 0:e7aedcbababd | 277 | //our step flags |
pruek | 0:e7aedcbababd | 278 | bool x_can_step = 0; |
pruek | 0:e7aedcbababd | 279 | bool y_can_step = 0; |
pruek | 0:e7aedcbababd | 280 | |
pruek | 0:e7aedcbababd | 281 | int A = 0; |
pruek | 0:e7aedcbababd | 282 | int B = 0; |
pruek | 0:e7aedcbababd | 283 | |
pruek | 0:e7aedcbababd | 284 | do { |
pruek | 0:e7aedcbababd | 285 | x_can_step = Taonoican_Step_x(current_steps.x, target_steps.x, x_direction); |
pruek | 0:e7aedcbababd | 286 | y_can_step = Taonoican_Step_y(current_steps.y, target_steps.y, y_direction); |
pruek | 0:e7aedcbababd | 287 | if (x_can_step) { |
pruek | 0:e7aedcbababd | 288 | x_counter += delta_steps.x; |
pruek | 0:e7aedcbababd | 289 | |
pruek | 0:e7aedcbababd | 290 | if (x_counter > 0) { |
pruek | 0:e7aedcbababd | 291 | x.step(1,x_direction,SPEED_X,ACCEL_OFF); |
pruek | 0:e7aedcbababd | 292 | x_counter -= max_delta; |
pruek | 0:e7aedcbababd | 293 | |
pruek | 0:e7aedcbababd | 294 | if (x_direction) |
pruek | 0:e7aedcbababd | 295 | current_steps.x++; |
pruek | 0:e7aedcbababd | 296 | else |
pruek | 0:e7aedcbababd | 297 | current_steps.x--; |
pruek | 0:e7aedcbababd | 298 | } |
pruek | 0:e7aedcbababd | 299 | else |
pruek | 0:e7aedcbababd | 300 | A++; |
pruek | 0:e7aedcbababd | 301 | } |
pruek | 0:e7aedcbababd | 302 | |
pruek | 0:e7aedcbababd | 303 | if (y_can_step) { |
pruek | 0:e7aedcbababd | 304 | y_counter += delta_steps.y; |
pruek | 0:e7aedcbababd | 305 | |
pruek | 0:e7aedcbababd | 306 | if (y_counter > 0) { |
pruek | 0:e7aedcbababd | 307 | y.step(1,y_direction,SPEED_Y,ACCEL_OFF); |
pruek | 0:e7aedcbababd | 308 | y_counter -= max_delta; |
pruek | 0:e7aedcbababd | 309 | |
pruek | 0:e7aedcbababd | 310 | if (y_direction) |
pruek | 0:e7aedcbababd | 311 | current_steps.y++; |
pruek | 0:e7aedcbababd | 312 | else |
pruek | 0:e7aedcbababd | 313 | current_steps.y--; |
pruek | 0:e7aedcbababd | 314 | } |
pruek | 0:e7aedcbababd | 315 | else |
pruek | 0:e7aedcbababd | 316 | B++; |
pruek | 0:e7aedcbababd | 317 | } |
pruek | 0:e7aedcbababd | 318 | if(A > 10000 || B>10000) |
pruek | 0:e7aedcbababd | 319 | break; |
pruek | 0:e7aedcbababd | 320 | |
pruek | 0:e7aedcbababd | 321 | //wait for next step. |
pruek | 0:e7aedcbababd | 322 | } while (x_can_step || y_can_step); |
pruek | 0:e7aedcbababd | 323 | //set our points to be the same |
pruek | 0:e7aedcbababd | 324 | current_steps.x = target_steps.x; |
pruek | 0:e7aedcbababd | 325 | current_steps.y = target_steps.y; |
pruek | 0:e7aedcbababd | 326 | current_mm.x = target_mm.x; |
pruek | 0:e7aedcbababd | 327 | current_mm.y = target_mm.y; |
pruek | 0:e7aedcbababd | 328 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 329 | } |
pruek | 0:e7aedcbababd | 330 | void calculate_deltas() |
pruek | 0:e7aedcbababd | 331 | { |
pruek | 0:e7aedcbababd | 332 | delta_mm.x = abs(target_mm.x - current_mm.x); |
pruek | 0:e7aedcbababd | 333 | delta_mm.y = abs(target_mm.y - current_mm.y); |
pruek | 0:e7aedcbababd | 334 | |
pruek | 0:e7aedcbababd | 335 | target_steps.x = X_STEPS_PER_MM * target_mm.x; |
pruek | 0:e7aedcbababd | 336 | target_steps.y = Y_STEPS_PER_MM * target_mm.y; |
pruek | 0:e7aedcbababd | 337 | if(Set == 1) { |
pruek | 0:e7aedcbababd | 338 | if(target_mm.x > MAX.x) |
pruek | 0:e7aedcbababd | 339 | MAX.x = target_mm.x; |
pruek | 0:e7aedcbababd | 340 | if(target_mm.x < MIN.x) |
pruek | 0:e7aedcbababd | 341 | MIN.x = target_mm.x; |
pruek | 0:e7aedcbababd | 342 | if(target_mm.y > MAX.y) |
pruek | 0:e7aedcbababd | 343 | MAX.y = target_mm.y; |
pruek | 0:e7aedcbababd | 344 | if(target_mm.y < MIN.y) |
pruek | 0:e7aedcbababd | 345 | MIN.y = target_mm.y; |
pruek | 0:e7aedcbababd | 346 | } |
pruek | 0:e7aedcbababd | 347 | |
pruek | 0:e7aedcbababd | 348 | |
pruek | 0:e7aedcbababd | 349 | delta_steps.x = abs(target_steps.x - current_steps.x); |
pruek | 0:e7aedcbababd | 350 | delta_steps.y = abs(target_steps.y - current_steps.y); |
pruek | 0:e7aedcbababd | 351 | |
pruek | 0:e7aedcbababd | 352 | x_direction = (target_steps.x >= current_steps.x); |
pruek | 0:e7aedcbababd | 353 | y_direction = (target_steps.y >= current_steps.y); |
pruek | 0:e7aedcbababd | 354 | } |
pruek | 0:e7aedcbababd | 355 | |
pruek | 0:e7aedcbababd | 356 | void Curve (float i, float j) |
pruek | 0:e7aedcbababd | 357 | { |
pruek | 0:e7aedcbababd | 358 | FloatPoint cent; |
pruek | 0:e7aedcbababd | 359 | cent.x = i+current_mm.x; |
pruek | 0:e7aedcbababd | 360 | cent.y = j+current_mm.y; |
pruek | 0:e7aedcbababd | 361 | |
pruek | 0:e7aedcbababd | 362 | // Centre coordinates are always relative |
pruek | 0:e7aedcbababd | 363 | float angleA, angleB, angle, radius, length, aX, aY, bX, bY; |
pruek | 0:e7aedcbababd | 364 | |
pruek | 0:e7aedcbababd | 365 | aX = (current_mm.x - cent.x); |
pruek | 0:e7aedcbababd | 366 | aY = (current_mm.y - cent.y); |
pruek | 0:e7aedcbababd | 367 | bX = (fp.x - cent.x); |
pruek | 0:e7aedcbababd | 368 | bY = (fp.y - cent.y); |
pruek | 0:e7aedcbababd | 369 | |
pruek | 0:e7aedcbababd | 370 | if (Mode_G == 2) { // Clockwise |
pruek | 0:e7aedcbababd | 371 | angleA = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 372 | angleB = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 373 | } else { // Counterclockwise |
pruek | 0:e7aedcbababd | 374 | angleA = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 375 | angleB = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 376 | } |
pruek | 0:e7aedcbababd | 377 | |
pruek | 0:e7aedcbababd | 378 | // Make sure angleB is always greater than angleA |
pruek | 0:e7aedcbababd | 379 | // and if not add 2PI so that it is (this also takes |
pruek | 0:e7aedcbababd | 380 | // care of the special case of angleA == angleB, |
pruek | 0:e7aedcbababd | 381 | // ie we want a complete circle) |
pruek | 0:e7aedcbababd | 382 | if (angleB <= angleA) angleB += 2 * PI; |
pruek | 0:e7aedcbababd | 383 | angle = angleB - angleA; |
pruek | 0:e7aedcbababd | 384 | |
pruek | 0:e7aedcbababd | 385 | radius = sqrt(aX * aX + aY * aY); |
pruek | 0:e7aedcbababd | 386 | length = radius * angle; |
pruek | 0:e7aedcbababd | 387 | int steps, s, step; |
pruek | 0:e7aedcbababd | 388 | steps = (int) ceil(length / curve_section); |
pruek | 0:e7aedcbababd | 389 | |
pruek | 0:e7aedcbababd | 390 | //FloatPoint newPoint; |
pruek | 0:e7aedcbababd | 391 | for (s = 1; s <= steps; s++) { |
pruek | 0:e7aedcbababd | 392 | step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW |
pruek | 0:e7aedcbababd | 393 | target_mm.x = cent.x + radius * cos(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 394 | target_mm.y = cent.y + radius * sin(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 395 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 396 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 397 | } |
pruek | 0:e7aedcbababd | 398 | } |
pruek | 0:e7aedcbababd | 399 | |
pruek | 0:e7aedcbababd | 400 | void Curve_Ex (float i, float j) |
pruek | 0:e7aedcbababd | 401 | { |
pruek | 0:e7aedcbababd | 402 | FloatPoint cent; |
pruek | 0:e7aedcbababd | 403 | cent.x = i+current_mm.x; |
pruek | 0:e7aedcbababd | 404 | cent.y = j+current_mm.y; |
pruek | 0:e7aedcbababd | 405 | |
pruek | 0:e7aedcbababd | 406 | // Centre coordinates are always relative |
pruek | 0:e7aedcbababd | 407 | float angleA, angleB, angle, radius, length, aX, aY, bX, bY; |
pruek | 0:e7aedcbababd | 408 | |
pruek | 0:e7aedcbababd | 409 | aX = (current_mm.x - cent.x); |
pruek | 0:e7aedcbababd | 410 | aY = (current_mm.y - cent.y); |
pruek | 0:e7aedcbababd | 411 | bX = (fp.x - cent.x); |
pruek | 0:e7aedcbababd | 412 | bY = (fp.y - cent.y); |
pruek | 0:e7aedcbababd | 413 | |
pruek | 0:e7aedcbababd | 414 | if (Mode_G == 2) { // Clockwise |
pruek | 0:e7aedcbababd | 415 | angleA = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 416 | angleB = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 417 | } else { // Counterclockwise |
pruek | 0:e7aedcbababd | 418 | angleA = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 419 | angleB = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 420 | } |
pruek | 0:e7aedcbababd | 421 | |
pruek | 0:e7aedcbababd | 422 | // Make sure angleB is always greater than angleA |
pruek | 0:e7aedcbababd | 423 | // and if not add 2PI so that it is (this also takes |
pruek | 0:e7aedcbababd | 424 | // care of the special case of angleA == angleB, |
pruek | 0:e7aedcbababd | 425 | // ie we want a complete circle) |
pruek | 0:e7aedcbababd | 426 | if (angleB <= angleA) angleB += 2 * PI; |
pruek | 0:e7aedcbababd | 427 | angle = angleB - angleA; |
pruek | 0:e7aedcbababd | 428 | |
pruek | 0:e7aedcbababd | 429 | radius = sqrt(aX * aX + aY * aY); |
pruek | 0:e7aedcbababd | 430 | length = radius * angle; |
pruek | 0:e7aedcbababd | 431 | int steps, s, step; |
pruek | 0:e7aedcbababd | 432 | steps = (int) ceil(length / curve_section); |
pruek | 0:e7aedcbababd | 433 | |
pruek | 0:e7aedcbababd | 434 | //FloatPoint newPoint; |
pruek | 0:e7aedcbababd | 435 | for (s = 1; s <= steps; s++) { |
pruek | 0:e7aedcbababd | 436 | step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW |
pruek | 0:e7aedcbababd | 437 | target_mm.x = cent.x + 2*radius * cos(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 438 | target_mm.y = cent.y + radius * sin(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 439 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 440 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 441 | } |
pruek | 0:e7aedcbababd | 442 | } |
pruek | 0:e7aedcbababd | 443 | |
pruek | 0:e7aedcbababd | 444 | void Curve_Ey (float i, float j) |
pruek | 0:e7aedcbababd | 445 | { |
pruek | 0:e7aedcbababd | 446 | FloatPoint cent; |
pruek | 0:e7aedcbababd | 447 | cent.x = i+current_mm.x; |
pruek | 0:e7aedcbababd | 448 | cent.y = j+current_mm.y; |
pruek | 0:e7aedcbababd | 449 | |
pruek | 0:e7aedcbababd | 450 | // Centre coordinates are always relative |
pruek | 0:e7aedcbababd | 451 | float angleA, angleB, angle, radius, length, aX, aY, bX, bY; |
pruek | 0:e7aedcbababd | 452 | |
pruek | 0:e7aedcbababd | 453 | aX = (current_mm.x - cent.x); |
pruek | 0:e7aedcbababd | 454 | aY = (current_mm.y - cent.y); |
pruek | 0:e7aedcbababd | 455 | bX = (fp.x - cent.x); |
pruek | 0:e7aedcbababd | 456 | bY = (fp.y - cent.y); |
pruek | 0:e7aedcbababd | 457 | |
pruek | 0:e7aedcbababd | 458 | if (Mode_G == 2) { // Clockwise |
pruek | 0:e7aedcbababd | 459 | angleA = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 460 | angleB = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 461 | } else { // Counterclockwise |
pruek | 0:e7aedcbababd | 462 | angleA = atan2(aY, aX); |
pruek | 0:e7aedcbababd | 463 | angleB = atan2(bY, bX); |
pruek | 0:e7aedcbababd | 464 | } |
pruek | 0:e7aedcbababd | 465 | |
pruek | 0:e7aedcbababd | 466 | // Make sure angleB is always greater than angleA |
pruek | 0:e7aedcbababd | 467 | // and if not add 2PI so that it is (this also takes |
pruek | 0:e7aedcbababd | 468 | // care of the special case of angleA == angleB, |
pruek | 0:e7aedcbababd | 469 | // ie we want a complete circle) |
pruek | 0:e7aedcbababd | 470 | if (angleB <= angleA) angleB += 2 * PI; |
pruek | 0:e7aedcbababd | 471 | angle = angleB - angleA; |
pruek | 0:e7aedcbababd | 472 | |
pruek | 0:e7aedcbababd | 473 | radius = sqrt(aX * aX + aY * aY); |
pruek | 0:e7aedcbababd | 474 | length = radius * angle; |
pruek | 0:e7aedcbababd | 475 | int steps, s, step; |
pruek | 0:e7aedcbababd | 476 | steps = (int) ceil(length / curve_section); |
pruek | 0:e7aedcbababd | 477 | |
pruek | 0:e7aedcbababd | 478 | //FloatPoint newPoint; |
pruek | 0:e7aedcbababd | 479 | for (s = 1; s <= steps; s++) { |
pruek | 0:e7aedcbababd | 480 | step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW |
pruek | 0:e7aedcbababd | 481 | target_mm.x = cent.x + radius * cos(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 482 | target_mm.y = cent.y + 2*radius * sin(angleA + angle * ((float) step / steps)); |
pruek | 0:e7aedcbababd | 483 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 484 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 485 | } |
pruek | 0:e7aedcbababd | 486 | } |
pruek | 0:e7aedcbababd | 487 | |
pruek | 0:e7aedcbababd | 488 | void Taonoi_Square(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN) |
pruek | 0:e7aedcbababd | 489 | { |
pruek | 0:e7aedcbababd | 490 | float Delta_X = abs(X_MAX - X_MIN); |
pruek | 0:e7aedcbababd | 491 | float Delta_Y = abs(Y_MAX - Y_MIN); |
pruek | 0:e7aedcbababd | 492 | int Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y); |
pruek | 0:e7aedcbababd | 493 | float Cen_X = (X_MAX + X_MIN) / 2; |
pruek | 0:e7aedcbababd | 494 | float Cen_Y = (Y_MAX + Y_MIN) / 2; |
pruek | 0:e7aedcbababd | 495 | Coundown = ceil((Max/5)*(Max/5+1)*(0.115)); |
pruek | 0:e7aedcbababd | 496 | target_mm.x = Cen_X; |
pruek | 0:e7aedcbababd | 497 | target_mm.y = Cen_Y; |
pruek | 0:e7aedcbababd | 498 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 499 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 500 | Pen_1 = 0; |
pruek | 0:e7aedcbababd | 501 | //percent_coundown(Coundown); |
pruek | 0:e7aedcbababd | 502 | //timer.start(); |
pruek | 0:e7aedcbababd | 503 | for(int i = 0; i < Max + 4 + SIZE; i += 5) { |
pruek | 0:e7aedcbababd | 504 | if(i % 2 != 0) { |
pruek | 0:e7aedcbababd | 505 | target_mm.x = current_mm.x + i; |
pruek | 0:e7aedcbababd | 506 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 507 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 508 | target_mm.y = current_mm.y + i; |
pruek | 0:e7aedcbababd | 509 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 510 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 511 | } else { |
pruek | 0:e7aedcbababd | 512 | target_mm.x = current_mm.x - i; |
pruek | 0:e7aedcbababd | 513 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 514 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 515 | target_mm.y = current_mm.y - i; |
pruek | 0:e7aedcbababd | 516 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 517 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 518 | } |
pruek | 0:e7aedcbababd | 519 | |
pruek | 0:e7aedcbababd | 520 | } |
pruek | 0:e7aedcbababd | 521 | Pen_1 = 1; |
pruek | 0:e7aedcbababd | 522 | for(int k=0; k<5; k++) { |
pruek | 0:e7aedcbababd | 523 | buzz=1; |
pruek | 0:e7aedcbababd | 524 | wait(0.5); |
pruek | 0:e7aedcbababd | 525 | buzz=0; |
pruek | 0:e7aedcbababd | 526 | } |
pruek | 0:e7aedcbababd | 527 | |
pruek | 0:e7aedcbababd | 528 | MAX.x = MAX.y = 0; |
pruek | 0:e7aedcbababd | 529 | MAX.x = MAX.y = 200; |
pruek | 0:e7aedcbababd | 530 | } |
pruek | 0:e7aedcbababd | 531 | |
pruek | 0:e7aedcbababd | 532 | void Taonoi_Around(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN) |
pruek | 0:e7aedcbababd | 533 | { |
pruek | 0:e7aedcbababd | 534 | float Delta_X = abs(X_MAX - X_MIN); |
pruek | 0:e7aedcbababd | 535 | float Delta_Y = abs(Y_MAX - Y_MIN); |
pruek | 0:e7aedcbababd | 536 | int Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y); |
pruek | 0:e7aedcbababd | 537 | float Cen_X = (X_MAX + X_MIN) / 2; |
pruek | 0:e7aedcbababd | 538 | float Cen_Y = (Y_MAX + Y_MIN) / 2; |
pruek | 0:e7aedcbababd | 539 | int R_max = Max/2+Curve_feq_MM; |
pruek | 0:e7aedcbababd | 540 | int Num_c = R_max/Curve_feq_MM+1; |
pruek | 0:e7aedcbababd | 541 | Coundown = ceil((12.5*Num_c+5*(Num_c-1)*(Num_c))*0.094); |
pruek | 0:e7aedcbababd | 542 | float R1 = Curve_feq_MM; |
pruek | 0:e7aedcbababd | 543 | float R2 = R1/2; |
pruek | 0:e7aedcbababd | 544 | //int Coundown = ceil((Max/5)*(Max/5+1)*(0.115)); |
pruek | 0:e7aedcbababd | 545 | target_mm.x = Cen_X; |
pruek | 0:e7aedcbababd | 546 | target_mm.y = Cen_Y; |
pruek | 0:e7aedcbababd | 547 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 548 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 549 | Pen_1 = 0; |
pruek | 0:e7aedcbababd | 550 | // percent_coundown(Coundown_c); |
pruek | 0:e7aedcbababd | 551 | target_mm.x = Cen_X + R1; |
pruek | 0:e7aedcbababd | 552 | target_mm.y = Cen_Y; |
pruek | 0:e7aedcbababd | 553 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 554 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 555 | do { |
pruek | 0:e7aedcbababd | 556 | Mode_G = 3; |
pruek | 0:e7aedcbababd | 557 | fp.x = Cen_X-R1; |
pruek | 0:e7aedcbababd | 558 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 559 | Curve(-R1,0); |
pruek | 0:e7aedcbababd | 560 | |
pruek | 0:e7aedcbababd | 561 | fp.x = Cen_X+R1+Curve_feq_MM; |
pruek | 0:e7aedcbababd | 562 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 563 | Curve(R1 + R2,0); |
pruek | 0:e7aedcbababd | 564 | |
pruek | 0:e7aedcbababd | 565 | |
pruek | 0:e7aedcbababd | 566 | R1 = R1 + Curve_feq_MM; |
pruek | 0:e7aedcbababd | 567 | } while(R1 <= R_max + SIZE); |
pruek | 0:e7aedcbababd | 568 | Pen_1 = 1; |
pruek | 0:e7aedcbababd | 569 | for(int k=0; k<5; k++) { |
pruek | 0:e7aedcbababd | 570 | buzz=1; |
pruek | 0:e7aedcbababd | 571 | wait(0.5); |
pruek | 0:e7aedcbababd | 572 | buzz=0; |
pruek | 0:e7aedcbababd | 573 | } |
pruek | 0:e7aedcbababd | 574 | |
pruek | 0:e7aedcbababd | 575 | Mode_G = 0; |
pruek | 0:e7aedcbababd | 576 | MAX.x = MAX.y =0; |
pruek | 0:e7aedcbababd | 577 | MIN.x = MIN.y =200; |
pruek | 0:e7aedcbababd | 578 | } |
pruek | 0:e7aedcbababd | 579 | |
pruek | 0:e7aedcbababd | 580 | void Taonoi_Ellipse(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN) |
pruek | 0:e7aedcbababd | 581 | { |
pruek | 0:e7aedcbababd | 582 | float Delta_X = abs(X_MAX - X_MIN); |
pruek | 0:e7aedcbababd | 583 | float Delta_Y = abs(Y_MAX - Y_MIN); |
pruek | 0:e7aedcbababd | 584 | int Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y); |
pruek | 0:e7aedcbababd | 585 | float Cen_X = (X_MAX + X_MIN) / 2; |
pruek | 0:e7aedcbababd | 586 | float Cen_Y = (Y_MAX + Y_MIN) / 2; |
pruek | 0:e7aedcbababd | 587 | int R_max = Max/2+Curve_feq_MM; |
pruek | 0:e7aedcbababd | 588 | int Num_c = R_max/Curve_feq_MM+1; |
pruek | 0:e7aedcbababd | 589 | Coundown = ceil((12.5*Num_c+5*(Num_c-1)*(Num_c))*0.094); |
pruek | 0:e7aedcbababd | 590 | float R1 = Curve_feq_MM; |
pruek | 0:e7aedcbababd | 591 | float R2 = R1/2; |
pruek | 0:e7aedcbababd | 592 | target_mm.x = Cen_X; |
pruek | 0:e7aedcbababd | 593 | target_mm.y = Cen_Y; |
pruek | 0:e7aedcbababd | 594 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 595 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 596 | Pen_1 = 0; |
pruek | 0:e7aedcbababd | 597 | //percent_coundown(Coundown_c); |
pruek | 0:e7aedcbababd | 598 | target_mm.x = Cen_X + R1; |
pruek | 0:e7aedcbababd | 599 | target_mm.y = Cen_Y; |
pruek | 0:e7aedcbababd | 600 | calculate_deltas(); |
pruek | 0:e7aedcbababd | 601 | Taonoi_Move(); |
pruek | 0:e7aedcbababd | 602 | do { |
pruek | 0:e7aedcbababd | 603 | Mode_G = 3; |
pruek | 0:e7aedcbababd | 604 | fp.x = Cen_X-R1; |
pruek | 0:e7aedcbababd | 605 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 606 | Curve_Ey(-R1,0); |
pruek | 0:e7aedcbababd | 607 | |
pruek | 0:e7aedcbababd | 608 | fp.x = Cen_X+R1+Curve_feq_MM; |
pruek | 0:e7aedcbababd | 609 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 610 | Curve_Ey(R1 + R2,0); |
pruek | 0:e7aedcbababd | 611 | |
pruek | 0:e7aedcbababd | 612 | |
pruek | 0:e7aedcbababd | 613 | R1 = R1 + Curve_feq_MM; |
pruek | 0:e7aedcbababd | 614 | } while(R1<=R_max + SIZE); |
pruek | 0:e7aedcbababd | 615 | Pen_1 = 1; |
pruek | 0:e7aedcbababd | 616 | for(int k=0; k<5; k++) { |
pruek | 0:e7aedcbababd | 617 | buzz=1; |
pruek | 0:e7aedcbababd | 618 | wait(0.5); |
pruek | 0:e7aedcbababd | 619 | buzz=0; |
pruek | 0:e7aedcbababd | 620 | } |
pruek | 0:e7aedcbababd | 621 | //timer.stop(); |
pruek | 0:e7aedcbababd | 622 | /*} else { |
pruek | 0:e7aedcbababd | 623 | do { |
pruek | 0:e7aedcbababd | 624 | Mode_G = 3; |
pruek | 0:e7aedcbababd | 625 | fp.x = Cen_X-2*R1; |
pruek | 0:e7aedcbababd | 626 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 627 | Curve_Ex(-R1,0); |
pruek | 0:e7aedcbababd | 628 | |
pruek | 0:e7aedcbababd | 629 | fp.x = Cen_X+2*R1+2*Curve_feq_MM; |
pruek | 0:e7aedcbababd | 630 | fp.y = Cen_Y; |
pruek | 0:e7aedcbababd | 631 | Curve_Ex(R1 + R2,0); |
pruek | 0:e7aedcbababd | 632 | |
pruek | 0:e7aedcbababd | 633 | |
pruek | 0:e7aedcbababd | 634 | R1 = R1 + Curve_feq_MM; |
pruek | 0:e7aedcbababd | 635 | } while(R1<=R_max + SIZE); |
pruek | 0:e7aedcbababd | 636 | Pen_1 = 1; |
pruek | 0:e7aedcbababd | 637 | Mode_G = 0; |
pruek | 0:e7aedcbababd | 638 | }*/ |
pruek | 0:e7aedcbababd | 639 | |
pruek | 0:e7aedcbababd | 640 | Mode_G = 0; |
pruek | 0:e7aedcbababd | 641 | MAX.x = MAX.y =0; |
pruek | 0:e7aedcbababd | 642 | MIN.x = MIN.y =200; |
pruek | 0:e7aedcbababd | 643 | } |
pruek | 0:e7aedcbababd | 644 | |
pruek | 0:e7aedcbababd | 645 | void beatup() |
pruek | 0:e7aedcbababd | 646 | { |
pruek | 0:e7aedcbababd | 647 | static uint8_t colon = 0; |
pruek | 0:e7aedcbababd | 648 | |
pruek | 0:e7aedcbababd | 649 | display.setColon(colon); |
pruek | 0:e7aedcbababd | 650 | if (colon) { |
pruek | 0:e7aedcbababd | 651 | second++; |
pruek | 0:e7aedcbababd | 652 | if (second >= 60) { |
pruek | 0:e7aedcbababd | 653 | second = 0; |
pruek | 0:e7aedcbababd | 654 | minute++; |
pruek | 0:e7aedcbababd | 655 | if (minute >= 60) { |
pruek | 0:e7aedcbababd | 656 | minute = 0; |
pruek | 0:e7aedcbababd | 657 | |
pruek | 0:e7aedcbababd | 658 | hour++; |
pruek | 0:e7aedcbababd | 659 | if (hour >= 24) { |
pruek | 0:e7aedcbababd | 660 | hour = 0; |
pruek | 0:e7aedcbababd | 661 | } |
pruek | 0:e7aedcbababd | 662 | display.write(0, hour / 10); |
pruek | 0:e7aedcbababd | 663 | display.write(1, hour % 10); |
pruek | 0:e7aedcbababd | 664 | } |
pruek | 0:e7aedcbababd | 665 | display.write(2, minute / 10); |
pruek | 0:e7aedcbababd | 666 | display.write(3, minute % 10); |
pruek | 0:e7aedcbababd | 667 | } |
pruek | 0:e7aedcbababd | 668 | } |
pruek | 0:e7aedcbababd | 669 | colon = 1 - colon; |
pruek | 0:e7aedcbababd | 670 | } |
pruek | 0:e7aedcbababd | 671 | |
pruek | 0:e7aedcbababd | 672 | /*void percent_coundown() |
pruek | 0:e7aedcbababd | 673 | static uint8_t colon = 0; |
pruek | 0:e7aedcbababd | 674 | int current_time = 0 ; |
pruek | 0:e7aedcbababd | 675 | int current_percent =0; |
pruek | 0:e7aedcbababd | 676 | display.setColon(colon); |
pruek | 0:e7aedcbababd | 677 | if (colon) { |
pruek | 0:e7aedcbababd | 678 | |
pruek | 0:e7aedcbababd | 679 | } |
pruek | 0:e7aedcbababd | 680 | colon = 1 - colon; |
pruek | 0:e7aedcbababd | 681 | } |
pruek | 0:e7aedcbababd | 682 | |
pruek | 0:e7aedcbababd | 683 | { |
pruek | 0:e7aedcbababd | 684 | int current_time = 0 ; |
pruek | 0:e7aedcbababd | 685 | int current_percent =0; |
pruek | 0:e7aedcbababd | 686 | display.write(0, 0); |
pruek | 0:e7aedcbababd | 687 | |
pruek | 0:e7aedcbababd | 688 | do { |
pruek | 0:e7aedcbababd | 689 | current_time=timer.read(); |
pruek | 0:e7aedcbababd | 690 | current_percent = current_time*100/time; |
pruek | 0:e7aedcbababd | 691 | display.write(1, current_percent/100); |
pruek | 0:e7aedcbababd | 692 | display.write(2, (current_percent%100)/10); |
pruek | 0:e7aedcbababd | 693 | display.write(3, current_percent%10); |
pruek | 0:e7aedcbababd | 694 | } while(time<=current_time); |
pruek | 0:e7aedcbababd | 695 | display.write(1, 1); |
pruek | 0:e7aedcbababd | 696 | display.write(2, 0); |
pruek | 0:e7aedcbababd | 697 | display.write(3, 0); |
pruek | 0:e7aedcbababd | 698 | for(int k=0; k<5; k++) { |
pruek | 0:e7aedcbababd | 699 | buzz=1; |
pruek | 0:e7aedcbababd | 700 | wait(5); |
pruek | 0:e7aedcbababd | 701 | buzz=0; |
pruek | 0:e7aedcbababd | 702 | } |
pruek | 0:e7aedcbababd | 703 | display.setColon(true); |
pruek | 0:e7aedcbababd | 704 | tick.attach(&beatup, 0.5); |
pruek | 0:e7aedcbababd | 705 | current_time=0; |
pruek | 0:e7aedcbababd | 706 | |
pruek | 0:e7aedcbababd | 707 | |
pruek | 0:e7aedcbababd | 708 | }*/ |
pruek | 0:e7aedcbababd | 709 | |
pruek | 0:e7aedcbababd | 710 |