Ben Katz / rubix_controller

servodisc goodness

Dependencies:   mbed-dev-f303

cube.cpp@11:16d807d6b9c5, 2018-03-07 (annotated)

Committer:
benkatz
Date:
Wed Mar 07 19:25:49 2018 +0000
Revision:
11:16d807d6b9c5
Parent:
6:1143996ac690 
Seems to work;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 6:1143996ac690 1 #include <stdio.h>
benkatz 6:1143996ac690 2 #include <stdint.h>
benkatz 6:1143996ac690 3 #include <cstring> //cstring is stupid.
benkatz 6:1143996ac690 4 #include <string.h>
benkatz 6:1143996ac690 5
benkatz 6:1143996ac690 6 #include "cube.h"
benkatz 6:1143996ac690 7
benkatz 6:1143996ac690 8 char soln_moves[] = "UDLRFB";
benkatz 6:1143996ac690 9 sequence_t solve_sequence;
benkatz 6:1143996ac690 10 move_t moves[30];
benkatz 6:1143996ac690 11
benkatz 6:1143996ac690 12 // 0, ... ,5, UDLRFB
benkatz 6:1143996ac690 13 // 6, 2
benkatz 6:1143996ac690 14 // 7, '
benkatz 6:1143996ac690 15 // 8, <space>
benkatz 6:1143996ac690 16 // -1 other
benkatz 6:1143996ac690 17
benkatz 6:1143996ac690 18 int get_char_type(char a)
benkatz 6:1143996ac690 19 {
benkatz 6:1143996ac690 20 for(int i = 0; i < 6; i++)
benkatz 6:1143996ac690 21 if(soln_moves[i] == a)
benkatz 6:1143996ac690 22 return i;
benkatz 6:1143996ac690 23 if(a == '2')
benkatz 6:1143996ac690 24 return 6;
benkatz 6:1143996ac690 25 if(a == 39) // '
benkatz 6:1143996ac690 26 return 7;
benkatz 6:1143996ac690 27 if(a == ' ')
benkatz 6:1143996ac690 28 return 8;
benkatz 6:1143996ac690 29
benkatz 6:1143996ac690 30 printf("[ERROR] unrecognized character in solution %c (decimal %d)\r\n",a,a);
benkatz 6:1143996ac690 31 return -1;
benkatz 6:1143996ac690 32 }
benkatz 6:1143996ac690 33
benkatz 6:1143996ac690 34 void string_to_sequence(char* string)
benkatz 6:1143996ac690 35 {
benkatz 6:1143996ac690 36 int move = 0;
benkatz 6:1143996ac690 37
benkatz 6:1143996ac690 38 for(int i = 0; i < strlen(string); i++)
benkatz 6:1143996ac690 39 {
benkatz 6:1143996ac690 40 int type = get_char_type(string[i]);
benkatz 6:1143996ac690 41
benkatz 6:1143996ac690 42 // direction
benkatz 6:1143996ac690 43 if(i%3 == 0)
benkatz 6:1143996ac690 44 {
benkatz 6:1143996ac690 45 if(type>5 || type <0)
benkatz 6:1143996ac690 46 {
benkatz 6:1143996ac690 47 printf("[ERROR] Could not find direction for move %d! Got %c instead.\r\n",move,string[i]);
benkatz 6:1143996ac690 48 return;
benkatz 6:1143996ac690 49 }
benkatz 6:1143996ac690 50 moves[move].face = (face_t)type;
benkatz 6:1143996ac690 51 }
benkatz 6:1143996ac690 52
benkatz 6:1143996ac690 53 if(i%3 == 1)
benkatz 6:1143996ac690 54 {
benkatz 6:1143996ac690 55 if(type == 6)
benkatz 6:1143996ac690 56 moves[move].n_turns = 2;
benkatz 6:1143996ac690 57 else if(type == 7)
benkatz 6:1143996ac690 58 moves[move].n_turns = -1;
benkatz 6:1143996ac690 59 else if(type == 8)
benkatz 6:1143996ac690 60 moves[move].n_turns = 1;
benkatz 6:1143996ac690 61 else
benkatz 6:1143996ac690 62 {
benkatz 6:1143996ac690 63 printf("[ERROR] Not a valid move modifier for move %d: %c\r\n",move,string[i]);
benkatz 6:1143996ac690 64 return;
benkatz 6:1143996ac690 65 }
benkatz 6:1143996ac690 66 }
benkatz 6:1143996ac690 67
benkatz 6:1143996ac690 68 if(i%3 == 2)
benkatz 6:1143996ac690 69 {
benkatz 6:1143996ac690 70 if(type != 8)
benkatz 6:1143996ac690 71 {
benkatz 6:1143996ac690 72 printf("[ERROR] Got invalid space character for move %d: %c\r\n",move,string[i]);
benkatz 6:1143996ac690 73 return;
benkatz 6:1143996ac690 74 }
benkatz 6:1143996ac690 75 move++;
benkatz 6:1143996ac690 76 }
benkatz 6:1143996ac690 77 }
benkatz 6:1143996ac690 78
benkatz 6:1143996ac690 79 solve_sequence.n_moves = move;
benkatz 6:1143996ac690 80 solve_sequence.moves = moves;
benkatz 6:1143996ac690 81 }
benkatz 6:1143996ac690 82
benkatz 6:1143996ac690 83 sequence_t* get_sequence()
benkatz 6:1143996ac690 84 {
benkatz 6:1143996ac690 85 return &solve_sequence;
benkatz 6:1143996ac690 86 }
benkatz 6:1143996ac690 87
benkatz 6:1143996ac690 88 void print_sequence(sequence_t* seq)
benkatz 6:1143996ac690 89 {
benkatz 6:1143996ac690 90 printf("print sequence 0%x\r\n",seq);
benkatz 6:1143996ac690 91 int n_moves = seq->n_moves;
benkatz 6:1143996ac690 92 printf("Sequence with %d moves\r\n",n_moves);
benkatz 6:1143996ac690 93 for(int i = 0; i < n_moves; i++)
benkatz 6:1143996ac690 94 printf("\tMove face %c %d turns.\r\n",soln_moves[seq->moves[i].face],seq->moves[i].n_turns);
benkatz 6:1143996ac690 95 }
benkatz 6:1143996ac690 96
benkatz 6:1143996ac690 97 int sequence_to_serial(sequence_t* seq, uint8_t* buffer, int n_bytes)
benkatz 6:1143996ac690 98 {
benkatz 6:1143996ac690 99 if(n_bytes < seq->n_moves)
benkatz 6:1143996ac690 100 {
benkatz 6:1143996ac690 101 printf("SERIAL BUFFER TOO SMALL!\r\n");
benkatz 6:1143996ac690 102 return 1;
benkatz 6:1143996ac690 103 }
benkatz 6:1143996ac690 104
benkatz 6:1143996ac690 105 // i = 0
benkatz 6:1143996ac690 106 buffer[0] = K_START;
benkatz 6:1143996ac690 107 // i = 1
benkatz 6:1143996ac690 108 buffer[1] = K_MOVES + (seq->n_moves & 0x3f);
benkatz 6:1143996ac690 109 // i = 2 .. (n_moves+2)
benkatz 6:1143996ac690 110 for(int i = 0; i < seq->n_moves; i++)
benkatz 6:1143996ac690 111 buffer[i+2] = (0x7 & (seq->moves[i].n_turns)) + (0x38 & ((uint8_t)seq->moves[i].face<<3));
benkatz 6:1143996ac690 112 return seq->n_moves + 2;
benkatz 6:1143996ac690 113 }
benkatz 6:1143996ac690 114
benkatz 6:1143996ac690 115 int serial_to_sequence(sequence_t* seq, uint8_t* buffer, int n_bytes)
benkatz 6:1143996ac690 116 {
benkatz 6:1143996ac690 117 printf("[serial to sequence] seq: 0x%x, buf: 0x%x, n_bytes: %x\r\n",seq,buffer,n_bytes);
benkatz 6:1143996ac690 118 int start_index = -1;
benkatz 6:1143996ac690 119 int n_moves = -1;
benkatz 6:1143996ac690 120 for(int i = 0; i < n_bytes; i++)
benkatz 6:1143996ac690 121 {
benkatz 6:1143996ac690 122 if(buffer[i] == K_START)
benkatz 6:1143996ac690 123 {
benkatz 6:1143996ac690 124 start_index = i+2;
benkatz 6:1143996ac690 125 break;
benkatz 6:1143996ac690 126 }
benkatz 6:1143996ac690 127 }
benkatz 6:1143996ac690 128
benkatz 6:1143996ac690 129 if(start_index == -1)
benkatz 6:1143996ac690 130 {
benkatz 6:1143996ac690 131 printf("FAILED TO FIND START OF MOVE!\r\n");
benkatz 6:1143996ac690 132 return 1;
benkatz 6:1143996ac690 133 }
benkatz 6:1143996ac690 134
benkatz 6:1143996ac690 135 if((buffer[start_index-1] & 0xC0) != K_MOVES)
benkatz 6:1143996ac690 136 {
benkatz 6:1143996ac690 137 printf("COULD NOT FIND NUMBER OF MOVES!\r\n");
benkatz 6:1143996ac690 138 return 1;
benkatz 6:1143996ac690 139 }
benkatz 6:1143996ac690 140
benkatz 6:1143996ac690 141 n_moves = buffer[start_index-1]&0x3f;
benkatz 6:1143996ac690 142 seq->n_moves = n_moves;
benkatz 6:1143996ac690 143 if(n_moves < 15)
benkatz 6:1143996ac690 144 {
benkatz 6:1143996ac690 145 printf("NUMBER OF MOVES LESS THAN 15.\r\n");
benkatz 6:1143996ac690 146 //return 1;
benkatz 6:1143996ac690 147 }
benkatz 6:1143996ac690 148
benkatz 6:1143996ac690 149 for(int i = 0; i < n_moves; i++)
benkatz 6:1143996ac690 150 {
benkatz 6:1143996ac690 151 uint8_t ser_move = buffer[start_index+i];
benkatz 6:1143996ac690 152 int8_t face = (face_t)((ser_move>>3) & 0x07);
benkatz 6:1143996ac690 153 int8_t n_turns = (int8_t)((ser_move) & 0x07);
benkatz 6:1143996ac690 154 // 2's complement negation
benkatz 6:1143996ac690 155 if(n_turns > 2) n_turns |= 0xf8;
benkatz 6:1143996ac690 156 if(face < 0 || face > 5)
benkatz 6:1143996ac690 157 {
benkatz 6:1143996ac690 158 printf("GOT INVALID FACE.\r\n");
benkatz 6:1143996ac690 159 return 1;
benkatz 6:1143996ac690 160 }
benkatz 6:1143996ac690 161 if(!(n_turns == -2 || n_turns == -1 || n_turns == 1 || n_turns == 2))
benkatz 6:1143996ac690 162 {
benkatz 6:1143996ac690 163 printf("GOT INVALID NUMBER OF TURNS: %d\r\n",n_turns);
benkatz 6:1143996ac690 164 printf("Serial data: " BYTE_TO_BINARY_PATTERN "\r\n",BYTE_TO_BINARY(ser_move));
benkatz 6:1143996ac690 165
benkatz 6:1143996ac690 166 return 1;
benkatz 6:1143996ac690 167 }
benkatz 6:1143996ac690 168 printf("move %d nturns: %d\r\n",i,n_turns);
benkatz 6:1143996ac690 169 seq->moves[i].n_turns = n_turns;
benkatz 6:1143996ac690 170 seq->moves[i].face = (face_t)face;
benkatz 6:1143996ac690 171 }
benkatz 6:1143996ac690 172
benkatz 6:1143996ac690 173 return 0;
benkatz 6:1143996ac690 174 }
benkatz 6:1143996ac690 175
benkatz 11:16d807d6b9c5 176 int check_double_move(int f1, int f2)
benkatz 11:16d807d6b9c5 177 {
benkatz 11:16d807d6b9c5 178 return (f1 == 0 && f2 == 3) ||
benkatz 11:16d807d6b9c5 179 (f2 == 0 && f1 == 3) ||
benkatz 11:16d807d6b9c5 180 (f2 == 1 && f1 == 4) ||
benkatz 11:16d807d6b9c5 181 (f2 == 1 && f1 == 4) ||
benkatz 11:16d807d6b9c5 182 (f2 == 2 && f1 == 5) ||
benkatz 11:16d807d6b9c5 183 (f2 == 2 && f1 == 5);
benkatz 11:16d807d6b9c5 184 }
benkatz 6:1143996ac690 185
benkatz 6:1143996ac690 186 int states[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 187 int move_counts[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 188 int wait_counter[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 189
benkatz 6:1143996ac690 190 //states:
benkatz 6:1143996ac690 191 // 0 - start command
benkatz 6:1143996ac690 192 // 1 - delay for command
benkatz 6:1143996ac690 193 // 2 - wait for and
benkatz 6:1143996ac690 194 // 3 - delay for and
benkatz 6:1143996ac690 195 // 4 - done.
benkatz 6:1143996ac690 196 void reset_mbed()
benkatz 6:1143996ac690 197 {
benkatz 6:1143996ac690 198 solve_sequence.moves = moves;
benkatz 6:1143996ac690 199 for(int i = 0; i < 6; i++)
benkatz 6:1143996ac690 200 {
benkatz 6:1143996ac690 201 states[i] = 0;
benkatz 6:1143996ac690 202 move_counts[i] = 0;
benkatz 6:1143996ac690 203 wait_counter[i] = 0;
benkatz 6:1143996ac690 204 }
benkatz 6:1143996ac690 205 }
benkatz 6:1143996ac690 206 void* run_sequence_2(void* command)
benkatz 6:1143996ac690 207 {
benkatz 6:1143996ac690 208
benkatz 6:1143996ac690 209 mbed_info_t* cmd = (mbed_info_t*)command;
benkatz 6:1143996ac690 210 if(cmd->seq == NULL)
benkatz 6:1143996ac690 211 return;
benkatz 6:1143996ac690 212 int n_moves = cmd->seq->n_moves;
benkatz 6:1143996ac690 213 int n_turns = 0;
benkatz 6:1143996ac690 214 if(states[cmd->face] == 0)
benkatz 6:1143996ac690 215 {
benkatz 6:1143996ac690 216 cmd->set_and(0,cmd->face); // and off
benkatz 6:1143996ac690 217
benkatz 6:1143996ac690 218 // check if done
benkatz 11:16d807d6b9c5 219 if(move_counts[cmd->face] >= n_moves)
benkatz 6:1143996ac690 220 {
benkatz 6:1143996ac690 221 printf("SEQUENCE DONE!\r\n");
benkatz 6:1143996ac690 222 cmd->set_and(1,cmd->face);
benkatz 6:1143996ac690 223 states[cmd->face] = 4;
benkatz 6:1143996ac690 224 reset_mbed();
benkatz 6:1143996ac690 225 cmd->seq = NULL;
benkatz 6:1143996ac690 226 return NULL;
benkatz 6:1143996ac690 227 }
benkatz 11:16d807d6b9c5 228
benkatz 11:16d807d6b9c5 229 // don't check if we're on the second to last move
benkatz 11:16d807d6b9c5 230 int current_move = move_counts[cmd->face];
benkatz 11:16d807d6b9c5 231 int second_to_last_move = n_moves - 1;
benkatz 6:1143996ac690 232
benkatz 11:16d807d6b9c5 233
benkatz 11:16d807d6b9c5 234 if( !(current_move >= second_to_last_move))
benkatz 11:16d807d6b9c5 235 {
benkatz 11:16d807d6b9c5 236
benkatz 11:16d807d6b9c5 237 // check the faces...
benkatz 11:16d807d6b9c5 238 int face_1 = cmd->seq->moves[move_counts[cmd->face]].face;
benkatz 11:16d807d6b9c5 239 int face_2 = cmd->seq->moves[move_counts[cmd->face] + 1].face;
benkatz 11:16d807d6b9c5 240
benkatz 11:16d807d6b9c5 241 // if they don't match, go to old thing.
benkatz 11:16d807d6b9c5 242 if(!check_double_move(face_1,face_2)) goto old_thing;
benkatz 11:16d807d6b9c5 243
benkatz 11:16d807d6b9c5 244 // first check the current move...
benkatz 11:16d807d6b9c5 245 if(cmd->face == face_1)
benkatz 11:16d807d6b9c5 246 n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
benkatz 11:16d807d6b9c5 247 else if(cmd->face == face_2)
benkatz 11:16d807d6b9c5 248 n_turns = cmd->seq->moves[move_counts[cmd->face]+1].n_turns;
benkatz 11:16d807d6b9c5 249
benkatz 11:16d807d6b9c5 250 move_counts[cmd->face]+=2;
benkatz 11:16d807d6b9c5 251
benkatz 11:16d807d6b9c5 252 // rotate if needed
benkatz 11:16d807d6b9c5 253 if(n_turns)
benkatz 11:16d807d6b9c5 254 cmd->rotate(n_turns,cmd->face);
benkatz 11:16d807d6b9c5 255
benkatz 11:16d807d6b9c5 256 // wait...
benkatz 11:16d807d6b9c5 257 states[cmd->face] = 1;
benkatz 11:16d807d6b9c5 258 wait_counter[cmd->face] = 0;
benkatz 11:16d807d6b9c5 259 return NULL;
benkatz 11:16d807d6b9c5 260
benkatz 11:16d807d6b9c5 261 }
benkatz 11:16d807d6b9c5 262
benkatz 11:16d807d6b9c5 263 old_thing:
benkatz 11:16d807d6b9c5 264 //***************** do the old thing ***************************
benkatz 11:16d807d6b9c5 265
benkatz 6:1143996ac690 266 // check how many turns needed
benkatz 6:1143996ac690 267 if(cmd->face == cmd->seq->moves[move_counts[cmd->face]].face)
benkatz 6:1143996ac690 268 {
benkatz 6:1143996ac690 269 n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
benkatz 6:1143996ac690 270 }
benkatz 6:1143996ac690 271 else n_turns = 0;
benkatz 6:1143996ac690 272
benkatz 6:1143996ac690 273 // increment move counter
benkatz 6:1143996ac690 274 move_counts[cmd->face]++;
benkatz 6:1143996ac690 275
benkatz 6:1143996ac690 276 // rotate if needed
benkatz 6:1143996ac690 277 if(n_turns)
benkatz 6:1143996ac690 278 cmd->rotate(n_turns,cmd->face);
benkatz 6:1143996ac690 279
benkatz 6:1143996ac690 280 // wait...
benkatz 6:1143996ac690 281 states[cmd->face] = 1;
benkatz 6:1143996ac690 282 wait_counter[cmd->face] = 0;
benkatz 6:1143996ac690 283 return NULL;
benkatz 11:16d807d6b9c5 284
benkatz 6:1143996ac690 285 }
benkatz 6:1143996ac690 286 else if(states[cmd->face] == 1)
benkatz 6:1143996ac690 287 {
benkatz 6:1143996ac690 288 //printf("b%d 1\n",cmd->face);
benkatz 6:1143996ac690 289 wait_counter[cmd->face]++;
benkatz 6:1143996ac690 290 if(wait_counter[cmd->face] < 500) return NULL;
benkatz 6:1143996ac690 291 states[cmd->face] = 2;
benkatz 6:1143996ac690 292 cmd->set_and(1,cmd->face);
benkatz 6:1143996ac690 293 return NULL;
benkatz 6:1143996ac690 294 }
benkatz 6:1143996ac690 295 else if(states[cmd->face] == 2)
benkatz 6:1143996ac690 296 {
benkatz 6:1143996ac690 297 //printf("b%d 2\n",cmd->face);
benkatz 6:1143996ac690 298 //printf("b 1 %d\n",cmd->face);
benkatz 6:1143996ac690 299 if(cmd->get_and())
benkatz 6:1143996ac690 300 {
benkatz 6:1143996ac690 301 wait_counter[cmd->face] = 0;
benkatz 6:1143996ac690 302 states[cmd->face] = 3;
benkatz 6:1143996ac690 303 return NULL;
benkatz 6:1143996ac690 304 }
benkatz 6:1143996ac690 305 }
benkatz 6:1143996ac690 306 else if(states[cmd->face] == 3)
benkatz 6:1143996ac690 307 {
benkatz 6:1143996ac690 308 //printf("b%d 3\n",cmd->face);
benkatz 6:1143996ac690 309 if(wait_counter[cmd->face]++ < 100) return NULL;
benkatz 6:1143996ac690 310 states[cmd->face] = 0;
benkatz 6:1143996ac690 311 cmd->set_and(0,cmd->face);
benkatz 6:1143996ac690 312 return NULL;
benkatz 6:1143996ac690 313 }
benkatz 6:1143996ac690 314 else if(states[cmd->face] == 4)
benkatz 6:1143996ac690 315 ;
benkatz 6:1143996ac690 316
benkatz 6:1143996ac690 317 return NULL;
benkatz 6:1143996ac690 318 //printf("b%d 4\n",cmd->face);
benkatz 6:1143996ac690 319 }