FRA221_2015 / Mbed 2 deprecated 16A_Autopancakemaker

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?

UserRevisionLine numberNew 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