Jonas Forsslund / Mbed 2 deprecated WoodenHapticsHID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM USBDevice

Fork of USBHID_TestCase by Samuel Mokrani

main.cpp@6:3d15e8b4d035, 2015-09-04 (annotated)

Committer:
jofo
Date:
Fri Sep 04 08:03:30 2015 +0000
Revision:
6:3d15e8b4d035
Parent:
5:2908292a8cf3
Child:
7:bb6454b72c57
2015-09-04 three weeks ago working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
samux 2:1db77338562f 1 #include "mbed.h"
samux 2:1db77338562f 2 #include "USBHID.h"
jofo 4:3ab1e94b3bc4 3 #include "WoodenDevice.h"
jofo 4:3ab1e94b3bc4 4
samux 2:1db77338562f 5 //We declare a USBHID device. Input out output reports have a length of 8 bytes
jofo 5:2908292a8cf3 6 USBHID hid(8, 8);
samux 2:1db77338562f 7
samux 2:1db77338562f 8 //This report will contain data to be sent
samux 2:1db77338562f 9 HID_REPORT send_report;
samux 2:1db77338562f 10 HID_REPORT recv_report;
jofo 4:3ab1e94b3bc4 11
jofo 4:3ab1e94b3bc4 12 DigitalOut myled1(LED1);
jofo 4:3ab1e94b3bc4 13 DigitalOut myled2(LED2);
jofo 4:3ab1e94b3bc4 14 DigitalOut myled3(LED3);
jofo 4:3ab1e94b3bc4 15 DigitalOut myled4(LED4);
jofo 4:3ab1e94b3bc4 16
jofo 6:3d15e8b4d035 17 DigitalOut enableEscons(p14,0);
samux 2:1db77338562f 18
jofo 4:3ab1e94b3bc4 19 InterruptIn encoder0_A(p5);
jofo 4:3ab1e94b3bc4 20 InterruptIn encoder0_B(p7);
jofo 4:3ab1e94b3bc4 21 InterruptIn encoder1_A(p8);
jofo 4:3ab1e94b3bc4 22 InterruptIn encoder1_B(p10);
jofo 4:3ab1e94b3bc4 23 InterruptIn encoder2_A(p11);
jofo 4:3ab1e94b3bc4 24 InterruptIn encoder2_B(p13);
jofo 4:3ab1e94b3bc4 25
jofo 6:3d15e8b4d035 26 Timeout msg_watchdog;
jofo 6:3d15e8b4d035 27
jofo 6:3d15e8b4d035 28
jofo 6:3d15e8b4d035 29
jofo 4:3ab1e94b3bc4 30
jofo 4:3ab1e94b3bc4 31 Serial pc(USBTX, USBRX); // tx, rx
jofo 4:3ab1e94b3bc4 32
jofo 4:3ab1e94b3bc4 33
jofo 4:3ab1e94b3bc4 34 int no_enc = 3;
jofo 4:3ab1e94b3bc4 35
jofo 4:3ab1e94b3bc4 36 int prev_state[3] = {-1,-1,-1};
jofo 4:3ab1e94b3bc4 37 bool encoder_raw[3][2] = {{false,false},{false,false},{false,false}};
jofo 4:3ab1e94b3bc4 38
jofo 4:3ab1e94b3bc4 39 PwmOut pwm[3]={p21,p22,p23};
jofo 4:3ab1e94b3bc4 40 DigitalOut direction[3]={p24,p25,p26};
jofo 4:3ab1e94b3bc4 41
jofo 4:3ab1e94b3bc4 42 int counter[3] = {0,0,0};
jofo 4:3ab1e94b3bc4 43
jofo 4:3ab1e94b3bc4 44 // Pre Cur Dir Dec
jofo 4:3ab1e94b3bc4 45 // 0 0 0 1 + 1
jofo 4:3ab1e94b3bc4 46 // 0 0 1 0 - 2
jofo 4:3ab1e94b3bc4 47 // 0 1 1 1 + 7
jofo 4:3ab1e94b3bc4 48 // 0 1 0 0 - 4
jofo 4:3ab1e94b3bc4 49 // 1 1 1 0 + 14
jofo 4:3ab1e94b3bc4 50 // 1 1 0 1 - 13
jofo 4:3ab1e94b3bc4 51 // 1 0 0 0 + 8
jofo 4:3ab1e94b3bc4 52 // 1 0 1 1 - 11
jofo 4:3ab1e94b3bc4 53 //
jofo 4:3ab1e94b3bc4 54 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
jofo 4:3ab1e94b3bc4 55 //int stable[16] = {0,1,-1,0,-1,0,0,1,1,0, 0,-1, 0,-1, 1, 0}; //OLD
jofo 4:3ab1e94b3bc4 56 int stable[16] = {0,-1,1,0,1,0,0,-1,-1,0, 0,1, 0,1, -1, 0};
jofo 4:3ab1e94b3bc4 57
jofo 4:3ab1e94b3bc4 58 void encoder_callback(int _encoder,int AB,bool value){
jofo 4:3ab1e94b3bc4 59 int cur_state;
jofo 4:3ab1e94b3bc4 60
jofo 4:3ab1e94b3bc4 61 encoder_raw[_encoder][AB]=value;
jofo 4:3ab1e94b3bc4 62
jofo 4:3ab1e94b3bc4 63 cur_state = encoder_raw[_encoder][0] << 1 | encoder_raw[_encoder][1];
jofo 4:3ab1e94b3bc4 64
jofo 4:3ab1e94b3bc4 65 if(prev_state[_encoder] < 0) prev_state[_encoder] = cur_state;
jofo 4:3ab1e94b3bc4 66 counter[_encoder] += stable[prev_state[_encoder] << 2 | cur_state];
jofo 4:3ab1e94b3bc4 67 prev_state[_encoder]=cur_state;
jofo 4:3ab1e94b3bc4 68 }
jofo 4:3ab1e94b3bc4 69
jofo 4:3ab1e94b3bc4 70 // "callback stubs"
jofo 4:3ab1e94b3bc4 71 void callback_0_A_rise(void) { encoder_callback(0,0,true);}
jofo 4:3ab1e94b3bc4 72 void callback_0_A_fall(void) { encoder_callback(0,0,false);}
jofo 4:3ab1e94b3bc4 73 void callback_0_B_rise(void) { encoder_callback(0,1,true);}
jofo 4:3ab1e94b3bc4 74 void callback_0_B_fall(void) { encoder_callback(0,1,false);}
jofo 4:3ab1e94b3bc4 75
jofo 4:3ab1e94b3bc4 76 void callback_1_A_rise(void) { encoder_callback(1,0,true);}
jofo 4:3ab1e94b3bc4 77 void callback_1_A_fall(void) { encoder_callback(1,0,false);}
jofo 4:3ab1e94b3bc4 78 void callback_1_B_rise(void) { encoder_callback(1,1,true);}
jofo 4:3ab1e94b3bc4 79 void callback_1_B_fall(void) { encoder_callback(1,1,false);}
jofo 4:3ab1e94b3bc4 80
jofo 4:3ab1e94b3bc4 81 void callback_2_A_rise(void) { encoder_callback(2,0,true);}
jofo 4:3ab1e94b3bc4 82 void callback_2_A_fall(void) { encoder_callback(2,0,false);}
jofo 4:3ab1e94b3bc4 83 void callback_2_B_rise(void) { encoder_callback(2,1,true);}
jofo 4:3ab1e94b3bc4 84 void callback_2_B_fall(void) { encoder_callback(2,1,false);}
jofo 4:3ab1e94b3bc4 85
jofo 4:3ab1e94b3bc4 86 // Our 12*4=48 byte message (used both up and down)
jofo 4:3ab1e94b3bc4 87 struct woodenhaptics_message {
jofo 4:3ab1e94b3bc4 88 float position_x;
jofo 4:3ab1e94b3bc4 89 float position_y;
jofo 4:3ab1e94b3bc4 90 float position_z;
jofo 4:3ab1e94b3bc4 91 float command_force_x;
jofo 4:3ab1e94b3bc4 92 float command_force_y;
jofo 4:3ab1e94b3bc4 93 float command_force_z;
jofo 4:3ab1e94b3bc4 94 float actual_current_0;
jofo 4:3ab1e94b3bc4 95 float actual_current_1;
jofo 4:3ab1e94b3bc4 96 float actual_current_2;
jofo 4:3ab1e94b3bc4 97 float temperature_0;
jofo 4:3ab1e94b3bc4 98 float temperature_1;
jofo 4:3ab1e94b3bc4 99 float temperature_2;
jofo 4:3ab1e94b3bc4 100
jofo 4:3ab1e94b3bc4 101 woodenhaptics_message():position_x(0),position_y(0),position_z(0),
jofo 4:3ab1e94b3bc4 102 command_force_x(0),command_force_y(0),command_force_z(0),
jofo 4:3ab1e94b3bc4 103 actual_current_0(0),actual_current_1(0),actual_current_2(0),
jofo 4:3ab1e94b3bc4 104 temperature_0(0),temperature_1(0),temperature_2(0){}
jofo 4:3ab1e94b3bc4 105 };
jofo 5:2908292a8cf3 106
jofo 5:2908292a8cf3 107 struct hid_to_pc_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 108 short encoder_a;
jofo 5:2908292a8cf3 109 short encoder_b;
jofo 5:2908292a8cf3 110 short encoder_c;
jofo 5:2908292a8cf3 111 unsigned short debug;
jofo 5:2908292a8cf3 112 };
jofo 5:2908292a8cf3 113
jofo 5:2908292a8cf3 114 struct pc_to_hid_message { // 4*2 = 8 bytes
jofo 5:2908292a8cf3 115 short current_motor_a_mA;
jofo 5:2908292a8cf3 116 short current_motor_b_mA;
jofo 5:2908292a8cf3 117 short current_motor_c_mA;
jofo 5:2908292a8cf3 118 unsigned int debug;
jofo 5:2908292a8cf3 119 };
jofo 5:2908292a8cf3 120
jofo 6:3d15e8b4d035 121 void escon_timeout() {
jofo 6:3d15e8b4d035 122 enableEscons = 0;
jofo 6:3d15e8b4d035 123 myled2 = 0;
jofo 6:3d15e8b4d035 124
jofo 6:3d15e8b4d035 125 myled3 = 0;
jofo 6:3d15e8b4d035 126 myled4 = 0;
jofo 6:3d15e8b4d035 127
jofo 6:3d15e8b4d035 128 for(int i=0;i<3;i++){
jofo 6:3d15e8b4d035 129 pwm[i].write(0);
jofo 6:3d15e8b4d035 130 direction[i]=0;
jofo 6:3d15e8b4d035 131 }
jofo 6:3d15e8b4d035 132 }
samux 2:1db77338562f 133
samux 2:1db77338562f 134 int main(void) {
jofo 4:3ab1e94b3bc4 135 myled1 = 1; // SETUP
jofo 6:3d15e8b4d035 136 myled2 = 1;
jofo 6:3d15e8b4d035 137 myled3 = 1;
jofo 6:3d15e8b4d035 138 myled4 = 1;
jofo 6:3d15e8b4d035 139
jofo 6:3d15e8b4d035 140 wait_ms(500);
jofo 6:3d15e8b4d035 141 myled1 = 0; // SETUP
jofo 4:3ab1e94b3bc4 142 myled2 = 0;
jofo 4:3ab1e94b3bc4 143 myled3 = 0;
jofo 6:3d15e8b4d035 144 myled4 = 0;
jofo 6:3d15e8b4d035 145
jofo 4:3ab1e94b3bc4 146
jofo 4:3ab1e94b3bc4 147 encoder0_A.rise(&callback_0_A_rise);
jofo 4:3ab1e94b3bc4 148 encoder0_A.fall(&callback_0_A_fall);
jofo 4:3ab1e94b3bc4 149 encoder0_B.rise(&callback_0_B_rise);
jofo 4:3ab1e94b3bc4 150 encoder0_B.fall(&callback_0_B_fall);
jofo 4:3ab1e94b3bc4 151
jofo 4:3ab1e94b3bc4 152 encoder1_A.rise(&callback_1_A_rise);
jofo 4:3ab1e94b3bc4 153 encoder1_A.fall(&callback_1_A_fall);
jofo 4:3ab1e94b3bc4 154 encoder1_B.rise(&callback_1_B_rise);
jofo 4:3ab1e94b3bc4 155 encoder1_B.fall(&callback_1_B_fall);
jofo 4:3ab1e94b3bc4 156
jofo 4:3ab1e94b3bc4 157 encoder2_A.rise(&callback_2_A_rise);
jofo 4:3ab1e94b3bc4 158 encoder2_A.fall(&callback_2_A_fall);
jofo 4:3ab1e94b3bc4 159 encoder2_B.rise(&callback_2_B_rise);
jofo 4:3ab1e94b3bc4 160 encoder2_B.fall(&callback_2_B_fall);
jofo 4:3ab1e94b3bc4 161
jofo 6:3d15e8b4d035 162 enableEscons = 0;
jofo 4:3ab1e94b3bc4 163
jofo 4:3ab1e94b3bc4 164 configuration config = default_woody();
jofo 4:3ab1e94b3bc4 165
jofo 4:3ab1e94b3bc4 166 for(int i=0;i<3;i++){
jofo 4:3ab1e94b3bc4 167 pwm[i].period_us(1000);
jofo 4:3ab1e94b3bc4 168 pwm[i].write(0);
jofo 4:3ab1e94b3bc4 169 direction[i]=0;
jofo 4:3ab1e94b3bc4 170 }
jofo 4:3ab1e94b3bc4 171
jofo 4:3ab1e94b3bc4 172
jofo 4:3ab1e94b3bc4 173 pc.baud(115200);
jofo 4:3ab1e94b3bc4 174
jofo 5:2908292a8cf3 175 send_report.length = 8;
jofo 5:2908292a8cf3 176 //woodenhaptics_message msg;
jofo 5:2908292a8cf3 177
jofo 5:2908292a8cf3 178 hid_to_pc_message hid_to_pc;
jofo 5:2908292a8cf3 179 hid_to_pc.debug = 0;
jofo 5:2908292a8cf3 180 pc_to_hid_message pc_to_hid;
jofo 4:3ab1e94b3bc4 181
jofo 6:3d15e8b4d035 182 // Timer t;
jofo 6:3d15e8b4d035 183 // t.start();
jofo 4:3ab1e94b3bc4 184
jofo 4:3ab1e94b3bc4 185 Timer usb_timer;
jofo 4:3ab1e94b3bc4 186 usb_timer.start();
jofo 4:3ab1e94b3bc4 187
jofo 4:3ab1e94b3bc4 188 Timer debug_t;
jofo 4:3ab1e94b3bc4 189 debug_t.start();
jofo 6:3d15e8b4d035 190
jofo 6:3d15e8b4d035 191 Timer message_timeout;
jofo 6:3d15e8b4d035 192 message_timeout.start();
jofo 4:3ab1e94b3bc4 193
jofo 4:3ab1e94b3bc4 194
jofo 4:3ab1e94b3bc4 195 pc.printf("Hello World Debug!\n\r");
jofo 4:3ab1e94b3bc4 196
jofo 6:3d15e8b4d035 197
samux 2:1db77338562f 198 while (1) {
jofo 4:3ab1e94b3bc4 199 myled1 = 1;
samux 2:1db77338562f 200
jofo 4:3ab1e94b3bc4 201
jofo 4:3ab1e94b3bc4 202
samux 2:1db77338562f 203 //try to read a msg
samux 2:1db77338562f 204 if(hid.readNB(&recv_report)) {
jofo 4:3ab1e94b3bc4 205 // TODO: Make sure we read the latest message, not the oldest.
jofo 4:3ab1e94b3bc4 206 myled3 = !myled3; // We got data
jofo 4:3ab1e94b3bc4 207
jofo 4:3ab1e94b3bc4 208
jofo 5:2908292a8cf3 209 if(recv_report.length == 8){ // It should always be!
jofo 4:3ab1e94b3bc4 210
jofo 6:3d15e8b4d035 211
jofo 6:3d15e8b4d035 212 //message_timeout.reset();
jofo 6:3d15e8b4d035 213 //enableEscons = 1;
jofo 6:3d15e8b4d035 214 //myled2 = 1;
jofo 6:3d15e8b4d035 215 msg_watchdog.detach();
jofo 6:3d15e8b4d035 216 msg_watchdog.attach(&escon_timeout,0.5);
jofo 6:3d15e8b4d035 217 enableEscons = 1;
jofo 6:3d15e8b4d035 218 myled2 = 1;
jofo 6:3d15e8b4d035 219
jofo 6:3d15e8b4d035 220
jofo 5:2908292a8cf3 221 pc_to_hid = *reinterpret_cast<pc_to_hid_message*>(recv_report.data);
jofo 4:3ab1e94b3bc4 222
jofo 5:2908292a8cf3 223 //if(debug_t.read() > 0.5){
jofo 5:2908292a8cf3 224 // pc.printf("Force: %f\n\r",pc_to_hid.current_motor_a_mA);
jofo 5:2908292a8cf3 225 // debug_t.reset();
jofo 5:2908292a8cf3 226 //}
jofo 4:3ab1e94b3bc4 227
jofo 4:3ab1e94b3bc4 228 //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
jofo 4:3ab1e94b3bc4 229
jofo 5:2908292a8cf3 230 //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
jofo 5:2908292a8cf3 231 float f[3] = {pc_to_hid.current_motor_a_mA*0.001, pc_to_hid.current_motor_b_mA*0.001,pc_to_hid.current_motor_c_mA*0.001};
jofo 4:3ab1e94b3bc4 232 for(int i=0;i<3;i++){
jofo 4:3ab1e94b3bc4 233 int dir = f[i] > 0 ? 1 : 0;
jofo 4:3ab1e94b3bc4 234 direction[i].write(dir);
jofo 4:3ab1e94b3bc4 235 // if(i==1)
jofo 4:3ab1e94b3bc4 236 // pc.printf("Direction: %d (direction %d) \n\r", dir, direction[1].read());
jofo 4:3ab1e94b3bc4 237 float abs_val = std::abs(f[i]);
jofo 5:2908292a8cf3 238 if(f[i] > 3.0)
jofo 4:3ab1e94b3bc4 239 pwm[i].write(0.9);
jofo 4:3ab1e94b3bc4 240 else
jofo 5:2908292a8cf3 241 pwm[i].write(0.8*abs_val/3.0+0.1);
jofo 4:3ab1e94b3bc4 242 }
jofo 6:3d15e8b4d035 243
jofo 4:3ab1e94b3bc4 244
jofo 4:3ab1e94b3bc4 245
jofo 4:3ab1e94b3bc4 246 /*
jofo 4:3ab1e94b3bc4 247 pwm[0].write(0.5);
jofo 4:3ab1e94b3bc4 248 pwm[1].write(0.5);
jofo 4:3ab1e94b3bc4 249 pwm[2].write(0.5);
jofo 4:3ab1e94b3bc4 250 */
samux 2:1db77338562f 251 }
jofo 4:3ab1e94b3bc4 252
jofo 4:3ab1e94b3bc4 253 myled1 = 1;
samux 2:1db77338562f 254 }
samux 2:1db77338562f 255
jofo 4:3ab1e94b3bc4 256
jofo 4:3ab1e94b3bc4 257
jofo 4:3ab1e94b3bc4 258 // "move the haptic device to the right"
jofo 5:2908292a8cf3 259 //vec p = getPosition(config, counter);
jofo 5:2908292a8cf3 260 //msg.position_x = p.x;
jofo 5:2908292a8cf3 261 //msg.position_y = p.y;//0.05*sin(t.read());
jofo 5:2908292a8cf3 262 //msg.position_z = p.z;
jofo 4:3ab1e94b3bc4 263
jofo 5:2908292a8cf3 264 //msg.temperature_0 = counter[0]; // TODO: temperature is used temprarily as a carrier for counter values
jofo 5:2908292a8cf3 265 //msg.temperature_1 = counter[1];
jofo 5:2908292a8cf3 266 //msg.temperature_2 = counter[2];
jofo 5:2908292a8cf3 267 hid_to_pc.encoder_a = counter[0];
jofo 5:2908292a8cf3 268 hid_to_pc.encoder_b = counter[1];
jofo 5:2908292a8cf3 269 hid_to_pc.encoder_c = counter[2];
jofo 5:2908292a8cf3 270
jofo 5:2908292a8cf3 271 if(usb_timer.read() > 0.001) {
jofo 4:3ab1e94b3bc4 272 usb_timer.reset();
jofo 4:3ab1e94b3bc4 273
jofo 5:2908292a8cf3 274 hid_to_pc.debug++;
jofo 5:2908292a8cf3 275 unsigned char* out_buf = reinterpret_cast<unsigned char*>(&hid_to_pc);
jofo 4:3ab1e94b3bc4 276
jofo 4:3ab1e94b3bc4 277 //Fill the report
jofo 4:3ab1e94b3bc4 278 for (int i = 0; i < send_report.length; i++) {
jofo 4:3ab1e94b3bc4 279 send_report.data[i] = out_buf[i];
jofo 4:3ab1e94b3bc4 280 }
jofo 4:3ab1e94b3bc4 281
jofo 4:3ab1e94b3bc4 282 //Send the report
jofo 4:3ab1e94b3bc4 283 hid.send(&send_report);
jofo 4:3ab1e94b3bc4 284 myled4 = !myled4;
jofo 4:3ab1e94b3bc4 285 }
jofo 4:3ab1e94b3bc4 286
jofo 4:3ab1e94b3bc4 287 //wait_us(100); // 0.1ms
samux 2:1db77338562f 288 }
samux 0:53dfbb3eae55 289 }