Ben Katz / rubix_controller

servodisc goodness

Dependencies:   mbed-dev-f303

cube.cpp@6:1143996ac690, 2018-01-15 (annotated)

Committer:
benkatz
Date:
Mon Jan 15 16:12:52 2018 +0000
Revision:
6:1143996ac690
Child:
7:a278f58cdbd3
Child:
11:16d807d6b9c5
Synchronization worked!

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 6:1143996ac690 176
benkatz 6:1143996ac690 177 int states[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 178 int move_counts[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 179 int wait_counter[6] = {0,0,0,0,0,0};
benkatz 6:1143996ac690 180
benkatz 6:1143996ac690 181 //states:
benkatz 6:1143996ac690 182 // 0 - start command
benkatz 6:1143996ac690 183 // 1 - delay for command
benkatz 6:1143996ac690 184 // 2 - wait for and
benkatz 6:1143996ac690 185 // 3 - delay for and
benkatz 6:1143996ac690 186 // 4 - done.
benkatz 6:1143996ac690 187 void reset_mbed()
benkatz 6:1143996ac690 188 {
benkatz 6:1143996ac690 189 solve_sequence.moves = moves;
benkatz 6:1143996ac690 190 for(int i = 0; i < 6; i++)
benkatz 6:1143996ac690 191 {
benkatz 6:1143996ac690 192 states[i] = 0;
benkatz 6:1143996ac690 193 move_counts[i] = 0;
benkatz 6:1143996ac690 194 wait_counter[i] = 0;
benkatz 6:1143996ac690 195 }
benkatz 6:1143996ac690 196 }
benkatz 6:1143996ac690 197 void* run_sequence_2(void* command)
benkatz 6:1143996ac690 198 {
benkatz 6:1143996ac690 199
benkatz 6:1143996ac690 200 mbed_info_t* cmd = (mbed_info_t*)command;
benkatz 6:1143996ac690 201 if(cmd->seq == NULL)
benkatz 6:1143996ac690 202 return;
benkatz 6:1143996ac690 203 int n_moves = cmd->seq->n_moves;
benkatz 6:1143996ac690 204 int n_turns = 0;
benkatz 6:1143996ac690 205 if(states[cmd->face] == 0)
benkatz 6:1143996ac690 206 {
benkatz 6:1143996ac690 207 cmd->set_and(0,cmd->face); // and off
benkatz 6:1143996ac690 208
benkatz 6:1143996ac690 209 // check if done
benkatz 6:1143996ac690 210 if(move_counts[cmd->face] > n_moves)
benkatz 6:1143996ac690 211 {
benkatz 6:1143996ac690 212 printf("SEQUENCE DONE!\r\n");
benkatz 6:1143996ac690 213 cmd->set_and(1,cmd->face);
benkatz 6:1143996ac690 214 states[cmd->face] = 4;
benkatz 6:1143996ac690 215 reset_mbed();
benkatz 6:1143996ac690 216 cmd->seq = NULL;
benkatz 6:1143996ac690 217 return NULL;
benkatz 6:1143996ac690 218 }
benkatz 6:1143996ac690 219
benkatz 6:1143996ac690 220 // check how many turns needed
benkatz 6:1143996ac690 221 if(cmd->face == cmd->seq->moves[move_counts[cmd->face]].face)
benkatz 6:1143996ac690 222 {
benkatz 6:1143996ac690 223 n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
benkatz 6:1143996ac690 224 }
benkatz 6:1143996ac690 225 else n_turns = 0;
benkatz 6:1143996ac690 226
benkatz 6:1143996ac690 227 // increment move counter
benkatz 6:1143996ac690 228 move_counts[cmd->face]++;
benkatz 6:1143996ac690 229
benkatz 6:1143996ac690 230 // rotate if needed
benkatz 6:1143996ac690 231 if(n_turns)
benkatz 6:1143996ac690 232 cmd->rotate(n_turns,cmd->face);
benkatz 6:1143996ac690 233
benkatz 6:1143996ac690 234 // wait...
benkatz 6:1143996ac690 235 states[cmd->face] = 1;
benkatz 6:1143996ac690 236 wait_counter[cmd->face] = 0;
benkatz 6:1143996ac690 237 return NULL;
benkatz 6:1143996ac690 238 }
benkatz 6:1143996ac690 239 else if(states[cmd->face] == 1)
benkatz 6:1143996ac690 240 {
benkatz 6:1143996ac690 241 //printf("b%d 1\n",cmd->face);
benkatz 6:1143996ac690 242 wait_counter[cmd->face]++;
benkatz 6:1143996ac690 243 if(wait_counter[cmd->face] < 500) return NULL;
benkatz 6:1143996ac690 244 states[cmd->face] = 2;
benkatz 6:1143996ac690 245 cmd->set_and(1,cmd->face);
benkatz 6:1143996ac690 246 return NULL;
benkatz 6:1143996ac690 247 }
benkatz 6:1143996ac690 248 else if(states[cmd->face] == 2)
benkatz 6:1143996ac690 249 {
benkatz 6:1143996ac690 250 //printf("b%d 2\n",cmd->face);
benkatz 6:1143996ac690 251 //printf("b 1 %d\n",cmd->face);
benkatz 6:1143996ac690 252 if(cmd->get_and())
benkatz 6:1143996ac690 253 {
benkatz 6:1143996ac690 254 wait_counter[cmd->face] = 0;
benkatz 6:1143996ac690 255 states[cmd->face] = 3;
benkatz 6:1143996ac690 256 return NULL;
benkatz 6:1143996ac690 257 }
benkatz 6:1143996ac690 258 }
benkatz 6:1143996ac690 259 else if(states[cmd->face] == 3)
benkatz 6:1143996ac690 260 {
benkatz 6:1143996ac690 261 //printf("b%d 3\n",cmd->face);
benkatz 6:1143996ac690 262 if(wait_counter[cmd->face]++ < 100) return NULL;
benkatz 6:1143996ac690 263 states[cmd->face] = 0;
benkatz 6:1143996ac690 264 cmd->set_and(0,cmd->face);
benkatz 6:1143996ac690 265 return NULL;
benkatz 6:1143996ac690 266 }
benkatz 6:1143996ac690 267 else if(states[cmd->face] == 4)
benkatz 6:1143996ac690 268 ;
benkatz 6:1143996ac690 269
benkatz 6:1143996ac690 270 return NULL;
benkatz 6:1143996ac690 271 //printf("b%d 4\n",cmd->face);
benkatz 6:1143996ac690 272 }