rumbling demo, and decode_byteHEADmaster

1 files changed, 35 insertions, 3 deletions

diff --git a/main.c b/main.c
index 4a5135c..a8b7fda 100644
--- a/main.c
+++ b/main.c
@@ -200,6 +200,18 @@ int send_recv_gc(char* bytes, int len)
 		// a value of >=128 means "0", <127 means "1" bit.
 }
 
+/* reads eight exploded bits from bufptr, and makes a 8bit-char from them.
+ * Bit ordering is MSB first. A bit is considered as 1, if bufptr[i] & 0x80 is
+ * *not* set, and vice versa. */
+char decode_byte(char* bufptr)
+{
+	char result=0;
+	for (int i=0;i<8;i++)
+		result|= ( (bufptr[7-i]&0x80)?0:(1<<i) );
+	
+	return result;
+}
+
 
 int main (void)
 {
@@ -214,6 +226,9 @@ int main (void)
 	int temp=0;
 	int n_received;
 
+	unsigned char left_trigger=0, right_trigger=0;
+	int rumble_count1=0, rumble_count2=0;
+
 	while(1)
 	{
 		temp++;
@@ -251,10 +266,27 @@ int main (void)
                
 		_delay_ms(0.3);
 
-	char foo[] = { 0x40, 0x03, 0x02 };
 
-	if (!(PIND & 0x10))
-		foo[2]=0x03;
+	// this is the GETSTATUS query. if the last byte is 0x01,
+	// the controller will rumble.
+	char foo[] = { 0x40, 0x03, 0x00 };
+
+
+
+
+	// depending from the left and right triggers, decide
+	// how hard, and at which frequency, to rumble.
+	left_trigger=decode_byte(buffer+48);
+	right_trigger=decode_byte(buffer+56);
+
+	rumble_count1++;                                         // responsible for the
+	if (rumble_count1 > 25) rumble_count1=0;                 // rumble strength
+
+	rumble_count2++;
+	if (rumble_count2 > 256+left_trigger) rumble_count2=0;   // rumble frequency.
+	
+	if (rumble_count1 < right_trigger/10 && rumble_count2 < 256)
+		foo[2]=0x01; // turn on rumble.
 
 	n_received=send_recv_gc(foo, 3);